From 12ca73947be14cada4bc2b37bd24fc39d36c2ae9 Mon Sep 17 00:00:00 2001 From: Nikolay March Date: Sat, 24 Apr 2021 12:38:00 +0300 Subject: [PATCH] Backlash Compensation for COREnn (#21612) Co-authored-by: Scott Lahteine --- Marlin/Configuration_adv.h | 3 ++ Marlin/src/feature/backlash.cpp | 47 ++++++++++++++++++++++----- Marlin/src/lcd/menu/menu_backlash.cpp | 11 +++++-- buildroot/tests/teensy35 | 4 +-- 4 files changed, 52 insertions(+), 13 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index d72feab43..eb3c7b0df 100644 --- a/Marlin/Configuration_adv.h +++ b/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) diff --git a/Marlin/src/feature/backlash.cpp b/Marlin/src/feature/backlash.cpp index b848214f0..610cfcb56 100644 --- a/Marlin/src/feature/backlash.cpp +++ b/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 - if (da == 0) CBI(changed_dir, X_AXIS); - if (db == 0) CBI(changed_dir, Y_AXIS); - if (dc == 0) CBI(changed_dir, Z_AXIS); + // Ignore direction change unless steps are taken in that direction + #if DISABLED(CORE_BACKLASH) || ENABLED(MARKFORGED_XY) + if (!da) CBI(changed_dir, X_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) - segment_proportion = _MIN(1.0f, block->millimeters / smoothing_mm); + if (segment_proportion == 0) 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); - residual_error[axis] -= 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 } } } diff --git a/Marlin/src/lcd/menu/menu_backlash.cpp b/Marlin/src/lcd/menu/menu_backlash.cpp index 9d0b970ae..c1dca025b 100644 --- a/Marlin/src/lcd/menu/menu_backlash.cpp +++ b/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 (AXIS_CAN_CALIBRATE(B)) EDIT_BACKLASH_DISTANCE(B); - if (AXIS_CAN_CALIBRATE(C)) EDIT_BACKLASH_DISTANCE(C); + if (_CAN_CALI(A)) EDIT_BACKLASH_DISTANCE(A); + if (_CAN_CALI(B)) EDIT_BACKLASH_DISTANCE(B); + 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); diff --git a/buildroot/tests/teensy35 b/buildroot/tests/teensy35 index 36a261a6a..3352c0552 100755 --- a/buildroot/tests/teensy35 +++ b/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 -exec_test $1 $2 "Teensy 3.5/3.6 COREXZ" "$3" +opt_enable COREXZ BACKLASH_COMPENSATION BACKLASH_GCODE CORE_BACKLASH +exec_test $1 $2 "Teensy 3.5/3.6 COREXZ | BACKLASH" "$3" # # Enable Dual Z with Dual Z endstops