Apply leveling for DELTA,

- Fix `prepare_move` function not calling `adjust_delta` - Add more shorthand for plan_buffer_line. - Fix wrong `federate` usage, assuming they are all mm/m - Minor `stepper.cpp` cleanup
10 years ago · d73b169de3
parent ce44bee28b
commit d73b169de3
2 changed files with 571 additions and 513 deletions
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@ -79,7 +79,7 @@
 // G4  - Dwell S<seconds> or P<milliseconds>
 // G10 - retract filament according to settings of M207
 // G11 - retract recover filament according to settings of M208
-// G28 - Home all Axis
+// G28 - Home one or more axes
 // G29 - Detailed Z-Probe, probes the bed at 3 or more points.  Will fail if you haven't homed yet.
 // G30 - Single Z Probe, probes bed at current XY location.
 // G31 - Dock sled (Z_PROBE_SLED only)
@ -477,8 +477,6 @@ bool enquecommand(const char *cmd)
  return true;
 }

-
-
 void setup_killpin()
 {
  #if defined(KILL_PIN) && KILL_PIN > -1
@ -966,10 +964,10 @@ static void axis_is_at_home(int axis) {
        return;
      }
      else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE) {
-        current_position[X_AXIS] = base_home_pos(X_AXIS) + home_offset[X_AXIS];
-        min_pos[X_AXIS] = base_min_pos(X_AXIS) + home_offset[X_AXIS];
-        max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + home_offset[X_AXIS],
-                                max(extruder_offset[1][X_AXIS], X2_MAX_POS) - duplicate_extruder_x_offset);
+        float xoff = home_offset[X_AXIS];
+        current_position[X_AXIS] = base_home_pos(X_AXIS) + xoff;
+                 min_pos[X_AXIS] = base_min_pos(X_AXIS) + xoff;
+                 max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + xoff, max(extruder_offset[1][X_AXIS], X2_MAX_POS) - duplicate_extruder_x_offset);
        return;
      }
    }
@ -1023,16 +1021,27 @@ static void axis_is_at_home(int axis) {
 }

 /**
- * Shorthand to tell the planner our current position (in mm).
+ * Some planner shorthand inline functions
 */
+inline void line_to_current_position() {
+  plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
+}
+inline void line_to_z(float zPosition) {
+  plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
+}
+inline void line_to_destination() {
+  plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+}
 inline void sync_plan_position() {
  plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 }

 #ifdef ENABLE_AUTO_BED_LEVELING
-#ifdef AUTO_BED_LEVELING_GRID

-#ifndef DELTA
+  #ifdef AUTO_BED_LEVELING_GRID
+
+    #ifndef DELTA
+
      static void set_bed_level_equation_lsq(double *plane_equation_coefficients) {
        vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
        planeNormal.debug("planeNormal");
@ -1051,11 +1060,12 @@ inline void sync_plan_position() {

        sync_plan_position();
      }
-#endif

-#else // not AUTO_BED_LEVELING_GRID
+    #endif // !DELTA
+
+  #else // !AUTO_BED_LEVELING_GRID

-static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
+    static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {

      plan_bed_level_matrix.set_to_identity();

@ -1078,11 +1088,12 @@ static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float
      current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z

      sync_plan_position();
-}
+    }

-#endif // AUTO_BED_LEVELING_GRID
+  #endif // !AUTO_BED_LEVELING_GRID
+
+  static void run_z_probe() {

-static void run_z_probe() {
    #ifdef DELTA
    
      float start_z = current_position[Z_AXIS];
@ -1102,14 +1113,14 @@ static void run_z_probe() {
      calculate_delta(current_position);
      plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
      
-  #else
+    #else // !DELTA

      plan_bed_level_matrix.set_to_identity();
      feedrate = homing_feedrate[Z_AXIS];

      // move down until you find the bed
      float zPosition = -10;
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
+      line_to_z(zPosition);
      st_synchronize();

      // we have to let the planner know where we are right now as it is not where we said to go.
@ -1118,21 +1129,20 @@ static void run_z_probe() {

      // move up the retract distance
      zPosition += home_retract_mm(Z_AXIS);
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
+      line_to_z(zPosition);
      st_synchronize();
      endstops_hit_on_purpose();

      // move back down slowly to find bed
-    if (homing_bump_divisor[Z_AXIS] >= 1) {
-      feedrate = homing_feedrate[Z_AXIS]/homing_bump_divisor[Z_AXIS];
-    } 
+      if (homing_bump_divisor[Z_AXIS] >= 1)
+        feedrate = homing_feedrate[Z_AXIS] / homing_bump_divisor[Z_AXIS];
      else {
-      feedrate = homing_feedrate[Z_AXIS]/10;
+        feedrate = homing_feedrate[Z_AXIS] / 10;
        SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less then 1");
      }

      zPosition -= home_retract_mm(Z_AXIS) * 2;
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
+      line_to_z(zPosition);
      st_synchronize();
      endstops_hit_on_purpose();

@ -1140,13 +1150,13 @@ static void run_z_probe() {
      // make sure the planner knows where we are as it may be a bit different than we last said to move to
      sync_plan_position();
      
-  #endif
-}
+    #endif // !DELTA
+  }

-static void do_blocking_move_to(float x, float y, float z) {
+  static void do_blocking_move_to(float x, float y, float z) {
    float oldFeedRate = feedrate;

-#ifdef DELTA
+    #ifdef DELTA

      feedrate = XY_TRAVEL_SPEED;
      
@ -1156,49 +1166,49 @@ static void do_blocking_move_to(float x, float y, float z) {
      prepare_move_raw();
      st_synchronize();

-#else
+    #else

      feedrate = homing_feedrate[Z_AXIS];

      current_position[Z_AXIS] = z;
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
+      line_to_current_position();
      st_synchronize();

      feedrate = xy_travel_speed;

      current_position[X_AXIS] = x;
      current_position[Y_AXIS] = y;
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
+      line_to_current_position();
      st_synchronize();

-#endif
+    #endif

    feedrate = oldFeedRate;
-}
+  }

-static void setup_for_endstop_move() {
+  static void setup_for_endstop_move() {
    saved_feedrate = feedrate;
    saved_feedmultiply = feedmultiply;
    feedmultiply = 100;
    previous_millis_cmd = millis();
-
    enable_endstops(true);
-}
+  }

-static void clean_up_after_endstop_move() {
-#ifdef ENDSTOPS_ONLY_FOR_HOMING
+  static void clean_up_after_endstop_move() {
+    #ifdef ENDSTOPS_ONLY_FOR_HOMING
      enable_endstops(false);
-#endif
-
+    #endif
    feedrate = saved_feedrate;
    feedmultiply = saved_feedmultiply;
    previous_millis_cmd = millis();
-}
+  }
+
+  static void engage_z_probe() {

-static void engage_z_probe() {
-  // Engage Z Servo endstop if enabled
    #ifdef SERVO_ENDSTOPS
-    if (servo_endstops[Z_AXIS] > -1) {
+
+      // Engage Z Servo endstop if enabled
+      if (servo_endstops[Z_AXIS] >= 0) {
        #if SERVO_LEVELING
          servos[servo_endstops[Z_AXIS]].attach(0);
        #endif
@ -1208,7 +1218,9 @@ static void engage_z_probe() {
          servos[servo_endstops[Z_AXIS]].detach();
        #endif
      }
+
    #elif defined(Z_PROBE_ALLEN_KEY)
+
      feedrate = homing_feedrate[X_AXIS];

      // Move to the start position to initiate deployment
@ -1230,25 +1242,26 @@ static void engage_z_probe() {
      st_synchronize();
      
      bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
-    if (z_min_endstop)
-    {
-        if (!Stopped)
-        {
+      if (z_min_endstop) {
+        if (!Stopped) {
          SERIAL_ERROR_START;
          SERIAL_ERRORLNPGM("Z-Probe failed to engage!");
          LCD_ALERTMESSAGEPGM("Err: ZPROBE");
        }
        Stop();
      }
-  #endif

-}
+    #endif // Z_PROBE_ALLEN_KEY
+
+  }
+
+  static void retract_z_probe() {

-static void retract_z_probe() {
-  // Retract Z Servo endstop if enabled
    #ifdef SERVO_ENDSTOPS
-    if (servo_endstops[Z_AXIS] > -1)
-    {
+
+      // Retract Z Servo endstop if enabled
+      if (servo_endstops[Z_AXIS] >= 0) {
+
        #if Z_RAISE_AFTER_PROBING > 0
          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_AFTER_PROBING);
          st_synchronize();
@ -1257,13 +1270,17 @@ static void retract_z_probe() {
        #if SERVO_LEVELING
          servos[servo_endstops[Z_AXIS]].attach(0);
        #endif
+
        servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
+
        #if SERVO_LEVELING
          delay(PROBE_SERVO_DEACTIVATION_DELAY);
          servos[servo_endstops[Z_AXIS]].detach();
        #endif
      }
+
    #elif defined(Z_PROBE_ALLEN_KEY)
+
      // Move up for safety
      feedrate = homing_feedrate[X_AXIS];
      destination[Z_AXIS] = current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING;
@ -1294,29 +1311,28 @@ static void retract_z_probe() {
      st_synchronize();
      
      bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
-    if (!z_min_endstop)
-    {
-        if (!Stopped)
-        {
+      if (!z_min_endstop) {
+        if (!Stopped) {
          SERIAL_ERROR_START;
          SERIAL_ERRORLNPGM("Z-Probe failed to retract!");
          LCD_ALERTMESSAGEPGM("Err: ZPROBE");
        }
        Stop();
      }
+
    #endif

-}
+  }

-enum ProbeAction {
+  enum ProbeAction {
    ProbeStay             = 0,
    ProbeEngage           = BIT(0),
    ProbeRetract          = BIT(1),
    ProbeEngageAndRetract = (ProbeEngage | ProbeRetract)
-};
+  };

-/// Probe bed height at position (x,y), returns the measured z value
-static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeEngageAndRetract, int verbose_level=1) {
+  // Probe bed height at position (x,y), returns the measured z value
+  static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeEngageAndRetract, int verbose_level=1) {
    // move to right place
    do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
    do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
@ -1343,10 +1359,15 @@ static float probe_pt(float x, float y, float z_before, ProbeAction retract_acti
      SERIAL_EOL;
    }
    return measured_z;
-}
+  }

-#ifdef DELTA
-static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
+  #ifdef DELTA
+
+    /**
+     * All DELTA leveling in the Marlin uses NONLINEAR_BED_LEVELING
+     */
+
+    static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
      if (bed_level[x][y] != 0.0) {
        return;  // Don't overwrite good values.
      }
@ -1362,11 +1383,11 @@ static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
        if (a < c) median = a;
      }
      bed_level[x][y] = median;
-}
+    }

-// Fill in the unprobed points (corners of circular print surface)
-// using linear extrapolation, away from the center.
-static void extrapolate_unprobed_bed_level() {
+    // Fill in the unprobed points (corners of circular print surface)
+    // using linear extrapolation, away from the center.
+    static void extrapolate_unprobed_bed_level() {
      int half = (AUTO_BED_LEVELING_GRID_POINTS-1)/2;
      for (int y = 0; y <= half; y++) {
        for (int x = 0; x <= half; x++) {
@ -1377,10 +1398,10 @@ static void extrapolate_unprobed_bed_level() {
          extrapolate_one_point(half+x, half+y, x>1?-1:0, y>1?-1:0);
        }
      }
-}
+    }

-// Print calibration results for plotting or manual frame adjustment.
-static void print_bed_level() {
+    // Print calibration results for plotting or manual frame adjustment.
+    static void print_bed_level() {
      for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
        for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
          SERIAL_PROTOCOL_F(bed_level[x][y], 2);
@ -1388,80 +1409,82 @@ static void print_bed_level() {
        }
        SERIAL_ECHOLN("");
      }
-}
+    }

-// Reset calibration results to zero.
-void reset_bed_level() {
+    // Reset calibration results to zero.
+    void reset_bed_level() {
      for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
        for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
          bed_level[x][y] = 0.0;
        }
      }
-}
+    }

-#endif // DELTA
+  #endif // DELTA

 #endif // ENABLE_AUTO_BED_LEVELING

 static void homeaxis(int axis) {
-#define HOMEAXIS_DO(LETTER) \
+  #define HOMEAXIS_DO(LETTER) \
    ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))

-  if (axis==X_AXIS ? HOMEAXIS_DO(X) :
-      axis==Y_AXIS ? HOMEAXIS_DO(Y) :
-      axis==Z_AXIS ? HOMEAXIS_DO(Z) :
-      0) {
-    int axis_home_dir = home_dir(axis);
-#ifdef DUAL_X_CARRIAGE
-    if (axis == X_AXIS)
-      axis_home_dir = x_home_dir(active_extruder);
-#endif
+  if (axis == X_AXIS ? HOMEAXIS_DO(X) :
+      axis == Y_AXIS ? HOMEAXIS_DO(Y) :
+      axis == Z_AXIS ? HOMEAXIS_DO(Z) : 0) {
+
+    int axis_home_dir;
+
+    #ifdef DUAL_X_CARRIAGE
+      if (axis == X_AXIS) axis_home_dir = x_home_dir(active_extruder);
+    #else
+      axis_home_dir = home_dir(axis);
+    #endif

    current_position[axis] = 0;
    sync_plan_position();

-
-#ifndef Z_PROBE_SLED
+    #ifndef Z_PROBE_SLED
      // Engage Servo endstop if enabled
      #ifdef SERVO_ENDSTOPS
        #if SERVO_LEVELING
-        if (axis==Z_AXIS) {
+          if (axis == Z_AXIS) {
            engage_z_probe();
          }
          else
-      #endif
-      if (servo_endstops[axis] > -1) {
+        #endif // SERVO_LEVELING
+
+        if (servo_endstops[axis] > -1)
          servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
-      }
-    #endif
-#endif // Z_PROBE_SLED
+
+      #endif // SERVO_ENDSTOPS
+
+    #endif // Z_PROBE_SLED
+
    #ifdef Z_DUAL_ENDSTOPS
-      if (axis==Z_AXIS) In_Homing_Process(true);
+      if (axis == Z_AXIS) In_Homing_Process(true);
    #endif
+
    destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
    feedrate = homing_feedrate[axis];
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    line_to_destination();
    st_synchronize();

    current_position[axis] = 0;
    sync_plan_position();
    destination[axis] = -home_retract_mm(axis) * axis_home_dir;
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    line_to_destination();
    st_synchronize();

-    destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;
+    destination[axis] = 2 * home_retract_mm(axis) * axis_home_dir;

    if (homing_bump_divisor[axis] >= 1)
-    {
-        feedrate = homing_feedrate[axis]/homing_bump_divisor[axis];
-    } 
-    else
-    {
-        feedrate = homing_feedrate[axis]/10;
+      feedrate = homing_feedrate[axis] / homing_bump_divisor[axis];
+    else {
+      feedrate = homing_feedrate[axis] / 10;
      SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less then 1");
    }

-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    line_to_destination();
    st_synchronize();
    #ifdef Z_DUAL_ENDSTOPS
      if (axis==Z_AXIS)
@ -1476,7 +1499,7 @@ static void homeaxis(int axis) {
          destination[axis] = fabs(z_endstop_adj);
          if (z_endstop_adj < 0) Lock_z_motor(true); else Lock_z2_motor(true);        
        }
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        line_to_destination();
        st_synchronize();
        Lock_z_motor(false);
        Lock_z2_motor(false);
@ -1489,7 +1512,7 @@ static void homeaxis(int axis) {
    if (endstop_adj[axis] * axis_home_dir < 0) {
      sync_plan_position();
      destination[axis] = endstop_adj[axis];
-      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+      line_to_destination();
      st_synchronize();
    }
 #endif
@ -1534,7 +1557,7 @@ void refresh_cmd_timeout(void)
      }
      plan_set_e_position(current_position[E_AXIS]);
      float oldFeedrate = feedrate;
-      feedrate=retract_feedrate*60;
+      feedrate = retract_feedrate * 60;
      retracted[active_extruder]=true;
      prepare_move();
      if(retract_zlift > 0.01) {
@ -1570,8 +1593,8 @@ void refresh_cmd_timeout(void)
      }
      plan_set_e_position(current_position[E_AXIS]);
      float oldFeedrate = feedrate;
-      feedrate=retract_recover_feedrate*60;
-      retracted[active_extruder]=false;
+      feedrate = retract_recover_feedrate * 60;
+      retracted[active_extruder] = false;
      prepare_move();
      feedrate = oldFeedrate;
    }
@ -1725,17 +1748,16 @@ inline void gcode_G4() {
 */
 inline void gcode_G28() {
  #ifdef ENABLE_AUTO_BED_LEVELING
+    plan_bed_level_matrix.set_to_identity();  //Reset the plane ("erase" all leveling data)
    #ifdef DELTA
      reset_bed_level();
-    #else
-      plan_bed_level_matrix.set_to_identity();  //Reset the plane ("erase" all leveling data)
    #endif
  #endif

  #if defined(MESH_BED_LEVELING)
    uint8_t mbl_was_active = mbl.active;
    mbl.active = 0;
-  #endif  // MESH_BED_LEVELING
+  #endif

  saved_feedrate = feedrate;
  saved_feedmultiply = feedmultiply;
@ -1758,7 +1780,7 @@ inline void gcode_G28() {

    for (int i = X_AXIS; i <= Z_AXIS; i++) destination[i] = 3 * Z_MAX_LENGTH;
    feedrate = 1.732 * homing_feedrate[X_AXIS];
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    line_to_destination();
    st_synchronize();
    endstops_hit_on_purpose();

@ -1806,7 +1828,7 @@ inline void gcode_G28() {
        } else {
          feedrate *= sqrt(pow(max_length(X_AXIS) / max_length(Y_AXIS), 2) + 1);
        }
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        line_to_destination();
        st_synchronize();

        axis_is_at_home(X_AXIS);
@ -1814,7 +1836,7 @@ inline void gcode_G28() {
        sync_plan_position();
        destination[X_AXIS] = current_position[X_AXIS];
        destination[Y_AXIS] = current_position[Y_AXIS];
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        line_to_destination();
        feedrate = 0.0;
        st_synchronize();
        endstops_hit_on_purpose();
@ -1881,7 +1903,7 @@ inline void gcode_G28() {
          #if defined(Z_RAISE_BEFORE_HOMING) && Z_RAISE_BEFORE_HOMING > 0
            destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
            feedrate = max_feedrate[Z_AXIS];
-            plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+            line_to_destination();
            st_synchronize();
          #endif
          HOMEAXIS(Z);
@ -1893,11 +1915,11 @@ inline void gcode_G28() {
          destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER);
          destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER);
          destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
-          feedrate = XY_TRAVEL_SPEED / 60;
+          feedrate = XY_TRAVEL_SPEED;
          current_position[Z_AXIS] = 0;

          sync_plan_position();
-          plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+          line_to_destination();
          st_synchronize();
          current_position[X_AXIS] = destination[X_AXIS];
          current_position[Y_AXIS] = destination[Y_AXIS];
@ -1919,7 +1941,7 @@ inline void gcode_G28() {
              plan_set_position(cpx, cpy, current_position[Z_AXIS], current_position[E_AXIS]);
              destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
              feedrate = max_feedrate[Z_AXIS];
-              plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+              line_to_destination();
              st_synchronize();
              HOMEAXIS(Z);
            }
@ -1972,7 +1994,7 @@ inline void gcode_G28() {
      destination[Z_AXIS] = current_position[Z_AXIS];
      destination[E_AXIS] = current_position[E_AXIS];
      feedrate = homing_feedrate[X_AXIS];
-      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+      line_to_destination();
      st_synchronize();
      current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
      sync_plan_position();
@ -1986,6 +2008,19 @@ inline void gcode_G28() {
  endstops_hit_on_purpose();
 }

+#if defined(MESH_BED_LEVELING) || defined(ENABLE_AUTO_BED_LEVELING)
+
+  // Check for known positions in X and Y
+  bool can_run_bed_leveling() {
+  	if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) return true;
+    LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
+    SERIAL_ECHO_START;
+    SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
+    return false;
+  }
+
+#endif // MESH_BED_LEVELING || ENABLE_AUTO_BED_LEVELING
+
 #ifdef MESH_BED_LEVELING

  /**
@ -2000,6 +2035,10 @@ inline void gcode_G28() {
   *
   */
  inline void gcode_G29() {
+
+    // Prevent leveling without first homing in X and Y
+    if (!can_run_bed_leveling()) return;
+
    static int probe_point = -1;
    int state = 0;
    if (code_seen('S') || code_seen('s')) {
@ -2116,13 +2155,8 @@ inline void gcode_G28() {
   */
  inline void gcode_G29() {

-    // Prevent user from running a G29 without first homing in X and Y
-    if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
-      LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
-      SERIAL_ECHO_START;
-      SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
-      return;
-    }
+    // Prevent leveling without first homing in X and Y
+    if (!can_run_bed_leveling()) return;

    int verbose_level = 1;

@ -2204,16 +2238,15 @@ inline void gcode_G28() {

    st_synchronize();

-    if (!dryrun)
-    {
+    if (!dryrun) {
+      // make sure the bed_level_rotation_matrix is identity or the planner will get it wrong
+      plan_bed_level_matrix.set_to_identity();
+
      #ifdef DELTA
        reset_bed_level();
      #else //!DELTA
-
-        // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
        //vector_3 corrected_position = plan_get_position_mm();
        //corrected_position.debug("position before G29");
-        plan_bed_level_matrix.set_to_identity();
        vector_3 uncorrected_position = plan_get_position();
        //uncorrected_position.debug("position during G29");
        current_position[X_AXIS] = uncorrected_position.x;
@ -2221,7 +2254,7 @@ inline void gcode_G28() {
        current_position[Z_AXIS] = uncorrected_position.z;
        sync_plan_position();

-      #endif
+      #endif // !DELTA
    }
    
    setup_for_endstop_move();
@ -2287,8 +2320,7 @@ inline void gcode_G28() {
          #ifdef DELTA
            // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
            float distance_from_center = sqrt(xProbe*xProbe + yProbe*yProbe);
-            if (distance_from_center > DELTA_PROBABLE_RADIUS)
-              continue;
+            if (distance_from_center > DELTA_PROBABLE_RADIUS) continue;
          #endif //DELTA

          // Enhanced G29 - Do not retract servo between probes
@ -2316,6 +2348,11 @@ inline void gcode_G28() {
          #endif

          probePointCounter++;
+
+          manage_heater();
+          manage_inactivity();
+          lcd_update();
+
        } //xProbe
      } //yProbe

@ -2402,16 +2439,14 @@ inline void gcode_G28() {
      if (verbose_level > 0)
        plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");

+      if (!dryrun) {
        // Correct the Z height difference from z-probe position and hotend tip position.
        // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
        // When the bed is uneven, this height must be corrected.
-      if (!dryrun)
-      {
-        float x_tmp, y_tmp, z_tmp, real_z;
-        real_z = float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS];  //get the real Z (since the auto bed leveling is already correcting the plane)
-        x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
-        y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
-        z_tmp = current_position[Z_AXIS];
+        float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
+              y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
+              z_tmp = current_position[Z_AXIS],
+              real_z = (float)st_get_position(Z_AXIS) / axis_steps_per_unit[Z_AXIS];  //get the real Z (since the auto bed leveling is already correcting the plane)

        apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp);         //Apply the correction sending the probe offset
        current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS];   //The difference is added to current position and sent to planner.
@ -4686,18 +4721,14 @@ void process_commands() {
      gcode_G28();
      break;

-    #if defined(MESH_BED_LEVELING)
-      case 29: // G29 Handle mesh based leveling
+    #if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
+      case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
        gcode_G29();
        break;
    #endif

    #ifdef ENABLE_AUTO_BED_LEVELING

-      case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
-        gcode_G29();
-        break;
-
      #ifndef Z_PROBE_SLED

        case 30: // G30 Single Z Probe
@ -5392,28 +5423,27 @@ void prepare_move()
  
  #ifdef SCARA //for now same as delta-code

-float difference[NUM_AXIS];
-for (int8_t i=0; i < NUM_AXIS; i++) {
-  difference[i] = destination[i] - current_position[i];
-}
+    float difference[NUM_AXIS];
+    for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = destination[i] - current_position[i];

-float cartesian_mm = sqrt(  sq(difference[X_AXIS]) +
+    float cartesian_mm = sqrt(  sq(difference[X_AXIS]) +
                                sq(difference[Y_AXIS]) +
                                sq(difference[Z_AXIS]));
-if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
-if (cartesian_mm < 0.000001) { return; }
-float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
-int steps = max(1, int(scara_segments_per_second * seconds));
+    if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
+    if (cartesian_mm < 0.000001) { return; }
+    float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
+    int steps = max(1, int(scara_segments_per_second * seconds));
+
    //SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
    //SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
    //SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
-for (int s = 1; s <= steps; s++) {
+
+    for (int s = 1; s <= steps; s++) {
      float fraction = float(s) / float(steps);
-  for(int8_t i=0; i < NUM_AXIS; i++) {
+      for(int8_t i = 0; i < NUM_AXIS; i++) {
        destination[i] = current_position[i] + difference[i] * fraction;
      }
  
-  
      calculate_delta(destination);
      //SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
      //SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]);
@ -5425,36 +5455,40 @@ for (int s = 1; s <= steps; s++) {
      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
        destination[E_AXIS], feedrate*feedmultiply/60/100.0,
        active_extruder);
-}
-#endif // SCARA
+    }
+
+  #endif // SCARA
+  
+  #ifdef DELTA

-#ifdef DELTA
    float difference[NUM_AXIS];
-  for (int8_t i=0; i < NUM_AXIS; i++) {
-    difference[i] = destination[i] - current_position[i];
-  }
+    for (int8_t i=0; i < NUM_AXIS; i++) difference[i] = destination[i] - current_position[i];
+
    float cartesian_mm = sqrt(sq(difference[X_AXIS]) +
                              sq(difference[Y_AXIS]) +
                              sq(difference[Z_AXIS]));
-  if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
-  if (cartesian_mm < 0.000001) { return; }
+    if (cartesian_mm < 0.000001) cartesian_mm = abs(difference[E_AXIS]);
+    if (cartesian_mm < 0.000001) return;
    float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
    int steps = max(1, int(delta_segments_per_second * seconds));
+
    // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
    // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
    // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
+
    for (int s = 1; s <= steps; s++) {
      float fraction = float(s) / float(steps);
-    for(int8_t i=0; i < NUM_AXIS; i++) {
-      destination[i] = current_position[i] + difference[i] * fraction;
-    }
+      for (int8_t i = 0; i < NUM_AXIS; i++) destination[i] = current_position[i] + difference[i] * fraction;
      calculate_delta(destination);
+      #ifdef ENABLE_AUTO_BED_LEVELING
+        adjust_delta(destination);
+      #endif
      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
                       destination[E_AXIS], feedrate*feedmultiply/60/100.0,
                       active_extruder);
    }

-#endif // DELTA
+  #endif // DELTA

 #ifdef DUAL_X_CARRIAGE
  if (active_extruder_parked)
@ -5500,13 +5534,13 @@ for (int s = 1; s <= steps; s++) {
 #if ! (defined DELTA || defined SCARA)
  // Do not use feedmultiply for E or Z only moves
  if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+    line_to_destination();
  } else {
 #if defined(MESH_BED_LEVELING)
-    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
    return;
 #else
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
 #endif  // MESH_BED_LEVELING
  }
 #endif // !(DELTA || SCARA)
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@ -507,83 +507,107 @@ ISR(TIMER1_COMPA_vect) {
    }

    if (TEST(out_bits, Z_AXIS)) {   // -direction
+
      Z_APPLY_DIR(INVERT_Z_DIR,0);
      count_direction[Z_AXIS] = -1;
-      if (check_endstops) 
-      {
-        #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
-          #ifndef Z_DUAL_ENDSTOPS
-            UPDATE_ENDSTOP(z, Z, min, MIN);
-          #else
-            bool z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
-            #if defined(Z2_MIN_PIN) && Z2_MIN_PIN > -1
-              bool z2_min_endstop=(READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING);
+
+      if (check_endstops) {
+
+        #if defined(Z_MIN_PIN) && Z_MIN_PIN >= 0
+
+          #ifdef Z_DUAL_ENDSTOPS
+
+            bool z_min_endstop = READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING,
+                z2_min_endstop =
+                  #if defined(Z2_MIN_PIN) && Z2_MIN_PIN >= 0
+                    READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING
                  #else
-              bool z2_min_endstop=z_min_endstop;
+                    z_min_endstop
                  #endif
-            if(((z_min_endstop && old_z_min_endstop) || (z2_min_endstop && old_z2_min_endstop)) && (current_block->steps[Z_AXIS] > 0))
-            {
+                ;
+
+            bool z_min_both = z_min_endstop && old_z_min_endstop,
+                z2_min_both = z2_min_endstop && old_z2_min_endstop;
+            if ((z_min_both || z2_min_both) && current_block->steps[Z_AXIS] > 0) {
              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-              endstop_z_hit=true;
-              if (!(performing_homing) || ((performing_homing)&&(z_min_endstop && old_z_min_endstop)&&(z2_min_endstop && old_z2_min_endstop))) //if not performing home or if both endstops were trigged during homing...
-              {
+              endstop_z_hit = true;
+              if (!performing_homing || (performing_homing && z_min_both && z2_min_both)) //if not performing home or if both endstops were trigged during homing...
                step_events_completed = current_block->step_event_count;
            }
-            }
            old_z_min_endstop = z_min_endstop;
            old_z2_min_endstop = z2_min_endstop;
-          #endif
-        #endif
-      }
+
+          #else // !Z_DUAL_ENDSTOPS
+
+            UPDATE_ENDSTOP(z, Z, min, MIN);
+
+          #endif // !Z_DUAL_ENDSTOPS
+
+        #endif // Z_MIN_PIN
+
+      } // check_endstops
+
    }
    else { // +direction
+
      Z_APPLY_DIR(!INVERT_Z_DIR,0);
      count_direction[Z_AXIS] = 1;
+
      if (check_endstops) {
+
        #if defined(Z_MAX_PIN) && Z_MAX_PIN >= 0
-          #ifndef Z_DUAL_ENDSTOPS
-            UPDATE_ENDSTOP(z, Z, max, MAX);
-          #else
-            bool z_max_endstop=(READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
-            #if defined(Z2_MAX_PIN) && Z2_MAX_PIN > -1
-              bool z2_max_endstop=(READ(Z2_MAX_PIN) != Z2_MAX_ENDSTOP_INVERTING);
+
+          #ifdef Z_DUAL_ENDSTOPS
+
+            bool z_max_endstop = READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING,
+                z2_max_endstop =
+                  #if defined(Z2_MAX_PIN) && Z2_MAX_PIN >= 0
+                    READ(Z2_MAX_PIN) != Z2_MAX_ENDSTOP_INVERTING
                  #else
-              bool z2_max_endstop=z_max_endstop;
+                    z_max_endstop
                  #endif
-            if(((z_max_endstop && old_z_max_endstop) || (z2_max_endstop && old_z2_max_endstop)) && (current_block->steps[Z_AXIS] > 0))
-            {
-              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-              endstop_z_hit=true;
+                ;

-//              if (z_max_endstop && old_z_max_endstop) SERIAL_ECHOLN("z_max_endstop = true");
-//              if (z2_max_endstop && old_z2_max_endstop) SERIAL_ECHOLN("z2_max_endstop = true");
+            bool z_max_both = z_max_endstop && old_z_max_endstop,
+                z2_max_both = z2_max_endstop && old_z2_max_endstop;
+            if ((z_max_both || z2_max_both) && current_block->steps[Z_AXIS] > 0) {
+              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+              endstop_z_hit = true;

+             // if (z_max_both) SERIAL_ECHOLN("z_max_endstop = true");
+             // if (z2_max_both) SERIAL_ECHOLN("z2_max_endstop = true");

-              if (!(performing_homing) || ((performing_homing)&&(z_max_endstop && old_z_max_endstop)&&(z2_max_endstop && old_z2_max_endstop))) //if not performing home or if both endstops were trigged during homing...
-              {
+              if (!performing_homing || (performing_homing && z_max_both && z2_max_both)) //if not performing home or if both endstops were trigged during homing...
                step_events_completed = current_block->step_event_count;
            }
-            }
            old_z_max_endstop = z_max_endstop;
            old_z2_max_endstop = z2_max_endstop;
-          #endif
-        #endif
-      }
-    }
+
+          #else // !Z_DUAL_ENDSTOPS
+
+            UPDATE_ENDSTOP(z, Z, max, MAX);
+
+          #endif // !Z_DUAL_ENDSTOPS
+
+        #endif // Z_MAX_PIN
+
+      } // check_endstops
+
+    } // +direction

    #ifndef ADVANCE
      if (TEST(out_bits, E_AXIS)) {  // -direction
        REV_E_DIR();
-        count_direction[E_AXIS]=-1;
+        count_direction[E_AXIS] = -1;
      }
      else { // +direction
        NORM_E_DIR();
-        count_direction[E_AXIS]=1;
+        count_direction[E_AXIS] = 1;
      }
    #endif //!ADVANCE

    // Take multiple steps per interrupt (For high speed moves)
-    for (int8_t i=0; i < step_loops; i++) {
+    for (int8_t i = 0; i < step_loops; i++) {
      #ifndef AT90USB
        MSerial.checkRx(); // Check for serial chars.
      #endif