Merge pull request #4868 from thinkyhead/rc_autumn_fixups

Cleanups for the Autumn release

Marlin/Marlin_main.cpp

@@ -1453,7 +1453,6 @@ inline float get_homing_bump_feedrate(AxisEnum axis) {
   return homing_feedrate_mm_s[axis] / hbd;
 }
 
-#if !IS_KINEMATIC
 //
 // line_to_current_position
 // Move the planner to the current position from wherever it last moved
@@ -1472,8 +1471,6 @@ inline float get_homing_bump_feedrate(AxisEnum axis) {
 }
 inline void line_to_destination() { line_to_destination(feedrate_mm_s); }
 
-#endif // !IS_KINEMATIC
-
 inline void set_current_to_destination() { memcpy(current_position, destination, sizeof(current_position)); }
 inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); }
 
@@ -1485,12 +1482,19 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination);
     #endif
+
+    if ( current_position[X_AXIS] == destination[X_AXIS]
+      && current_position[Y_AXIS] == destination[Y_AXIS]
+      && current_position[Z_AXIS] == destination[Z_AXIS]
+      && current_position[E_AXIS] == destination[E_AXIS]
+    ) return;
+
     refresh_cmd_timeout();
     inverse_kinematics(destination);
     planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], destination[E_AXIS], MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
     set_current_to_destination();
   }
-#endif
+#endif // IS_KINEMATIC
 
 /**
  *  Plan a move to (X, Y, Z) and set the current_position
@@ -1557,16 +1561,12 @@ void do_blocking_move_to(const float &x, const float &y, const float &z, const f
       #endif
     }
 
-    #if ENABLED(DEBUG_LEVELING_FEATURE)
-      if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to");
-    #endif
-
   #elif IS_SCARA
 
     set_destination_to_current();
 
     // If Z needs to raise, do it before moving XY
-    if (current_position[Z_AXIS] < z) {
+    if (destination[Z_AXIS] < z) {
       destination[Z_AXIS] = z;
       prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate_mm_s[Z_AXIS]);
     }
@@ -1576,7 +1576,7 @@ void do_blocking_move_to(const float &x, const float &y, const float &z, const f
     prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S);
 
     // If Z needs to lower, do it after moving XY
-    if (current_position[Z_AXIS] > z) {
+    if (destination[Z_AXIS] > z) {
       destination[Z_AXIS] = z;
       prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate_mm_s[Z_AXIS]);
     }
@@ -1607,6 +1607,10 @@ void do_blocking_move_to(const float &x, const float &y, const float &z, const f
   stepper.synchronize();
 
   feedrate_mm_s = old_feedrate_mm_s;
+
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to");
+  #endif
 }
 void do_blocking_move_to_x(const float &x, const float &fr_mm_s/*=0.0*/) {
   do_blocking_move_to(x, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s);
@@ -1999,12 +2003,12 @@ static void clean_up_after_endstop_or_probe_move() {
     // Clear endstop flags
     endstops.hit_on_purpose();
 
+    // Tell the planner where we actually are
+    planner.sync_from_steppers();
+
     // Get Z where the steppers were interrupted
     set_current_from_steppers_for_axis(Z_AXIS);
 
-    // Tell the planner where we actually are
-    SYNC_PLAN_POSITION_KINEMATIC();
-
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) DEBUG_POS("<<< do_probe_move", current_position);
     #endif
@@ -2122,8 +2126,13 @@ static void clean_up_after_endstop_or_probe_move() {
 
   /**
    * Reset calibration results to zero.
+   *
+   * TODO: Proper functions to disable / enable
+   *       bed leveling via a flag, correcting the
+   *       current position in each case.
    */
   void reset_bed_level() {
+    planner.abl_enabled = false;
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("reset_bed_level");
     #endif
@@ -2131,7 +2140,6 @@ static void clean_up_after_endstop_or_probe_move() {
       planner.bed_level_matrix.set_to_identity();
     #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
       memset(bed_level_grid, 0, sizeof(bed_level_grid));
-      nonlinear_grid_spacing[X_AXIS] = nonlinear_grid_spacing[Y_AXIS] = 0;
     #endif
   }
 
@@ -2188,18 +2196,27 @@ static void clean_up_after_endstop_or_probe_move() {
 /**
  * Home an individual linear axis
  */
-
+static void do_homing_move(const AxisEnum axis, float distance, float fr_mm_s=0.0) {
-static void do_homing_move(AxisEnum axis, float where, float fr_mm_s=0.0) {
 
   #if HOMING_Z_WITH_PROBE && ENABLED(BLTOUCH)
     bool deploy_bltouch = (axis == Z_AXIS && where < 0);
     if (deploy_bltouch) set_bltouch_deployed(true);
   #endif
 
+  // Tell the planner we're at Z=0
   current_position[axis] = 0;
+
+  #if IS_SCARA
+    SYNC_PLAN_POSITION_KINEMATIC();
+    current_position[axis] = distance;
+    inverse_kinematics(current_position);
+    planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate_mm_s[axis], active_extruder);
+  #else
     sync_plan_position();
-  current_position[axis] = where;
+    current_position[axis] = distance;
     planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate_mm_s[axis], active_extruder);
+  #endif
+
   stepper.synchronize();
 
   #if HOMING_Z_WITH_PROBE && ENABLED(BLTOUCH)
@@ -2256,6 +2273,9 @@ static void homeaxis(AxisEnum axis) {
     if (axis == Z_AXIS) stepper.set_homing_flag(true);
   #endif
 
+  // Fast move towards endstop until triggered
+  do_homing_move(axis, 1.5 * max_length(axis) * axis_home_dir);
+
   // When homing Z with probe respect probe clearance
   const float bump = axis_home_dir * (
     #if HOMING_Z_WITH_PROBE
@@ -2264,12 +2284,13 @@ static void homeaxis(AxisEnum axis) {
     home_bump_mm(axis)
   );
 
-  // 1. Fast move towards endstop until triggered
+  // If a second homing move is configured...
-  // 2. Move away from the endstop by the axis HOME_BUMP_MM
+  if (bump) {
-  // 3. Slow move towards endstop until triggered
+    // Move away from the endstop by the axis HOME_BUMP_MM
-  do_homing_move(axis, 1.5 * max_length(axis) * axis_home_dir);
     do_homing_move(axis, -bump);
+    // Slow move towards endstop until triggered
     do_homing_move(axis, 2 * bump, get_homing_bump_feedrate(axis));
+  }
 
   #if ENABLED(Z_DUAL_ENDSTOPS)
     if (axis == Z_AXIS) {
@@ -2849,7 +2870,8 @@ inline void gcode_G4() {
 
     // Move all carriages together linearly until an endstop is hit.
     current_position[X_AXIS] = current_position[Y_AXIS] = current_position[Z_AXIS] = (Z_MAX_LENGTH + 10);
-    planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate_mm_s[X_AXIS], active_extruder);
+    feedrate_mm_s = homing_feedrate_mm_s[X_AXIS];
+    line_to_current_position();
     stepper.synchronize();
     endstops.hit_on_purpose(); // clear endstop hit flags
 
@@ -2902,22 +2924,20 @@ inline void gcode_G4() {
     destination[Y_AXIS] = LOGICAL_Y_POSITION(Z_SAFE_HOMING_Y_POINT);
     destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height
 
-    #if HAS_BED_PROBE
-      destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER;
-      destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER;
-    #endif
-
-    #if ENABLED(DEBUG_LEVELING_FEATURE)
-      if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination);
-    #endif
-
     if (position_is_reachable(
           destination
-          #if HAS_BED_PROBE
+          #if HOMING_Z_WITH_PROBE
             , true
           #endif
         )
     ) {
+      #if HOMING_Z_WITH_PROBE
+        destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER;
+        destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER;
+      #endif
+      #if ENABLED(DEBUG_LEVELING_FEATURE)
+        if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination);
+      #endif
       do_blocking_move_to_xy(destination[X_AXIS], destination[Y_AXIS]);
       HOMEAXIS(Z);
     }
@@ -3133,19 +3153,13 @@ inline void gcode_G28() {
         #if ENABLED(MESH_G28_REST_ORIGIN)
           current_position[Z_AXIS] = 0.0;
           set_destination_to_current();
-          feedrate_mm_s = homing_feedrate_mm_s[Z_AXIS];
+          line_to_destination(homing_feedrate_mm_s[Z_AXIS]);
-          line_to_destination();
           stepper.synchronize();
           #if ENABLED(DEBUG_LEVELING_FEATURE)
             if (DEBUGGING(LEVELING)) DEBUG_POS("MBL Rest Origin", current_position);
           #endif
         #else
-          current_position[Z_AXIS] = MESH_HOME_SEARCH_Z -
+          planner.unapply_leveling(current_position);
-            mbl.get_z(RAW_CURRENT_POSITION(X_AXIS), RAW_CURRENT_POSITION(Y_AXIS))
-            #if Z_HOME_DIR > 0
-              + Z_MAX_POS
-            #endif
-          ;
           #if ENABLED(DEBUG_LEVELING_FEATURE)
             if (DEBUGGING(LEVELING)) DEBUG_POS("MBL adjusted MESH_HOME_SEARCH_Z", current_position);
           #endif
@@ -3155,8 +3169,7 @@ inline void gcode_G28() {
         current_position[Z_AXIS] = pre_home_z;
         SYNC_PLAN_POSITION_KINEMATIC();
         mbl.set_active(true);
-        current_position[Z_AXIS] = pre_home_z -
+        planner.unapply_leveling(current_position);
-          mbl.get_z(RAW_CURRENT_POSITION(X_AXIS), RAW_CURRENT_POSITION(Y_AXIS));
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (DEBUGGING(LEVELING)) DEBUG_POS("MBL Home X or Y", current_position);
         #endif
@@ -3505,16 +3518,15 @@ inline void gcode_G28() {
 
     stepper.synchronize();
 
-    if (!dryrun) {
+    // Disable auto bed leveling during G29
+    bool auto_bed_leveling_was_enabled = planner.abl_enabled,
+         abl_should_reenable = auto_bed_leveling_was_enabled;
 
-      // Reset the bed_level_matrix because leveling
+    planner.abl_enabled = false;
-      // needs to be done without leveling enabled.
-      reset_bed_level();
 
-      //
+    if (!dryrun) {
       // Re-orient the current position without leveling
       // based on where the steppers are positioned.
-      //
       get_cartesian_from_steppers();
       memcpy(current_position, cartes, sizeof(cartes));
 
@@ -3525,9 +3537,12 @@ inline void gcode_G28() {
     setup_for_endstop_or_probe_move();
 
     // Deploy the probe. Probe will raise if needed.
-    if (DEPLOY_PROBE()) return;
+    if (DEPLOY_PROBE()) {
+      planner.abl_enabled = abl_should_reenable;
+      return;
+    }
 
-    float xProbe, yProbe, measured_z = 0;
+    float xProbe = 0, yProbe = 0, measured_z = 0;
 
     #if ENABLED(AUTO_BED_LEVELING_GRID)
 
@@ -3537,11 +3552,16 @@ inline void gcode_G28() {
 
       #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
 
-        nonlinear_grid_spacing[X_AXIS] = xGridSpacing;
-        nonlinear_grid_spacing[Y_AXIS] = yGridSpacing;
         float zoffset = zprobe_zoffset;
         if (code_seen('Z')) zoffset += code_value_axis_units(Z_AXIS);
 
+        if (xGridSpacing != nonlinear_grid_spacing[X_AXIS] || yGridSpacing != nonlinear_grid_spacing[Y_AXIS]) {
+          nonlinear_grid_spacing[X_AXIS] = xGridSpacing;
+          nonlinear_grid_spacing[Y_AXIS] = yGridSpacing;
+          // Can't re-enable (on error) until the new grid is written
+          abl_should_reenable = false;
+        }
+
       #elif ENABLED(AUTO_BED_LEVELING_LINEAR_GRID)
 
         /**
@@ -3600,6 +3620,11 @@ inline void gcode_G28() {
 
           measured_z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
 
+          if (measured_z == NAN) {
+            planner.abl_enabled = abl_should_reenable;
+            return;
+          }
+
           #if ENABLED(AUTO_BED_LEVELING_LINEAR_GRID)
 
             mean += measured_z;
@@ -3639,6 +3664,11 @@ inline void gcode_G28() {
         measured_z = points[i].z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
       }
 
+      if (measured_z == NAN) {
+        planner.abl_enabled = abl_should_reenable;
+        return;
+      }
+
       if (!dryrun) {
         vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
         if (planeNormal.z < 0) {
@@ -3647,12 +3677,23 @@ inline void gcode_G28() {
           planeNormal.z *= -1;
         }
         planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+
+        // Can't re-enable (on error) until the new grid is written
+        abl_should_reenable = false;
       }
 
     #endif // AUTO_BED_LEVELING_3POINT
 
     // Raise to _Z_CLEARANCE_DEPLOY_PROBE. Stow the probe.
-    if (STOW_PROBE()) return;
+    if (STOW_PROBE()) {
+      planner.abl_enabled = abl_should_reenable;
+      return;
+    }
+
+    //
+    // Unless this is a dry run, auto bed leveling will
+    // definitely be enabled after this point
+    //
 
     // Restore state after probing
     clean_up_after_endstop_or_probe_move();
@@ -3842,6 +3883,9 @@ inline void gcode_G28() {
     report_current_position();
 
     KEEPALIVE_STATE(IN_HANDLER);
+
+    // Auto Bed Leveling is complete! Enable if possible.
+    planner.abl_enabled = dryrun ? abl_should_reenable : true;
   }
 
 #endif // AUTO_BED_LEVELING_FEATURE
@@ -3925,6 +3969,8 @@ inline void gcode_G92() {
     SYNC_PLAN_POSITION_KINEMATIC();
   else if (didE)
     sync_plan_position_e();
+
+  report_current_position();
 }
 
 #if ENABLED(ULTIPANEL) || ENABLED(EMERGENCY_PARSER)
@@ -4186,7 +4232,11 @@ inline void gcode_M42() {
   if (pin_number < 0) return;
 
   for (uint8_t i = 0; i < COUNT(sensitive_pins); i++)
-    if (pin_number == sensitive_pins[i]) return;
+    if (pin_number == sensitive_pins[i]) {
+      SERIAL_ERROR_START;
+      SERIAL_ERRORLNPGM(MSG_ERR_PROTECTED_PIN);
+      return;
+    }
 
   pinMode(pin_number, OUTPUT);
   digitalWrite(pin_number, pin_status);
@@ -7736,7 +7786,7 @@ void ok_to_send() {
 
   // Get the Z adjustment for non-linear bed leveling
   float nonlinear_z_offset(float cartesian[XYZ]) {
-    if (nonlinear_grid_spacing[X_AXIS] == 0 || nonlinear_grid_spacing[Y_AXIS] == 0) return 0; // G29 not done!
+    if (planner.abl_enabled) return;
 
     int half_x = (ABL_GRID_POINTS_X - 1) / 2,
         half_y = (ABL_GRID_POINTS_Y - 1) / 2;
@@ -7846,15 +7896,19 @@ void ok_to_send() {
       )                                      \
     )
 
+  #define DELTA_RAW_IK() do {   \
+    delta[A_AXIS] = DELTA_Z(1); \
+    delta[B_AXIS] = DELTA_Z(2); \
+    delta[C_AXIS] = DELTA_Z(3); \
+  } while(0)
+
   #define DELTA_LOGICAL_IK() do {      \
     const float raw[XYZ] = {           \
       RAW_X_POSITION(logical[X_AXIS]), \
       RAW_Y_POSITION(logical[Y_AXIS]), \
       RAW_Z_POSITION(logical[Z_AXIS])  \
     };                                 \
-    delta[A_AXIS] = DELTA_Z(1);        \
+    DELTA_RAW_IK();                    \
-    delta[B_AXIS] = DELTA_Z(2);        \
-    delta[C_AXIS] = DELTA_Z(3);        \
   } while(0)
 
   #define DELTA_DEBUG() do { \
@@ -8012,7 +8066,7 @@ void get_cartesian_from_steppers() {
 void set_current_from_steppers_for_axis(const AxisEnum axis) {
   get_cartesian_from_steppers();
   #if PLANNER_LEVELING
-    planner.unapply_leveling(cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS]);
+    planner.unapply_leveling(cartes);
   #endif
   if (axis == ALL_AXES)
     memcpy(current_position, cartes, sizeof(cartes));
@@ -8091,101 +8145,123 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
    * This calls planner.buffer_line several times, adding
    * small incremental moves for DELTA or SCARA.
    */
-  inline bool prepare_kinematic_move_to(float logical[NUM_AXIS]) {
+  inline bool prepare_kinematic_move_to(float ltarget[NUM_AXIS]) {
 
     // Get the top feedrate of the move in the XY plane
     float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
 
-    // If the move is only in Z don't split up the move.
+    // If the move is only in Z/E don't split up the move
-    // This shortcut cannot be used if planar bed leveling
+    if (ltarget[X_AXIS] == current_position[X_AXIS] && ltarget[Y_AXIS] == current_position[Y_AXIS]) {
-    // is in use, but is fine with mesh-based bed leveling
+      inverse_kinematics(ltarget);
-    if (logical[X_AXIS] == current_position[X_AXIS] && logical[Y_AXIS] == current_position[Y_AXIS]) {
+      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], _feedrate_mm_s, active_extruder);
-      inverse_kinematics(logical);
-      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
       return true;
     }
 
-    // Get the distance moved in XYZ
+    // Get the cartesian distances moved in XYZE
     float difference[NUM_AXIS];
-    LOOP_XYZE(i) difference[i] = logical[i] - current_position[i];
+    LOOP_XYZE(i) difference[i] = ltarget[i] - current_position[i];
 
+    // Get the linear distance in XYZ
     float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
+
+    // If the move is very short, check the E move distance
     if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = abs(difference[E_AXIS]);
+
+    // No E move either? Game over.
     if (UNEAR_ZERO(cartesian_mm)) return false;
 
     // Minimum number of seconds to move the given distance
     float seconds = cartesian_mm / _feedrate_mm_s;
 
     // The number of segments-per-second times the duration
-    // gives the number of segments we should produce
+    // gives the number of segments
     uint16_t segments = delta_segments_per_second * seconds;
 
+    // For SCARA minimum segment size is 0.5mm
     #if IS_SCARA
       NOMORE(segments, cartesian_mm * 2);
     #endif
 
+    // At least one segment is required
     NOLESS(segments, 1);
 
-    // Each segment produces this much of the move
+    // The approximate length of each segment
-    float inv_segments = 1.0 / segments,
+    float segment_distance[XYZE] = {
-          segment_distance[XYZE] = {
+            difference[X_AXIS] / segments,
-            difference[X_AXIS] * inv_segments,
+            difference[Y_AXIS] / segments,
-            difference[Y_AXIS] * inv_segments,
+            difference[Z_AXIS] / segments,
-            difference[Z_AXIS] * inv_segments,
+            difference[E_AXIS] / segments
-            difference[E_AXIS] * inv_segments
           };
 
     // SERIAL_ECHOPAIR("mm=", cartesian_mm);
     // SERIAL_ECHOPAIR(" seconds=", seconds);
     // SERIAL_ECHOLNPAIR(" segments=", segments);
 
-    // Send all the segments to the planner
+    // Drop one segment so the last move is to the exact target.
+    // If there's only 1 segment, loops will be skipped entirely.
+    --segments;
 
-    #if ENABLED(DELTA) && ENABLED(USE_RAW_KINEMATICS)
+    // Using "raw" coordinates saves 6 float subtractions
+    // per segment, saving valuable CPU cycles
 
-      #define DELTA_E raw[E_AXIS]
+    #if ENABLED(USE_RAW_KINEMATICS)
-      #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) raw[i] += ADDEND;
-      #define DELTA_IK() do {       \
-        delta[A_AXIS] = DELTA_Z(1); \
-        delta[B_AXIS] = DELTA_Z(2); \
-        delta[C_AXIS] = DELTA_Z(3); \
-      } while(0)
 
       // Get the raw current position as starting point
-      float raw[ABC] = {
+      float raw[XYZE] = {
         RAW_CURRENT_POSITION(X_AXIS),
         RAW_CURRENT_POSITION(Y_AXIS),
-        RAW_CURRENT_POSITION(Z_AXIS)
+        RAW_CURRENT_POSITION(Z_AXIS),
+        current_position[E_AXIS]
       };
 
+      #define DELTA_VAR raw
+
+      // Delta can inline its kinematics
+      #if ENABLED(DELTA)
+        #define DELTA_IK() DELTA_RAW_IK()
+      #else
+        #define DELTA_IK() inverse_kinematics(raw)
+      #endif
+
     #else
 
-      #define DELTA_E logical[E_AXIS]
+      // Get the logical current position as starting point
-      #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) logical[i] += ADDEND;
+      float logical[XYZE];
+      memcpy(logical, current_position, sizeof(logical));
+
+      #define DELTA_VAR logical
 
+      // Delta can inline its kinematics
       #if ENABLED(DELTA)
         #define DELTA_IK() DELTA_LOGICAL_IK()
       #else
         #define DELTA_IK() inverse_kinematics(logical)
       #endif
 
-      // Get the logical current position as starting point
-      LOOP_XYZE(i) logical[i] = current_position[i];
-
     #endif
 
     #if ENABLED(USE_DELTA_IK_INTERPOLATION)
 
-      // Get the starting delta for interpolation
+      // Only interpolate XYZ. Advance E normally.
-      if (segments >= 2) inverse_kinematics(logical);
+      #define DELTA_NEXT(ADDEND) LOOP_XYZ(i) DELTA_VAR[i] += ADDEND;
 
+      // Get the starting delta if interpolation is possible
+      if (segments >= 2) DELTA_IK();
+
+      // Loop using decrement
       for (uint16_t s = segments + 1; --s;) {
-        if (s > 1) {
+        // Are there at least 2 moves left?
+        if (s >= 2) {
           // Save the previous delta for interpolation
           float prev_delta[ABC] = { delta[A_AXIS], delta[B_AXIS], delta[C_AXIS] };
 
           // Get the delta 2 segments ahead (rather than the next)
           DELTA_NEXT(segment_distance[i] + segment_distance[i]);
+
+          // Advance E normally
+          DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
+
+          // Get the exact delta for the move after this
           DELTA_IK();
 
           // Move to the interpolated delta position first
@@ -8193,33 +8269,43 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
             (prev_delta[A_AXIS] + delta[A_AXIS]) * 0.5,
             (prev_delta[B_AXIS] + delta[B_AXIS]) * 0.5,
             (prev_delta[C_AXIS] + delta[C_AXIS]) * 0.5,
-            logical[E_AXIS], _feedrate_mm_s, active_extruder
+            DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder
           );
 
+          // Advance E once more for the next move
+          DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
+
           // Do an extra decrement of the loop
           --s;
         }
         else {
-          // Get the last segment delta (only when segments is odd)
+          // Get the last segment delta. (Used when segments is odd)
-          DELTA_NEXT(segment_distance[i])
+          DELTA_NEXT(segment_distance[i]);
+          DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
           DELTA_IK();
         }
 
         // Move to the non-interpolated position
-        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_E, _feedrate_mm_s, active_extruder);
+        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder);
       }
 
     #else
 
+      #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) DELTA_VAR[i] += ADDEND;
+
       // For non-interpolated delta calculate every segment
       for (uint16_t s = segments + 1; --s;) {
-        DELTA_NEXT(segment_distance[i])
+        DELTA_NEXT(segment_distance[i]);
         DELTA_IK();
-        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
+        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder);
       }
 
     #endif
 
+    // Since segment_distance is only approximate,
+    // the final move must be to the exact destination.
+    inverse_kinematics(ltarget);
+    planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], _feedrate_mm_s, active_extruder);
     return true;
   }
 
@@ -8554,7 +8640,7 @@ void prepare_move_to_destination() {
     cartes[Y_AXIS] = a_sin + b_sin + SCARA_OFFSET_Y;  //theta+phi
 
     /*
-      SERIAL_ECHOPAIR("Angle a=", a);
+      SERIAL_ECHOPAIR("SCARA FK Angle a=", a);
       SERIAL_ECHOPAIR(" b=", b);
       SERIAL_ECHOPAIR(" a_sin=", a_sin);
       SERIAL_ECHOPAIR(" a_cos=", a_cos);

Marlin/SanityCheck.h

@@ -450,6 +450,13 @@
 
 #endif
 
+/**
+ * Homing Bump
+ */
+#if X_HOME_BUMP_MM < 0 || Y_HOME_BUMP_MM < 0 || Z_HOME_BUMP_MM < 0
+  #error "[XYZ]_HOME_BUMP_MM must be greater than or equal to 0."
+#endif
+
 /**
  * Make sure Z_SAFE_HOMING point is reachable
  */

Marlin/language.h

@@ -151,6 +151,7 @@
 #define MSG_ERR_M421_PARAMETERS             "M421 requires XYZ or IJZ parameters"
 #define MSG_ERR_MESH_XY                     "Mesh XY or IJ cannot be resolved"
 #define MSG_ERR_ARC_ARGS                    "G2/G3 bad parameters"
+#define MSG_ERR_PROTECTED_PIN               "Protected Pin"
 #define MSG_ERR_M428_TOO_FAR                "Too far from reference point"
 #define MSG_ERR_M303_DISABLED               "PIDTEMP disabled"
 #define MSG_M119_REPORT                     "Reporting endstop status"

Marlin/planner.cpp

@@ -98,6 +98,10 @@ float Planner::min_feedrate_mm_s,
       Planner::max_e_jerk,
       Planner::min_travel_feedrate_mm_s;
 
+#if ENABLED(AUTO_BED_LEVELING_FEATURE)
+  bool Planner::abl_enabled = false; // Flag that auto bed leveling is enabled
+#endif
+
 #if ENABLED(AUTO_BED_LEVELING_LINEAR)
   matrix_3x3 Planner::bed_level_matrix; // Transform to compensate for bed level
 #endif
@@ -135,8 +139,8 @@ Planner::Planner() { init(); }
 
 void Planner::init() {
   block_buffer_head = block_buffer_tail = 0;
-  memset(position, 0, sizeof(position)); // clear position
+  memset(position, 0, sizeof(position));
-  LOOP_XYZE(i) previous_speed[i] = 0.0;
+  memset(previous_speed, 0, sizeof(previous_speed));
   previous_nominal_speed = 0.0;
   #if ENABLED(AUTO_BED_LEVELING_LINEAR)
     bed_level_matrix.set_to_identity();
@@ -524,6 +528,11 @@ void Planner::check_axes_activity() {
 #if PLANNER_LEVELING
 
   void Planner::apply_leveling(float &lx, float &ly, float &lz) {
+
+    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+      if (!abl_enabled) return;
+    #endif
+
     #if ENABLED(MESH_BED_LEVELING)
 
       if (mbl.active())
@@ -561,28 +570,34 @@ void Planner::check_axes_activity() {
     #endif
   }
 
-  void Planner::unapply_leveling(float &lx, float &ly, float &lz) {
+  void Planner::unapply_leveling(float logical[XYZ]) {
+
+    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+      if (!abl_enabled) return;
+    #endif
+
     #if ENABLED(MESH_BED_LEVELING)
 
       if (mbl.active())
-        lz -= mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
+        logical[Z_AXIS] -= mbl.get_z(RAW_X_POSITION(logical[X_AXIS]), RAW_Y_POSITION(logical[Y_AXIS]));
 
     #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
 
       matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
 
-      float dx = lx - (X_TILT_FULCRUM), dy = ly - (Y_TILT_FULCRUM), dz = lz;
+      float dx = RAW_X_POSITION(logical[X_AXIS]) - (X_TILT_FULCRUM),
+            dy = RAW_Y_POSITION(logical[Y_AXIS]) - (Y_TILT_FULCRUM),
+            dz = RAW_Z_POSITION(logical[Z_AXIS]);
 
       apply_rotation_xyz(inverse, dx, dy, dz);
 
-      lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
+      logical[X_AXIS] = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
-      ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
+      logical[Y_AXIS] = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
-      lz = LOGICAL_Z_POSITION(dz);
+      logical[Z_AXIS] = LOGICAL_Z_POSITION(dz);
 
     #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
 
-      float tmp[XYZ] = { lx, ly, 0 };
+      logical[Z_AXIS] -= nonlinear_z_offset(logical);
-      lz -= nonlinear_z_offset(tmp);
 
     #endif
   }
@@ -625,30 +640,29 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
        dz = target[Z_AXIS] - position[Z_AXIS];
 
   /*
-  SERIAL_ECHO_START;
+  SERIAL_ECHOPAIR("  Planner FR:", fr_mm_s);
-  SERIAL_ECHOPGM("Planner ", x);
+  SERIAL_CHAR(' ');
   #if IS_KINEMATIC
-    SERIAL_ECHOPAIR("A:", x);
+    SERIAL_ECHOPAIR("A:", lx);
     SERIAL_ECHOPAIR(" (", dx);
-    SERIAL_ECHOPAIR(") B:", y);
+    SERIAL_ECHOPAIR(") B:", ly);
   #else
-    SERIAL_ECHOPAIR("X:", x);
+    SERIAL_ECHOPAIR("X:", lx);
     SERIAL_ECHOPAIR(" (", dx);
-    SERIAL_ECHOPAIR(") Y:", y);
+    SERIAL_ECHOPAIR(") Y:", ly);
   #endif
   SERIAL_ECHOPAIR(" (", dy);
-  #elif ENABLED(DELTA)
+  #if ENABLED(DELTA)
-    SERIAL_ECHOPAIR(") C:", z);
+    SERIAL_ECHOPAIR(") C:", lz);
   #else
-    SERIAL_ECHOPAIR(") Z:", z);
+    SERIAL_ECHOPAIR(") Z:", lz);
   #endif
   SERIAL_ECHOPAIR(" (", dz);
   SERIAL_ECHOLNPGM(")");
   //*/
 
   // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
-  if (DEBUGGING(DRYRUN))
+  if (DEBUGGING(DRYRUN)) position[E_AXIS] = target[E_AXIS];
-    position[E_AXIS] = target[E_AXIS];
 
   long de = target[E_AXIS] - position[E_AXIS];
 
@@ -1131,7 +1145,7 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
   block->recalculate_flag = true; // Always calculate trapezoid for new block
 
   // Update previous path unit_vector and nominal speed
-  LOOP_XYZE(i) previous_speed[i] = current_speed[i];
+  memcpy(previous_speed, current_speed, sizeof(previous_speed));
   previous_nominal_speed = block->nominal_speed;
 
   #if ENABLED(LIN_ADVANCE)
@@ -1177,8 +1191,8 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
   // Move buffer head
   block_buffer_head = next_buffer_head;
 
-  // Update position
+  // Update the position (only when a move was queued)
-  LOOP_XYZE(i) position[i] = target[i];
+  memcpy(position, target, sizeof(position));
 
   recalculate();
 
@@ -1204,7 +1218,14 @@ void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
   stepper.set_position(nx, ny, nz, ne);
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
 
-  LOOP_XYZE(i) previous_speed[i] = 0.0;
+  memset(previous_speed, 0, sizeof(previous_speed));
+}
+
+/**
+ * Sync from the stepper positions. (e.g., after an interrupted move)
+ */
+void Planner::sync_from_steppers() {
+  LOOP_XYZE(i) position[i] = stepper.position((AxisEnum)i);
 }
 
 /**

Marlin/planner.h

@@ -137,6 +137,7 @@ class Planner {
     static float min_travel_feedrate_mm_s;
 
     #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+      static bool abl_enabled;            // Flag that bed leveling is enabled
       static matrix_3x3 bed_level_matrix; // Transform to compensate for bed level
     #endif
 
@@ -218,7 +219,7 @@ class Planner {
        * as it will be given to the planner and steppers.
        */
       static void apply_leveling(float &lx, float &ly, float &lz);
-      static void unapply_leveling(float &lx, float &ly, float &lz);
+      static void unapply_leveling(float logical[XYZ]);
 
     #endif
 
@@ -242,6 +243,11 @@ class Planner {
      */
     static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float& e);
 
+    /**
+     * Sync from the stepper positions. (e.g., after an interrupted move)
+     */
+    static void sync_from_steppers();
+
     /**
      * Set the E position (mm) of the planner (and the E stepper)
      */

Marlin/stepper.cpp

@@ -357,11 +357,10 @@ void Stepper::isr() {
     }
     else {
       OCR1A = 2000; // 1kHz.
+      return;
     }
   }
 
-  if (current_block) {
-
   // Update endstops state, if enabled
   if (endstops.enabled
     #if HAS_BED_PROBE
@@ -668,7 +667,6 @@ void Stepper::isr() {
     planner.discard_current_block();
   }
 }
-}
 
 #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
 
@@ -936,6 +934,9 @@ void Stepper::synchronize() { while (planner.blocks_queued()) idle(); }
  * derive the current XYZ position later on.
  */
 void Stepper::set_position(const long& x, const long& y, const long& z, const long& e) {
+
+  synchronize(); // Bad to set stepper counts in the middle of a move
+
   CRITICAL_SECTION_START;
 
   #if ENABLED(COREXY)