Merge pull request #4982 from thinkyhead/rc_abl_bugfix

Fix planner with kinematics, delta ABL

Marlin/Conditionals_post.h

@@ -706,4 +706,8 @@
   // Stepper pulse duration, in cycles
   #define STEP_PULSE_CYCLES ((MINIMUM_STEPPER_PULSE) * CYCLES_PER_MICROSECOND)
 
+  #ifndef DELTA_ENDSTOP_ADJ
+    #define DELTA_ENDSTOP_ADJ { 0 }
+  #endif
+
 #endif // CONDITIONALS_POST_H

Marlin/Marlin_main.cpp

@@ -456,6 +456,18 @@ static uint8_t target_extruder;
   #define XY_PROBE_FEEDRATE_MM_S PLANNER_XY_FEEDRATE()
 #endif
 
+#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+  #define ADJUST_DELTA(V) \
+    if (planner.abl_enabled) { \
+      const float zadj = bilinear_z_offset(V); \
+      delta[A_AXIS] += zadj; \
+      delta[B_AXIS] += zadj; \
+      delta[C_AXIS] += zadj; \
+    }
+#elif IS_KINEMATIC
+  #define ADJUST_DELTA(V) NOOP
+#endif
+
 #if ENABLED(Z_DUAL_ENDSTOPS)
   float z_endstop_adj = 0;
 #endif
@@ -711,8 +723,7 @@ inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position);
     #endif
-    inverse_kinematics(current_position);
-    planner.set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS]);
+    planner.set_position_mm_kinematic(current_position);
   }
   #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_kinematic()
 
@@ -1541,8 +1552,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
     ) 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);
+    planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
     set_current_to_destination();
   }
 #endif // IS_KINEMATIC
@@ -6778,8 +6788,7 @@ inline void gcode_M503() {
 
     // Define runplan for move axes
     #if IS_KINEMATIC
-      #define RUNPLAN(RATE_MM_S) inverse_kinematics(destination); \
-                                 planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder);
+      #define RUNPLAN(RATE_MM_S) planner.buffer_line_kinematic(destination, RATE_MM_S, active_extruder);
     #else
       #define RUNPLAN(RATE_MM_S) line_to_destination(RATE_MM_S);
     #endif
@@ -6894,17 +6903,14 @@ inline void gcode_M503() {
     KEEPALIVE_STATE(IN_HANDLER);
 
     // Set extruder to saved position
-    current_position[E_AXIS] = lastpos[E_AXIS];
-    destination[E_AXIS] = lastpos[E_AXIS];
+    destination[E_AXIS] = current_position[E_AXIS] = lastpos[E_AXIS];
     planner.set_e_position_mm(current_position[E_AXIS]);
 
     #if IS_KINEMATIC
-      // Move XYZ to starting position, then E
-      inverse_kinematics(lastpos);
-      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
-      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], lastpos[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
+      // Move XYZ to starting position
+      planner.buffer_line_kinematic(lastpos, FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
     #else
-      // Move XY to starting position, then Z, then E
+      // Move XY to starting position, then Z
       destination[X_AXIS] = lastpos[X_AXIS];
       destination[Y_AXIS] = lastpos[Y_AXIS];
       RUNPLAN(FILAMENT_CHANGE_XY_FEEDRATE);
@@ -7295,15 +7301,11 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
             float z_diff = hotend_offset[Z_AXIS][active_extruder] - hotend_offset[Z_AXIS][tmp_extruder],
                   z_raise = 0.3 + (z_diff > 0.0 ? z_diff : 0.0);
 
+            set_destination_to_current();
+
             // Always raise by some amount
-            planner.buffer_line(
-              current_position[X_AXIS],
-              current_position[Y_AXIS],
-              current_position[Z_AXIS] + z_raise,
-              current_position[E_AXIS],
-              planner.max_feedrate_mm_s[Z_AXIS],
-              active_extruder
-            );
+            destination[Z_AXIS] += z_raise;
+            planner.buffer_line_kinematic(destination, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
             stepper.synchronize();
 
             move_extruder_servo(active_extruder);
@@ -7311,14 +7313,8 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
 
             // Move back down, if needed
             if (z_raise != z_diff) {
-              planner.buffer_line(
-                current_position[X_AXIS],
-                current_position[Y_AXIS],
-                current_position[Z_AXIS] + z_diff,
-                current_position[E_AXIS],
-                planner.max_feedrate_mm_s[Z_AXIS],
-                active_extruder
-              );
+              destination[Z_AXIS] = current_position[Z_AXIS] + z_diff;
+              planner.buffer_line_kinematic(destination, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
               stepper.synchronize();
             }
           #endif
@@ -8670,8 +8666,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
 
     // If the move is only in Z/E don't split up the move
     if (ltarget[X_AXIS] == current_position[X_AXIS] && ltarget[Y_AXIS] == current_position[Y_AXIS]) {
-      inverse_kinematics(ltarget);
-      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], _feedrate_mm_s, active_extruder);
+      planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
       return true;
     }
 
@@ -8764,7 +8759,10 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
       #define DELTA_NEXT(ADDEND) LOOP_XYZ(i) DELTA_VAR[i] += ADDEND;
 
       // Get the starting delta if interpolation is possible
-      if (segments >= 2) DELTA_IK();
+      if (segments >= 2) {
+        DELTA_IK();
+        ADJUST_DELTA(DELTA_VAR); // Adjust Z if bed leveling is enabled
+      }
 
       // Loop using decrement
       for (uint16_t s = segments + 1; --s;) {
@@ -8781,6 +8779,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
 
           // Get the exact delta for the move after this
           DELTA_IK();
+          ADJUST_DELTA(DELTA_VAR); // Adjust Z if bed leveling is enabled
 
           // Move to the interpolated delta position first
           planner.buffer_line(
@@ -8801,6 +8800,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
           DELTA_NEXT(segment_distance[i]);
           DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
           DELTA_IK();
+          ADJUST_DELTA(DELTA_VAR); // Adjust Z if bed leveling is enabled
         }
 
         // Move to the non-interpolated position
@@ -8815,6 +8815,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
       for (uint16_t s = segments + 1; --s;) {
         DELTA_NEXT(segment_distance[i]);
         DELTA_IK();
+        ADJUST_DELTA(DELTA_VAR);
         planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder);
       }
 
@@ -8822,8 +8823,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
 
     // 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);
+    planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
     return true;
   }
 
@@ -9063,21 +9063,11 @@ void prepare_move_to_destination() {
 
       clamp_to_software_endstops(arc_target);
 
-      #if IS_KINEMATIC
-        inverse_kinematics(arc_target);
-        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder);
-      #else
-        planner.buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder);
-      #endif
+      planner.buffer_line_kinematic(arc_target, fr_mm_s, active_extruder);
     }
 
     // Ensure last segment arrives at target location.
-    #if IS_KINEMATIC
-      inverse_kinematics(logical);
-      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], fr_mm_s, active_extruder);
-    #else
-      planner.buffer_line(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS], logical[E_AXIS], fr_mm_s, active_extruder);
-    #endif
+    planner.buffer_line_kinematic(logical, fr_mm_s, active_extruder);
 
     // As far as the parser is concerned, the position is now == target. In reality the
     // motion control system might still be processing the action and the real tool position
@@ -9472,11 +9462,22 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
       #endif // !SWITCHING_EXTRUDER
 
       previous_cmd_ms = ms; // refresh_cmd_timeout()
-      planner.buffer_line(
-        current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],
-        current_position[E_AXIS] + EXTRUDER_RUNOUT_EXTRUDE,
-        MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder
-      );
+
+      #if IS_KINEMATIC
+        inverse_kinematics(current_position);
+        ADJUST_DELTA(current_position);
+        planner.buffer_line(
+          delta[A_AXIS], delta[B_AXIS], delta[C_AXIS],
+          current_position[E_AXIS] + EXTRUDER_RUNOUT_EXTRUDE,
+          MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder
+        );
+      #else
+        planner.buffer_line(
+          current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],
+          current_position[E_AXIS] + EXTRUDER_RUNOUT_EXTRUDE,
+          MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder
+        );
+      #endif
       stepper.synchronize();
       planner.set_e_position_mm(current_position[E_AXIS]);
       #if ENABLED(SWITCHING_EXTRUDER)

Marlin/SanityCheck.h

@@ -518,6 +518,10 @@
  */
 #if HAS_ABL
 
+  #if ENABLED(USE_RAW_KINEMATICS)
+    #error "USE_RAW_KINEMATICS is not compatible with AUTO_BED_LEVELING"
+  #endif
+
   /**
    * Delta and SCARA have limited bed leveling options
    */

Marlin/configuration_store.cpp

@@ -589,7 +589,10 @@ void Config_ResetDefault() {
   #endif
 
   #if ENABLED(DELTA)
-    endstop_adj[X_AXIS] = endstop_adj[Y_AXIS] = endstop_adj[Z_AXIS] = 0;
+    const float adj[ABC] = DELTA_ENDSTOP_ADJ;
+    endstop_adj[A_AXIS] = adj[A_AXIS];
+    endstop_adj[B_AXIS] = adj[B_AXIS];
+    endstop_adj[C_AXIS] = adj[C_AXIS];
     delta_radius =  DELTA_RADIUS;
     delta_diagonal_rod =  DELTA_DIAGONAL_ROD;
     delta_segments_per_second =  DELTA_SEGMENTS_PER_SECOND;

Marlin/example_configurations/delta/biv2.5/Configuration.h

@@ -441,6 +441,8 @@
   // in ultralcd.cpp@lcd_delta_calibrate_menu()
   //#define DELTA_CALIBRATION_MENU
 
+  //#define DELTA_ENDSTOP_ADJ { 0, 0, 0 }
+
 #endif
 
 // Enable this option for Toshiba steppers

Marlin/example_configurations/delta/generic/Configuration.h

@@ -441,6 +441,8 @@
   // in ultralcd.cpp@lcd_delta_calibrate_menu()
   //#define DELTA_CALIBRATION_MENU
 
+  //#define DELTA_ENDSTOP_ADJ { 0, 0, 0 }
+
 #endif
 
 // Enable this option for Toshiba steppers

Marlin/example_configurations/delta/kossel_mini/Configuration.h

@@ -441,6 +441,8 @@
   // in ultralcd.cpp@lcd_delta_calibrate_menu()
   //#define DELTA_CALIBRATION_MENU
 
+  //#define DELTA_ENDSTOP_ADJ { 0, 0, 0 }
+
 #endif
 
 // Enable this option for Toshiba steppers

Marlin/example_configurations/delta/kossel_pro/Configuration.h

@@ -430,6 +430,8 @@
   // in ultralcd.cpp@lcd_delta_calibrate_menu()
   //#define DELTA_CALIBRATION_MENU
 
+  //#define DELTA_ENDSTOP_ADJ { 0, 0, 0 }
+
 #endif
 
 // Enable this option for Toshiba steppers

Marlin/example_configurations/delta/kossel_xl/Configuration.h

@@ -439,6 +439,8 @@
   // in ultralcd.cpp@lcd_delta_calibrate_menu()
   //#define DELTA_CALIBRATION_MENU
 
+  //#define DELTA_ENDSTOP_ADJ { 0, 0, 0 }
+
 #endif
 
 // Enable this option for Toshiba steppers

Marlin/planner.cpp

@@ -522,7 +522,9 @@ void Planner::check_axes_activity() {
 }
 
 #if PLANNER_LEVELING
-
+  /**
+   * lx, ly, lz - logical (cartesian, not delta) positions in mm
+   */
   void Planner::apply_leveling(float &lx, float &ly, float &lz) {
 
     #if HAS_ABL
@@ -549,19 +551,7 @@ void Planner::check_axes_activity() {
     #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
 
       float tmp[XYZ] = { lx, ly, 0 };
-
-      #if ENABLED(DELTA)
-
-        float offset = bilinear_z_offset(tmp);
-        lx += offset;
-        ly += offset;
-        lz += offset;
-
-      #else
-
-        lz += bilinear_z_offset(tmp);
-
-      #endif
+      lz += bilinear_z_offset(tmp);
 
     #endif
   }
@@ -601,15 +591,17 @@ void Planner::check_axes_activity() {
 #endif // PLANNER_LEVELING
 
 /**
- * Planner::buffer_line
+ * Planner::_buffer_line
  *
  * Add a new linear movement to the buffer.
  *
- *  x,y,z,e   - target position in mm
- *  fr_mm_s   - (target) speed of the move
- *  extruder  - target extruder
+ * Leveling and kinematics should be applied ahead of calling this.
+ *
+ *  a,b,c,e     - target positions in mm or degrees
+ *  fr_mm_s     - (target) speed of the move
+ *  extruder    - target extruder
  */
-void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, const uint8_t extruder) {
+void Planner::_buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder) {
   // Calculate the buffer head after we push this byte
   int next_buffer_head = next_block_index(block_buffer_head);
 
@@ -617,43 +609,39 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
   // Rest here until there is room in the buffer.
   while (block_buffer_tail == next_buffer_head) idle();
 
-  #if PLANNER_LEVELING
-    apply_leveling(lx, ly, lz);
-  #endif
-
   // The target position of the tool in absolute steps
   // Calculate target position in absolute steps
   //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
-  long target[NUM_AXIS] = {
-    lround(lx * axis_steps_per_mm[X_AXIS]),
-    lround(ly * axis_steps_per_mm[Y_AXIS]),
-    lround(lz * axis_steps_per_mm[Z_AXIS]),
+  long target[XYZE] = {
+    lround(a * axis_steps_per_mm[X_AXIS]),
+    lround(b * axis_steps_per_mm[Y_AXIS]),
+    lround(c * axis_steps_per_mm[Z_AXIS]),
     lround(e * axis_steps_per_mm[E_AXIS])
   };
 
-  long dx = target[X_AXIS] - position[X_AXIS],
-       dy = target[Y_AXIS] - position[Y_AXIS],
-       dz = target[Z_AXIS] - position[Z_AXIS];
+  long da = target[X_AXIS] - position[X_AXIS],
+       db = target[Y_AXIS] - position[Y_AXIS],
+       dc = target[Z_AXIS] - position[Z_AXIS];
 
   /*
   SERIAL_ECHOPAIR("  Planner FR:", fr_mm_s);
   SERIAL_CHAR(' ');
   #if IS_KINEMATIC
-    SERIAL_ECHOPAIR("A:", lx);
-    SERIAL_ECHOPAIR(" (", dx);
-    SERIAL_ECHOPAIR(") B:", ly);
+    SERIAL_ECHOPAIR("A:", a);
+    SERIAL_ECHOPAIR(" (", da);
+    SERIAL_ECHOPAIR(") B:", b);
   #else
-    SERIAL_ECHOPAIR("X:", lx);
-    SERIAL_ECHOPAIR(" (", dx);
-    SERIAL_ECHOPAIR(") Y:", ly);
+    SERIAL_ECHOPAIR("X:", a);
+    SERIAL_ECHOPAIR(" (", da);
+    SERIAL_ECHOPAIR(") Y:", b);
   #endif
-  SERIAL_ECHOPAIR(" (", dy);
+  SERIAL_ECHOPAIR(" (", db);
   #if ENABLED(DELTA)
-    SERIAL_ECHOPAIR(") C:", lz);
+    SERIAL_ECHOPAIR(") C:", c);
   #else
-    SERIAL_ECHOPAIR(") Z:", lz);
+    SERIAL_ECHOPAIR(") Z:", c);
   #endif
-  SERIAL_ECHOPAIR(" (", dz);
+  SERIAL_ECHOPAIR(" (", dc);
   SERIAL_CHAR(')');
   SERIAL_EOL;
   //*/
@@ -692,24 +680,24 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
   #if ENABLED(COREXY)
     // corexy planning
     // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
-    block->steps[A_AXIS] = labs(dx + dy);
-    block->steps[B_AXIS] = labs(dx - dy);
-    block->steps[Z_AXIS] = labs(dz);
+    block->steps[A_AXIS] = labs(da + db);
+    block->steps[B_AXIS] = labs(da - db);
+    block->steps[Z_AXIS] = labs(dc);
   #elif ENABLED(COREXZ)
     // corexz planning
-    block->steps[A_AXIS] = labs(dx + dz);
-    block->steps[Y_AXIS] = labs(dy);
-    block->steps[C_AXIS] = labs(dx - dz);
+    block->steps[A_AXIS] = labs(da + dc);
+    block->steps[Y_AXIS] = labs(db);
+    block->steps[C_AXIS] = labs(da - dc);
   #elif ENABLED(COREYZ)
     // coreyz planning
-    block->steps[X_AXIS] = labs(dx);
-    block->steps[B_AXIS] = labs(dy + dz);
-    block->steps[C_AXIS] = labs(dy - dz);
+    block->steps[X_AXIS] = labs(da);
+    block->steps[B_AXIS] = labs(db + dc);
+    block->steps[C_AXIS] = labs(db - dc);
   #else
     // default non-h-bot planning
-    block->steps[X_AXIS] = labs(dx);
-    block->steps[Y_AXIS] = labs(dy);
-    block->steps[Z_AXIS] = labs(dz);
+    block->steps[X_AXIS] = labs(da);
+    block->steps[Y_AXIS] = labs(db);
+    block->steps[Z_AXIS] = labs(dc);
   #endif
 
   block->steps[E_AXIS] = labs(de) * volumetric_multiplier[extruder] * flow_percentage[extruder] * 0.01 + 0.5;
@@ -733,33 +721,33 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
     block->e_to_p_pressure = baricuda_e_to_p_pressure;
   #endif
 
-  // Compute direction bits for this block
-  uint8_t db = 0;
+  // Compute direction bit-mask for this block
+  uint8_t dm = 0;
   #if ENABLED(COREXY)
-    if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis
-    if (dy < 0) SBI(db, Y_HEAD); // ...and Y
-    if (dz < 0) SBI(db, Z_AXIS);
-    if (dx + dy < 0) SBI(db, A_AXIS); // Motor A direction
-    if (dx - dy < 0) SBI(db, B_AXIS); // Motor B direction
+    if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
+    if (db < 0) SBI(dm, Y_HEAD); // ...and Y
+    if (dc < 0) SBI(dm, Z_AXIS);
+    if (da + db < 0) SBI(dm, A_AXIS); // Motor A direction
+    if (da - db < 0) SBI(dm, B_AXIS); // Motor B direction
   #elif ENABLED(COREXZ)
-    if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis
-    if (dy < 0) SBI(db, Y_AXIS);
-    if (dz < 0) SBI(db, Z_HEAD); // ...and Z
-    if (dx + dz < 0) SBI(db, A_AXIS); // Motor A direction
-    if (dx - dz < 0) SBI(db, C_AXIS); // Motor C direction
+    if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
+    if (db < 0) SBI(dm, Y_AXIS);
+    if (dc < 0) SBI(dm, Z_HEAD); // ...and Z
+    if (da + dc < 0) SBI(dm, A_AXIS); // Motor A direction
+    if (da - dc < 0) SBI(dm, C_AXIS); // Motor C direction
   #elif ENABLED(COREYZ)
-    if (dx < 0) SBI(db, X_AXIS);
-    if (dy < 0) SBI(db, Y_HEAD); // Save the real Extruder (head) direction in Y Axis
-    if (dz < 0) SBI(db, Z_HEAD); // ...and Z
-    if (dy + dz < 0) SBI(db, B_AXIS); // Motor B direction
-    if (dy - dz < 0) SBI(db, C_AXIS); // Motor C direction
+    if (da < 0) SBI(dm, X_AXIS);
+    if (db < 0) SBI(dm, Y_HEAD); // Save the real Extruder (head) direction in Y Axis
+    if (dc < 0) SBI(dm, Z_HEAD); // ...and Z
+    if (db + dc < 0) SBI(dm, B_AXIS); // Motor B direction
+    if (db - dc < 0) SBI(dm, C_AXIS); // Motor C direction
   #else
-    if (dx < 0) SBI(db, X_AXIS);
-    if (dy < 0) SBI(db, Y_AXIS);
-    if (dz < 0) SBI(db, Z_AXIS);
+    if (da < 0) SBI(dm, X_AXIS);
+    if (db < 0) SBI(dm, Y_AXIS);
+    if (dc < 0) SBI(dm, Z_AXIS);
   #endif
-  if (de < 0) SBI(db, E_AXIS);
-  block->direction_bits = db;
+  if (de < 0) SBI(dm, E_AXIS);
+  block->direction_bits = dm;
 
   block->active_extruder = extruder;
 
@@ -872,29 +860,29 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
   #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
     float delta_mm[7];
     #if ENABLED(COREXY)
-      delta_mm[X_HEAD] = dx * steps_to_mm[A_AXIS];
-      delta_mm[Y_HEAD] = dy * steps_to_mm[B_AXIS];
-      delta_mm[Z_AXIS] = dz * steps_to_mm[Z_AXIS];
-      delta_mm[A_AXIS] = (dx + dy) * steps_to_mm[A_AXIS];
-      delta_mm[B_AXIS] = (dx - dy) * steps_to_mm[B_AXIS];
+      delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
+      delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
+      delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
+      delta_mm[A_AXIS] = (da + db) * steps_to_mm[A_AXIS];
+      delta_mm[B_AXIS] = (da - db) * steps_to_mm[B_AXIS];
     #elif ENABLED(COREXZ)
-      delta_mm[X_HEAD] = dx * steps_to_mm[A_AXIS];
-      delta_mm[Y_AXIS] = dy * steps_to_mm[Y_AXIS];
-      delta_mm[Z_HEAD] = dz * steps_to_mm[C_AXIS];
-      delta_mm[A_AXIS] = (dx + dz) * steps_to_mm[A_AXIS];
-      delta_mm[C_AXIS] = (dx - dz) * steps_to_mm[C_AXIS];
+      delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
+      delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
+      delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
+      delta_mm[A_AXIS] = (da + dc) * steps_to_mm[A_AXIS];
+      delta_mm[C_AXIS] = (da - dc) * steps_to_mm[C_AXIS];
     #elif ENABLED(COREYZ)
-      delta_mm[X_AXIS] = dx * steps_to_mm[X_AXIS];
-      delta_mm[Y_HEAD] = dy * steps_to_mm[B_AXIS];
-      delta_mm[Z_HEAD] = dz * steps_to_mm[C_AXIS];
-      delta_mm[B_AXIS] = (dy + dz) * steps_to_mm[B_AXIS];
-      delta_mm[C_AXIS] = (dy - dz) * steps_to_mm[C_AXIS];
+      delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
+      delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
+      delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
+      delta_mm[B_AXIS] = (db + dc) * steps_to_mm[B_AXIS];
+      delta_mm[C_AXIS] = (db - dc) * steps_to_mm[C_AXIS];
     #endif
   #else
     float delta_mm[4];
-    delta_mm[X_AXIS] = dx * steps_to_mm[X_AXIS];
-    delta_mm[Y_AXIS] = dy * steps_to_mm[Y_AXIS];
-    delta_mm[Z_AXIS] = dz * steps_to_mm[Z_AXIS];
+    delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
+    delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
+    delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
   #endif
   delta_mm[E_AXIS] = 0.01 * (de * steps_to_mm[E_AXIS]) * volumetric_multiplier[extruder] * flow_percentage[extruder];
 
@@ -1196,22 +1184,34 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
  *
  * On CORE machines stepper ABC will be translated from the given XYZ.
  */
-void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
 
-  #if PLANNER_LEVELING
-    apply_leveling(lx, ly, lz);
-  #endif
-
-  long nx = position[X_AXIS] = lround(lx * axis_steps_per_mm[X_AXIS]),
-       ny = position[Y_AXIS] = lround(ly * axis_steps_per_mm[Y_AXIS]),
-       nz = position[Z_AXIS] = lround(lz * axis_steps_per_mm[Z_AXIS]),
+void Planner::_set_position_mm(const float &a, const float &b, const float &c, const float &e) {
+  long na = position[X_AXIS] = lround(a * axis_steps_per_mm[X_AXIS]),
+       nb = position[Y_AXIS] = lround(b * axis_steps_per_mm[Y_AXIS]),
+       nc = position[Z_AXIS] = lround(c * axis_steps_per_mm[Z_AXIS]),
        ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
-  stepper.set_position(nx, ny, nz, ne);
+  stepper.set_position(na, nb, nc, ne);
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
 
   memset(previous_speed, 0, sizeof(previous_speed));
 }
 
+void Planner::set_position_mm_kinematic(const float position[NUM_AXIS]) {
+  #if PLANNER_LEVELING
+    float pos[XYZ] = { position[X_AXIS], position[Y_AXIS], position[Z_AXIS] };
+    apply_leveling(pos);
+  #else
+    const float * const pos = position;
+  #endif
+  #if IS_KINEMATIC
+    inverse_kinematics(pos);
+    _set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], position[E_AXIS]);
+  #else
+    _set_position_mm(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], position[E_AXIS]);
+  #endif
+}
+
+
 /**
  * Sync from the stepper positions. (e.g., after an interrupted move)
  */
@@ -1237,12 +1237,7 @@ void Planner::reset_acceleration_rates() {
 // Recalculate position, steps_to_mm if axis_steps_per_mm changes!
 void Planner::refresh_positioning() {
   LOOP_XYZE(i) steps_to_mm[i] = 1.0 / axis_steps_per_mm[i];
-  #if IS_KINEMATIC
-    inverse_kinematics(current_position);
-    set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS]);
-  #else
-    set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-  #endif
+  set_position_mm_kinematic(current_position);
   reset_acceleration_rates();
 }
 

Marlin/planner.h

@@ -43,6 +43,12 @@
 class Planner;
 extern Planner planner;
 
+#if IS_KINEMATIC
+  // for inline buffer_line_kinematic
+  extern float delta[ABC];
+  void inverse_kinematics(const float logical[XYZ]);
+#endif
+
 /**
  * struct block_t
  *
@@ -218,18 +224,68 @@ class Planner {
        * as it will be given to the planner and steppers.
        */
       static void apply_leveling(float &lx, float &ly, float &lz);
+      static void apply_leveling(float logical[XYZ]) { apply_leveling(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS]); }
       static void unapply_leveling(float logical[XYZ]);
 
     #endif
 
     /**
-     * Add a new linear movement to the buffer.
+     * Planner::_buffer_line
      *
-     *  x,y,z,e   - target position in mm
+     * Add a new direct linear movement to the buffer.
+     *
+     * Leveling and kinematics should be applied ahead of this.
+     *
+     *  a,b,c,e   - target position in mm or degrees
      *  fr_mm_s   - (target) speed of the move (mm/s)
      *  extruder  - target extruder
      */
-    static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float& e, float fr_mm_s, const uint8_t extruder);
+    static void _buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder);
+
+    static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
+
+    /**
+     * Add a new linear movement to the buffer.
+     * The target is NOT translated to delta/scara
+     *
+     * Leveling will be applied to input on cartesians.
+     * Kinematic machines should call buffer_line_kinematic (for leveled moves).
+     * (Cartesians may also call buffer_line_kinematic.)
+     *
+     *  lx,ly,lz,e   - target position in mm or degrees
+     *  fr_mm_s      - (target) speed of the move (mm/s)
+     *  extruder     - target extruder
+     */
+    static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, const uint8_t extruder) {
+      #if PLANNER_LEVELING && IS_CARTESIAN
+        apply_leveling(lx, ly, lz);
+      #endif
+      _buffer_line(lx, ly, lz, e, fr_mm_s, extruder);
+    }
+
+    /**
+     * Add a new linear movement to the buffer.
+     * The target is cartesian, it's translated to delta/scara if
+     * needed.
+     *
+     *  target   - x,y,z,e CARTESIAN target in mm
+     *  fr_mm_s  - (target) speed of the move (mm/s)
+     *  extruder - target extruder
+     */
+    static FORCE_INLINE void buffer_line_kinematic(const float target[XYZE], float fr_mm_s, const uint8_t extruder) {
+      #if PLANNER_LEVELING
+        float pos[XYZ] = { target[X_AXIS], target[Y_AXIS], target[Z_AXIS] };
+        apply_leveling(pos);
+      #else
+        const float * const pos = target;
+      #endif
+      #if IS_KINEMATIC
+        inverse_kinematics(pos);
+        _buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], target[E_AXIS], fr_mm_s, extruder);
+      #else
+        _buffer_line(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], target[E_AXIS], fr_mm_s, extruder);
+      #endif
+    }
 
     /**
      * Set the planner.position and individual stepper positions.
@@ -240,9 +296,14 @@ class Planner {
      *
      * Clears previous speed values.
      */
-    static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float& e);
+    static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
+      #if PLANNER_LEVELING && IS_CARTESIAN
+        apply_leveling(lx, ly, lz);
+      #endif
+      _set_position_mm(lx, ly, lz, e);
+    }
+    static void set_position_mm_kinematic(const float position[NUM_AXIS]);
     static void set_position_mm(const AxisEnum axis, const float& v);
-
     static FORCE_INLINE void set_z_position_mm(const float& z) { set_position_mm(Z_AXIS, z); }
     static FORCE_INLINE void set_e_position_mm(const float& e) { set_position_mm(E_AXIS, e); }
 

Marlin/planner_bezier.cpp

@@ -187,13 +187,7 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
     bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
     bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
     clamp_to_software_endstops(bez_target);
-
-    #if IS_KINEMATIC
-      inverse_kinematics(bez_target);
-      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], bez_target[E_AXIS], fr_mm_s, extruder);
-    #else
-      planner.buffer_line(bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS], bez_target[E_AXIS], fr_mm_s, extruder);
-    #endif
+    planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder);
   }
 }
 

Marlin/stepper.cpp

@@ -966,7 +966,7 @@ void Stepper::synchronize() { while (planner.blocks_queued()) idle(); }
  * This allows get_axis_position_mm to correctly
  * derive the current XYZ position later on.
  */
-void Stepper::set_position(const long& x, const long& y, const long& z, const long& e) {
+void Stepper::set_position(const long &a, const long &b, const long &c, const long &e) {
 
   synchronize(); // Bad to set stepper counts in the middle of a move
 
@@ -975,37 +975,37 @@ void Stepper::set_position(const long& x, const long& y, const long& z, const lo
   #if ENABLED(COREXY)
     // corexy positioning
     // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
-    count_position[A_AXIS] = x + y;
-    count_position[B_AXIS] = x - y;
-    count_position[Z_AXIS] = z;
+    count_position[A_AXIS] = a + b;
+    count_position[B_AXIS] = a - b;
+    count_position[Z_AXIS] = c;
   #elif ENABLED(COREXZ)
     // corexz planning
-    count_position[A_AXIS] = x + z;
-    count_position[Y_AXIS] = y;
-    count_position[C_AXIS] = x - z;
+    count_position[A_AXIS] = a + c;
+    count_position[Y_AXIS] = b;
+    count_position[C_AXIS] = a - c;
   #elif ENABLED(COREYZ)
     // coreyz planning
-    count_position[X_AXIS] = x;
-    count_position[B_AXIS] = y + z;
-    count_position[C_AXIS] = y - z;
+    count_position[X_AXIS] = a;
+    count_position[B_AXIS] = y + c;
+    count_position[C_AXIS] = y - c;
   #else
     // default non-h-bot planning
-    count_position[X_AXIS] = x;
-    count_position[Y_AXIS] = y;
-    count_position[Z_AXIS] = z;
+    count_position[X_AXIS] = a;
+    count_position[Y_AXIS] = b;
+    count_position[Z_AXIS] = c;
   #endif
 
   count_position[E_AXIS] = e;
   CRITICAL_SECTION_END;
 }
 
-void Stepper::set_position(const AxisEnum &axis, const long& v) {
+void Stepper::set_position(const AxisEnum &axis, const long &v) {
   CRITICAL_SECTION_START;
   count_position[axis] = v;
   CRITICAL_SECTION_END;
 }
 
-void Stepper::set_e_position(const long& e) {
+void Stepper::set_e_position(const long &e) {
   CRITICAL_SECTION_START;
   count_position[E_AXIS] = e;
   CRITICAL_SECTION_END;

Marlin/stepper.h

@@ -188,9 +188,9 @@ class Stepper {
     //
     // Set the current position in steps
     //
-    static void set_position(const long& x, const long& y, const long& z, const long& e);
-    static void set_position(const AxisEnum& a, const long& v);
-    static void set_e_position(const long& e);
+    static void set_position(const long &a, const long &b, const long &c, const long &e);
+    static void set_position(const AxisEnum &a, const long &v);
+    static void set_e_position(const long &e);
 
     //
     // Set direction bits for all steppers

Marlin/ultralcd.cpp

@@ -561,12 +561,7 @@ void kill_screen(const char* lcd_msg) {
 #if ENABLED(ULTIPANEL)
 
   inline void line_to_current(AxisEnum axis) {
-    #if ENABLED(DELTA)
-      inverse_kinematics(current_position);
-      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder);
-    #else // !DELTA
-      planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder);
-    #endif // !DELTA
+    planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder);
   }
 
   #if ENABLED(SDSUPPORT)
@@ -1351,12 +1346,7 @@ void kill_screen(const char* lcd_msg) {
    */
   inline void manage_manual_move() {
     if (manual_move_axis != (int8_t)NO_AXIS && ELAPSED(millis(), manual_move_start_time) && !planner.is_full()) {
-      #if ENABLED(DELTA)
-        inverse_kinematics(current_position);
-        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index);
-      #else
-        planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index);
-      #endif
+      planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index);
       manual_move_axis = (int8_t)NO_AXIS;
     }
   }