示例#1
0
文件: umcwriter.c 项目: companje/UP3D
void umcwriter_planner_set_a_position(double A)
{
  double pos[3];
  plan_get_position(pos);
  pos[2] = A;
  plan_set_position(pos);
}
示例#2
0
文件: umcwriter.c 项目: companje/UP3D
void umcwriter_planner_set_position(double X, double Y, double A)
{
  umcwriter_planner_sync();
  double pos[3];
  pos[settings.x_axes] = X * settings.x_dir;
  pos[settings.y_axes] = Y * settings.y_dir;
  pos[2] = A;
  plan_set_position(pos);
  st_reset();
}
static void parkHeadForLeftAdjustment()
{
    add_homeing[Z_AXIS] -= current_position[Z_AXIS];
    current_position[Z_AXIS] = 0;
    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);

    char buffer[32];
    sprintf_P(buffer, PSTR("G1 F%i Z5"), int(homing_feedrate[Z_AXIS]));
    enquecommand(buffer);
    sprintf_P(buffer, PSTR("G1 F%i X%i Y%i"), int(homing_feedrate[X_AXIS]), BED_LEFT_ADJUST_X, BED_LEFT_ADJUST_Y);
    enquecommand(buffer);
    sprintf_P(buffer, PSTR("G1 F%i Z0"), int(homing_feedrate[Z_AXIS]));
    enquecommand(buffer);
}
示例#4
0
void lcd_set_home_offsets()
{
    for(int8_t i=0; i < NUM_AXIS; i++) {
      if (i != E_AXIS) {
        add_homing[i] -= current_position[i];
        current_position[i] = 0.0;
      }
    }
    plan_set_position(0.0, 0.0, 0.0, current_position[E_AXIS]);

    // Audio feedback
    enquecommand_P(PSTR("M300 S659 P200"));
    enquecommand_P(PSTR("M300 S698 P200"));
    lcd_return_to_status();
}
static void storeHomingZ_parkHeadForLeftAdjustment()
{
    add_homeing[Z_AXIS] -= current_position[Z_AXIS];
    Config_StoreSettings();
    current_position[Z_AXIS] = 0;
    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);

    char buffer[32];
    sprintf_P(buffer, PSTR("G1 F%i Z5"), int(homing_feedrate[Z_AXIS]));
    enquecommand(buffer);
    sprintf_P(buffer, PSTR("G1 F%i X%i Y%i"), int(homing_feedrate[X_AXIS]), 35, 10);
    enquecommand(buffer);
    sprintf_P(buffer, PSTR("G1 F%i Z0"), int(homing_feedrate[Z_AXIS]));
    enquecommand(buffer);
}
void FirmwareTest() {
  ECHO_EM("---------- FIRMWARE TEST --------------");
  ECHO_EM("--------- by MarlinKimbra -------------");
  ECHO_EV(MSG_FWTEST_01);
  ECHO_EV(MSG_FWTEST_02);
  ECHO_EV(MSG_FWTEST_YES_NO);
  serial_answer = ' ';
  while (serial_answer!='y' && serial_answer!='Y' && serial_answer!='n' && serial_answer!='N') {
    serial_answer = MYSERIAL.read();
  }
  if (serial_answer=='y' || serial_answer=='Y') {
    ECHO_EV(MSG_FWTEST_03);

    ECHO_EM(" ");
    ECHO_EM("***** ENDSTOP X *****");
    #if PIN_EXISTS(X_MIN) && (X_HOME_DIR == -1)
      if (!READ(X_MIN_PIN)^X_MIN_ENDSTOP_INVERTING) {
        ECHO_M("MIN ENDSTOP X: ");
        ECHO_EV(MSG_ENDSTOP_OPEN);
      }
      else {
        ECHO_M("X ENDSTOP ");
        ECHO_EM(MSG_FWTEST_ERROR);
        ECHO_M(MSG_FWTEST_INVERT);
        ECHO_M("#define X_MIN_ENDSTOP_LOGIC ");
        ECHO_M(MSG_FWTEST_INTO);
        #if MECH(CARTESIAN)
          ECHO_EM("Configuration_Cartesian.h");
        #elif MECH(COREXY)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(COREXZ)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(DELTA)
          ECHO_EM("Configuration_Delta.h");
        #elif MECH(SCARA)
          ECHO_EM("Configuration_Scara.h");
        #endif
        return;
      }
      ECHO_V(MSG_FWTEST_PRESS);
      ECHO_EM("X");
      ECHO_EV(MSG_FWTEST_YES);
      serial_answer = ' ';
      while (serial_answer!='y' && serial_answer!='Y' && !(READ(X_MIN_PIN)^X_MIN_ENDSTOP_INVERTING)) {
        serial_answer = MYSERIAL.read();
      }
      if (READ(X_MIN_PIN)^X_MIN_ENDSTOP_INVERTING) {
        ECHO_M("MIN ENDSTOP X: ");
        ECHO_EV(MSG_ENDSTOP_HIT);
      }
      else {
        ECHO_M("X ");
        ECHO_EV(MSG_FWTEST_ENDSTOP_ERR);
        return;
      }
    #elif PIN_EXISTS(X_MAX) && X_HOME_DIR == 1
      if (!READ(X_MAX_PIN)^X_MAX_ENDSTOP_INVERTING) {
        ECHO_M("MAX ENDSTOP X: ");
        ECHO_EV(MSG_ENDSTOP_OPEN);
      }
      else {
        ECHO_M("X ENDSTOP ");
        ECHO_EM(MSG_FWTEST_ERROR);
        ECHO_M(MSG_FWTEST_INVERT);
        ECHO_M("#define X_MAX_ENDSTOP_LOGIC ");
        ECHO_M(MSG_FWTEST_INTO);
        #if MECH(CARTESIAN)
          ECHO_EM("Configuration_Cartesian.h");
        #elif MECH(COREXY)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(COREXZ)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(DELTA)
          ECHO_EM("Configuration_Delta.h");
        #elif MECH(SCARA)
          ECHO_EM("Configuration_Scara.h");
        #endif
        return;
      }
      ECHO_V(MSG_FWTEST_PRESS);
      ECHO_EM("X");
      ECHO_EV(MSG_FWTEST_YES);
      serial_answer = ' ';
      while (serial_answer!='y' && serial_answer!='Y' && !(READ(X_MAX_PIN)^X_MAX_ENDSTOP_INVERTING)) {
        serial_answer = MYSERIAL.read();
      }
      if (READ(X_MAX_PIN)^X_MAX_ENDSTOP_INVERTING) {
        ECHO_M("MAX ENDSTOP X: ");
        ECHO_EV(MSG_ENDSTOP_HIT);
      }
      else {
        ECHO_M("X ");
        ECHO_EV(MSG_FWTEST_ENDSTOP_ERR);
        return;
      }
    #elif X_HOME_DIR == -1
      ECHO_M(MSG_FWTEST_ERROR);
      ECHO_M("!!! X_MIN_PIN ");
      ECHO_EM(MSG_FWTEST_NDEF);
      return;
    #elif X_HOME_DIR == 1
      ECHO_M(MSG_FWTEST_ERROR);
      ECHO_M("!!! X_MAX_PIN ");
      ECHO_EM(MSG_FWTEST_NDEF);
      return;
    #endif

    ECHO_EM(" ");
    ECHO_EM("***** ENDSTOP Y *****");
    #if PIN_EXISTS(Y_MIN) && Y_HOME_DIR == -1
      if (!READ(Y_MIN_PIN)^Y_MIN_ENDSTOP_INVERTING) {
        ECHO_M("MIN ENDSTOP Y: ");
        ECHO_EV(MSG_ENDSTOP_OPEN);
      }
      else {
        ECHO_M("Y ENDSTOP ");
        ECHO_EM(MSG_FWTEST_ERROR);
        ECHO_M(MSG_FWTEST_INVERT);
        ECHO_M("#define Y_MIN_ENDSTOP_LOGIC ");
        ECHO_M(MSG_FWTEST_INTO);
        #if MECH(CARTESIAN)
          ECHO_EM("Configuration_Cartesian.h");
        #elif MECH(COREXY)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(COREXZ)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(DELTA)
          ECHO_EM("Configuration_Delta.h");
        #elif MECH(SCARA)
          ECHO_EM("Configuration_Scara.h");
        #endif
        return;
      }
      ECHO_V(MSG_FWTEST_PRESS);
      ECHO_EM("Y");
      ECHO_EV(MSG_FWTEST_YES);
      serial_answer = ' ';
      while (serial_answer!='y' && serial_answer!='Y' && !(READ(Y_MIN_PIN)^Y_MIN_ENDSTOP_INVERTING)) {
        serial_answer = MYSERIAL.read();
      }
      if (READ(Y_MIN_PIN)^Y_MIN_ENDSTOP_INVERTING) {
        ECHO_M("MIN ENDSTOP Y: ");
        ECHO_EV(MSG_ENDSTOP_HIT);
      }
      else {
        ECHO_M("Y ");
        ECHO_EV(MSG_FWTEST_ENDSTOP_ERR);
        return;
      }
    #elif PIN_EXISTS(Y_MAX) && Y_HOME_DIR == 1
      if (!READ(Y_MAX_PIN)^Y_MAX_ENDSTOP_INVERTING) {
        ECHO_M("MAX ENDSTOP Y: ");
        ECHO_EV(MSG_ENDSTOP_OPEN);
      }
      else {
        ECHO_M("Y ENDSTOP ");
        ECHO_EM(MSG_FWTEST_ERROR);
        ECHO_M(MSG_FWTEST_INVERT);
        ECHO_M("#define Y_MAX_ENDSTOP_LOGIC ");
        ECHO_M(MSG_FWTEST_INTO);
        #if MECH(CARTESIAN)
          ECHO_EM("Configuration_Cartesian.h");
        #elif MECH(COREXY)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(COREXZ)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(DELTA)
          ECHO_EM("Configuration_Delta.h");
        #elif MECH(SCARA)
          ECHO_EM("Configuration_Scara.h");
        #endif
        return;
      }
      ECHO_V(MSG_FWTEST_PRESS);
      ECHO_EM("Y");
      ECHO_EV(MSG_FWTEST_YES);
      serial_answer = ' ';
      while (serial_answer!='y' && serial_answer!='Y' && !(READ(Y_MAX_PIN)^Y_MAX_ENDSTOP_INVERTING)) {
        serial_answer = MYSERIAL.read();
      }
      if (READ(Y_MAX_PIN)^Y_MAX_ENDSTOP_INVERTING) {
        ECHO_M("MAX ENDSTOP Y: ");
        ECHO_EV(MSG_ENDSTOP_HIT);
      }
      else {
        ECHO_M("Y ");
        ECHO_EV(MSG_FWTEST_ENDSTOP_ERR);
        return;
      }
    #elif Y_HOME_DIR == -1
      ECHO_M(MSG_FWTEST_ERROR);
      ECHO_M("!!! Y_MIN_PIN ");
      ECHO_EM(MSG_FWTEST_NDEF);
      return;
    #elif Y_HOME_DIR == 1
      ECHO_M(MSG_FWTEST_ERROR);
      ECHO_M("!!! Y_MAX_PIN ");
      ECHO_EM(MSG_FWTEST_NDEF);
      return;
    #endif

    ECHO_EM(" ");
    ECHO_EM("***** ENDSTOP Z *****");
    #if PIN_EXISTS(Z_MIN) && Z_HOME_DIR == -1
      if (!READ(Z_MIN_PIN)^Z_MIN_ENDSTOP_INVERTING) {
        ECHO_M("MIN ENDSTOP Z: ");
        ECHO_EV(MSG_ENDSTOP_OPEN);
      }
      else {
        ECHO_M("Z ENDSTOP ");
        ECHO_EM(MSG_FWTEST_ERROR);
        ECHO_M(MSG_FWTEST_INVERT);
        ECHO_M("#define Z_MIN_ENDSTOP_LOGIC ");
        ECHO_M(MSG_FWTEST_INTO);
        #if MECH(CARTESIAN)
          ECHO_EM("Configuration_Cartesian.h");
        #elif MECH(COREXY)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(COREXZ)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(DELTA)
          ECHO_EM("Configuration_Delta.h");
        #elif MECH(SCARA)
          ECHO_EM("Configuration_Scara.h");
        #endif
        return;
      }
      ECHO_V(MSG_FWTEST_PRESS);
      ECHO_EM("Z");
      ECHO_EV(MSG_FWTEST_YES);
      serial_answer = ' ';
      while (serial_answer!='y' && serial_answer!='Y' && !(READ(Z_MIN_PIN)^Z_MIN_ENDSTOP_INVERTING)) {
        serial_answer = MYSERIAL.read();
      }
      if (READ(Z_MIN_PIN)^Z_MIN_ENDSTOP_INVERTING) {
        ECHO_M("MIN ENDSTOP Z: ");
        ECHO_EV(MSG_ENDSTOP_HIT);
      }
      else {
        ECHO_M("Z ");
        ECHO_EV(MSG_FWTEST_ENDSTOP_ERR);
        return;
      }
    #elif PIN_EXISTS(Z_MAX) && Z_HOME_DIR == 1
      if (!READ(Z_MAX_PIN)^Z_MAX_ENDSTOP_INVERTING) {
        ECHO_M("MAX ENDSTOP Z: ");
        ECHO_EV(MSG_ENDSTOP_OPEN);
      }
      else {
        ECHO_M("Z ENDSTOP ");
        ECHO_EM(MSG_FWTEST_ERROR);
        ECHO_M(MSG_FWTEST_INVERT);
        ECHO_M("#define Z_MAX_ENDSTOP_LOGIC ");
        ECHO_M(MSG_FWTEST_INTO);
        #if MECH(CARTESIAN)
          ECHO_EM("Configuration_Cartesian.h");
        #elif MECH(COREXY)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(COREXZ)
          ECHO_EM("Configuration_Core.h");
        #elif MECH(DELTA)
          ECHO_EM("Configuration_Delta.h");
        #elif MECH(SCARA)
          ECHO_EM("Configuration_Scara.h");
        #endif
        return;
      }
      ECHO_V(MSG_FWTEST_PRESS);
      ECHO_EM("Z");
      ECHO_EV(MSG_FWTEST_YES);
      serial_answer = ' ';
      while (serial_answer!='y' && serial_answer!='Y' && !(READ(Z_MAX_PIN)^Z_MAX_ENDSTOP_INVERTING)) {
        serial_answer = MYSERIAL.read();
      }
      if (READ(Z_MAX_PIN)^Z_MAX_ENDSTOP_INVERTING) {
        ECHO_M("MAX ENDSTOP Z: ");
        ECHO_EV(MSG_ENDSTOP_HIT);
      }
      else {
        ECHO_M("Z ");
        ECHO_EV(MSG_FWTEST_ENDSTOP_ERR);
        return;
      }
    #elif Z_HOME_DIR == -1
      ECHO_M(MSG_FWTEST_ERROR);
      ECHO_M("!!! Z_MIN_PIN ");
      ECHO_EM(MSG_FWTEST_NDEF);
      return;
    #elif Z_HOME_DIR == 1
      ECHO_M(MSG_FWTEST_ERROR);
      ECHO_M("!!! Z_MAX_PIN ");
      ECHO_EM(MSG_FWTEST_NDEF);
      return;
    #endif

    ECHO_EM("ENDSTOP ");
    ECHO_M(MSG_FWTEST_OK);
    ECHO_EM(" ");
  }

  #if HAS(POWER_SWITCH)
    SET_OUTPUT(PS_ON_PIN);
    WRITE(PS_ON_PIN, PS_ON_AWAKE);
  #endif

  // Reset position to 0
  st_synchronize();
  for (int8_t i = 0; i < NUM_AXIS; i++) current_position[i] = 0;
  plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);

  ECHO_EM("***** TEST MOTOR  *****");
  ECHO_EV(MSG_FWTEST_ATTENTION);
  ECHO_EV(MSG_FWTEST_YES);
  serial_answer = ' ';
  while (serial_answer!='y' && serial_answer!='Y') {
    serial_answer = MYSERIAL.read();
  }
  ECHO_EV(MSG_FWTEST_04);
  ECHO_EM(" ");
  ECHO_EM("***** MOTOR X *****");
  destination[X_AXIS] = 10;
  prepare_move();
  st_synchronize();

  ECHO_EV(MSG_FWTEST_XAXIS);
  ECHO_EV(MSG_FWTEST_YES_NO);
  serial_answer = ' ';
  while (serial_answer!='y' && serial_answer!='Y' && serial_answer!='n' && serial_answer!='N') {
    serial_answer = MYSERIAL.read();
  }
  if (serial_answer=='y' || serial_answer=='Y') {
    ECHO_EM("MOTOR X ");
    ECHO_M(MSG_FWTEST_OK);
  }
  else {
    ECHO_M(MSG_FWTEST_INVERT);
    ECHO_M("#define INVERT_X_DIR ");
    ECHO_M(MSG_FWTEST_INTO);
    #if MECH(CARTESIAN)
      ECHO_EM("Configuration_Cartesian.h");
    #elif MECH(COREXY)
      ECHO_EM("Configuration_Core.h");
    #elif MECH(COREXZ)
      ECHO_EM("Configuration_Core.h");
    #elif MECH(DELTA)
      ECHO_EM("Configuration_Delta.h");
    #elif MECH(SCARA)
      ECHO_EM("Configuration_Scara.h");
    #endif
    return;
  }
  ECHO_EM(" ");
  ECHO_EM("***** MOTOR Y *****");
  destination[Y_AXIS] = 10;
  prepare_move();
  st_synchronize();
  ECHO_EV(MSG_FWTEST_YAXIS);
  ECHO_EV(MSG_FWTEST_YES_NO);
  serial_answer = ' ';
  while (serial_answer!='y' && serial_answer!='Y' && serial_answer!='n' && serial_answer!='N') {
    serial_answer = MYSERIAL.read();
  }
  if (serial_answer=='y' || serial_answer=='Y') {
    ECHO_EM("MOTOR Y ");
    ECHO_M(MSG_FWTEST_OK);
  }
  else {
    ECHO_M(MSG_FWTEST_INVERT);
    ECHO_M("#define INVERT_Y_DIR ");
    ECHO_M(MSG_FWTEST_INTO);
    #if MECH(CARTESIAN)
      ECHO_EM("Configuration_Cartesian.h");
    #elif MECH(COREXY)
      ECHO_EM("Configuration_Core.h");
    #elif MECH(COREXZ)
      ECHO_EM("Configuration_Core.h");
    #elif MECH(DELTA)
      ECHO_EM("Configuration_Delta.h");
    #elif MECH(SCARA)
      ECHO_EM("Configuration_Scara.h");
    #endif
    return;
  }
  ECHO_EM(" ");
  ECHO_EM("***** MOTOR Z *****");
  destination[Z_AXIS] = 10;
  prepare_move();
  st_synchronize();
  ECHO_EV(MSG_FWTEST_ZAXIS);
  ECHO_EV(MSG_FWTEST_YES_NO);
  serial_answer = ' ';
  while (serial_answer!='y' && serial_answer!='Y' && serial_answer!='n' && serial_answer!='N') {
    serial_answer = MYSERIAL.read();
  }
  if (serial_answer=='y' || serial_answer=='Y') {
    ECHO_EM("MOTOR Z ");
    ECHO_M(MSG_FWTEST_OK);
  }
  else {
    ECHO_M(MSG_FWTEST_INVERT);
    ECHO_M("#define INVERT_Z_DIR ");
    ECHO_M(MSG_FWTEST_INTO);
    #if MECH(CARTESIAN)
      ECHO_EM("Configuration_Cartesian.h");
    #elif MECH(COREXY)
      ECHO_EM("Configuration_Core.h");
    #elif MECH(COREXZ)
      ECHO_EM("Configuration_Core.h");
    #elif MECH(DELTA)
      ECHO_EM("Configuration_Delta.h");
    #elif MECH(SCARA)
      ECHO_EM("Configuration_Scara.h");
    #endif
    return;
  }
  ECHO_EM("MOTOR ");
  ECHO_M(MSG_FWTEST_OK);
  ECHO_EM(" ");
  ECHO_V(MSG_FWTEST_END);
}
示例#7
0
float Probe_Bed(float x_pos, float y_pos, int n)
{
    //returns Probed Z average height
    float ProbeDepth[n];
    float ProbeDepthAvg=0;
    
    //force bed heater off for probing
    int save_bed_targ = target_raw_bed;
    target_raw_bed = 0;
    WRITE(HEATER_BED_PIN,LOW);
    
    if (Z_HOME_DIR==-1)
    {
      //int probe_flag =1;
      float meas = 0;
      int fails = 0;
      saved_feedrate = feedrate;
      saved_feedmultiply = feedmultiply;
      feedmultiply = 100;
      //previous_millis_cmd = millis();
      
      //Move to probe position
      if (x_pos >= 0) destination[X_AXIS]=x_pos;
      if (y_pos >= 0) destination[Y_AXIS]=y_pos;
      //destination[Z_AXIS]=current_position[Z_AXIS];
      destination[Z_AXIS]=Z_HOME_RETRACT_MM;
      feedrate = 9000;
      prepare_move();

	  enable_endstops(true);
      SERIAL_ECHO("PRE-PROBE current_position[Z_AXIS]=");SERIAL_ECHOLN(current_position[Z_AXIS]);

	  SERIAL_ECHOLN("Ready to probe...");

        //Probe bed n times
        //*******************************************************************************************Bed Loop*************************************
        for(int8_t i=0; i < n ; i++)
        {
            //int z = 0;

			//fast probe 
			//plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); 
			destination[Z_AXIS] = 1.1 * Z_MAX_LENGTH * Z_HOME_DIR; 
			feedrate = homing_feedrate[Z_AXIS]; 
			plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); 
			st_synchronize();
		
			//feedrate = 0.0;
            
            SERIAL_ECHO("current_position[Z_AXIS]=");SERIAL_ECHOLN(current_position[Z_AXIS]);
            if(endstop_z_hit == true)
            {
	            SERIAL_ECHO("endstops_trigsteps[Z_AXIS]=");SERIAL_ECHOLN(endstops_trigsteps[Z_AXIS]);
	            ProbeDepth[i]= endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS];
	            meas = ProbeDepth[i];
	            SERIAL_ECHO("ProbeDepth[");SERIAL_ECHO(i);SERIAL_ECHO("]=");SERIAL_ECHOLN(ProbeDepth[i]);
            	//*************************************************************************************************************
		        if (i > 0 ) //Second probe has happened so compare results
		        {
		            if (abs(ProbeDepth[i] - ProbeDepth[i - 1]) > .05)
		            { //keep going until readings match to avoid sticky bed
		              SERIAL_ECHO("Probing again: ");
		              SERIAL_ECHO(ProbeDepth[i]); SERIAL_ECHO(" - "); SERIAL_ECHO(ProbeDepth[i - 1]);SERIAL_ECHO(" = "); SERIAL_ECHOLN(abs(ProbeDepth[i] - ProbeDepth[i - 1]));
		              meas = ProbeDepth[i];
		              i--; i--; //Throw out both that don't match because we don't know which one is accurate
		              if(fails++ > 4) break;
		            }
		        }
	        }else{
	        	SERIAL_ECHOLN("Probe not triggered.");
	        	i=n-1;
	        }
            //**************************************************************************************************************************************************
            //fast move clear
		    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], meas, current_position[E_AXIS]);
		    destination[Z_AXIS] = Z_HOME_RETRACT_MM;
		    feedrate = fast_home_feedrate[Z_AXIS];
		    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
		    st_synchronize();

            //check z stop isn't still triggered
            if ( READ(X_MIN_PIN) != X_ENDSTOPS_INVERTING )
            {
                SERIAL_ECHOLN("Poking Stuck Bed:");
                destination[Z_AXIS] = -1; prepare_move();
                destination[Z_AXIS] = Z_HOME_RETRACT_MM; prepare_move();
			    st_synchronize();
                i--; //Throw out this meaningless measurement
            }
            feedrate = 0;
        } //end probe loop
		#ifdef ENDSTOPS_ONLY_FOR_HOMING
		  enable_endstops(false);
		#endif
		 
		feedrate = saved_feedrate;
		feedmultiply = saved_feedmultiply;
		//previous_millis_cmd = millis();
		endstops_hit_on_purpose();
    }
    for(int8_t i=0;i<n;i++)
    {
    	ProbeDepthAvg += ProbeDepth[i];
    }
    ProbeDepthAvg /= n;
    SERIAL_ECHO("Probed Z="); SERIAL_ECHOLN(ProbeDepthAvg);
    SERIAL_ECHO("RAW current_position[Z_AXIS]=");SERIAL_ECHOLN(current_position[Z_AXIS]);
    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], Z_HOME_RETRACT_MM, current_position[E_AXIS]);
    current_position[Z_AXIS] = Z_HOME_RETRACT_MM;

    target_raw_bed = save_bed_targ;
    return ProbeDepthAvg;
 }