Backlash Compensation for COREnn (#21612)

Co-authored-by: Scott Lahteine <github@thinkyhead.com>

Marlin/Configuration_adv.h

@@ -972,6 +972,9 @@
   #define BACKLASH_DISTANCE_MM { 0, 0, 0 } // (mm)
   #define BACKLASH_CORRECTION    0.0       // 0.0 = no correction; 1.0 = full correction
 
+  // Add steps for motor direction changes on CORE kinematics
+  //#define CORE_BACKLASH
+
   // Set BACKLASH_SMOOTHING_MM to spread backlash correction over multiple segments
   // to reduce print artifacts. (Enabling this is costly in memory and computation!)
   //#define BACKLASH_SMOOTHING_MM 3 // (mm)

Marlin/src/feature/backlash.cpp

@@ -63,10 +63,24 @@ Backlash backlash;
 void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const uint8_t dm, block_t * const block) {
   static uint8_t last_direction_bits;
   uint8_t changed_dir = last_direction_bits ^ dm;
-  // Ignore direction change if no steps are taken in that direction
+  // Ignore direction change unless steps are taken in that direction
-  if (da == 0) CBI(changed_dir, X_AXIS);
+  #if DISABLED(CORE_BACKLASH) || ENABLED(MARKFORGED_XY)
-  if (db == 0) CBI(changed_dir, Y_AXIS);
+    if (!da) CBI(changed_dir, X_AXIS);
-  if (dc == 0) CBI(changed_dir, Z_AXIS);
+    if (!db) CBI(changed_dir, Y_AXIS);
+    if (!dc) CBI(changed_dir, Z_AXIS);
+  #elif CORE_IS_XY
+    if (!(da + db)) CBI(changed_dir, X_AXIS);
+    if (!(da - db)) CBI(changed_dir, Y_AXIS);
+    if (!dc)        CBI(changed_dir, Z_AXIS);
+  #elif CORE_IS_XZ
+    if (!(da + dc)) CBI(changed_dir, X_AXIS);
+    if (!(da - dc)) CBI(changed_dir, Z_AXIS);
+    if (!db)        CBI(changed_dir, Y_AXIS);
+  #elif CORE_IS_YZ
+    if (!(db + dc)) CBI(changed_dir, Y_AXIS);
+    if (!(db - dc)) CBI(changed_dir, Z_AXIS);
+    if (!da)        CBI(changed_dir, X_AXIS);
+  #endif
   last_direction_bits ^= changed_dir;
 
   if (correction == 0) return;
@@ -105,18 +119,35 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
           // Take up a portion of the residual_error in this segment, but only when
           // the current segment travels in the same direction as the correction
           if (reversing == (error_correction < 0)) {
-            if (segment_proportion == 0)
+            if (segment_proportion == 0) segment_proportion = _MIN(1.0f, block->millimeters / smoothing_mm);
-              segment_proportion = _MIN(1.0f, block->millimeters / smoothing_mm);
             error_correction = CEIL(segment_proportion * error_correction);
           }
           else
             error_correction = 0; // Don't take up any backlash in this segment, as it would subtract steps
         }
       #endif
-      // Making a correction reduces the residual error and adds block steps
+
+      // This correction reduces the residual error and adds block steps
       if (error_correction) {
         block->steps[axis] += ABS(error_correction);
+        #if ENABLED(CORE_BACKLASH)
+          switch (axis) {
+            case CORE_AXIS_1:
+              //block->steps[CORE_AXIS_2] += influence_distance_mm[axis] * planner.settings.axis_steps_per_mm[CORE_AXIS_2];
+              //SERIAL_ECHOLNPAIR("CORE_AXIS_1 dir change. distance=", distance_mm[axis], " r.err=", residual_error[axis],
+              //  " da=", da, " db=", db, " block->steps[axis]=", block->steps[axis], " err_corr=", error_correction);
+              break;
+            case CORE_AXIS_2:
+              //block->steps[CORE_AXIS_1] += influence_distance_mm[axis] * planner.settings.axis_steps_per_mm[CORE_AXIS_1];;
+              //SERIAL_ECHOLNPAIR("CORE_AXIS_2 dir change. distance=", distance_mm[axis], " r.err=", residual_error[axis],
+              //  " da=", da, " db=", db, " block->steps[axis]=", block->steps[axis], " err_corr=", error_correction);
+              break;
+            case NORMAL_AXIS: break;
+          }
+          residual_error[axis] = 0; // No residual_error needed for next CORE block, I think...
+        #else
           residual_error[axis] -= error_correction;
+        #endif
       }
     }
   }

Marlin/src/lcd/menu/menu_backlash.cpp

@@ -38,10 +38,15 @@ void menu_backlash() {
 
   EDIT_ITEM_FAST(percent, MSG_BACKLASH_CORRECTION, &backlash.correction, all_off, all_on);
 
+  #if DISABLED(CORE_BACKLASH) || ENABLED(MARKFORGED_XY)
+    #define _CAN_CALI AXIS_CAN_CALIBRATE
+  #else
+    #define _CAN_CALI(A) true
+  #endif
   #define EDIT_BACKLASH_DISTANCE(N) EDIT_ITEM_FAST(float43, MSG_BACKLASH_##N, &backlash.distance_mm[_AXIS(N)], 0.0f, 9.9f);
-  if (AXIS_CAN_CALIBRATE(A)) EDIT_BACKLASH_DISTANCE(A);
+  if (_CAN_CALI(A)) EDIT_BACKLASH_DISTANCE(A);
-  if (AXIS_CAN_CALIBRATE(B)) EDIT_BACKLASH_DISTANCE(B);
+  if (_CAN_CALI(B)) EDIT_BACKLASH_DISTANCE(B);
-  if (AXIS_CAN_CALIBRATE(C)) EDIT_BACKLASH_DISTANCE(C);
+  if (_CAN_CALI(C)) EDIT_BACKLASH_DISTANCE(C);
 
   #ifdef BACKLASH_SMOOTHING_MM
     EDIT_ITEM_FAST(float43, MSG_BACKLASH_SMOOTHING, &backlash.smoothing_mm, 0.0f, 9.9f);

buildroot/tests/teensy35

@@ -93,8 +93,8 @@ exec_test $1 $2 "Teensy 3.5/3.6 COREXY" "$3"
 #
 restore_configs
 opt_set MOTHERBOARD BOARD_TEENSY35_36
-opt_enable COREXZ
+opt_enable COREXZ BACKLASH_COMPENSATION BACKLASH_GCODE CORE_BACKLASH
-exec_test $1 $2 "Teensy 3.5/3.6 COREXZ" "$3"
+exec_test $1 $2 "Teensy 3.5/3.6 COREXZ | BACKLASH" "$3"
 
 #
 # Enable Dual Z with Dual Z endstops