From 87c4cd20e514b9fc94450e9503e268cc014490e8 Mon Sep 17 00:00:00 2001 From: tombrazier <68918209+tombrazier@users.noreply.github.com> Date: Sat, 26 Feb 2022 20:30:33 +0000 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20steps-to-mm=20with=20backl?= =?UTF-8?q?ash=20(#23814)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Scott Lahteine --- Marlin/src/feature/backlash.cpp | 31 ++++++++++++++++++++++++++----- Marlin/src/feature/backlash.h | 9 ++++++++- Marlin/src/module/planner.cpp | 27 ++++++++++++++++++++------- 3 files changed, 54 insertions(+), 13 deletions(-) diff --git a/Marlin/src/feature/backlash.cpp b/Marlin/src/feature/backlash.cpp index 24c0f2ca0c15..23458cf86cb9 100644 --- a/Marlin/src/feature/backlash.cpp +++ b/Marlin/src/feature/backlash.cpp @@ -29,6 +29,11 @@ #include "../module/motion.h" #include "../module/planner.h" +axis_bits_t Backlash::last_direction_bits; +#ifdef BACKLASH_SMOOTHING_MM + xyz_long_t Backlash::residual_error{0}; +#endif + #ifdef BACKLASH_DISTANCE_MM #if ENABLED(BACKLASH_GCODE) xyz_float_t Backlash::distance_mm = BACKLASH_DISTANCE_MM; @@ -61,7 +66,6 @@ Backlash backlash; */ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const axis_bits_t dm, block_t * const block) { - static axis_bits_t last_direction_bits; axis_bits_t changed_dir = last_direction_bits ^ dm; // Ignore direction change unless steps are taken in that direction #if DISABLED(CORE_BACKLASH) || EITHER(MARKFORGED_XY, MARKFORGED_YX) @@ -91,10 +95,6 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const // smoothing distance. Since the computation of this proportion involves a floating point // division, defer computation until needed. float segment_proportion = 0; - - // Residual error carried forward across multiple segments, so correction can be applied - // to segments where there is no direction change. - static xyz_long_t residual_error{0}; #else // No direction change, no correction. if (!changed_dir) return; @@ -153,6 +153,27 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const } } +int32_t Backlash::applied_steps(const AxisEnum axis) { + if (axis >= LINEAR_AXES) return 0; + + const bool reversing = TEST(last_direction_bits, axis); + + #ifdef BACKLASH_SMOOTHING_MM + const int32_t residual_error_axis = residual_error[axis]; + #else + constexpr int32_t residual_error_axis = 0; + #endif + + // At startup it is assumed the last move was forwards. So the applied + // steps will always be a non-positive number. + + if (!reversing) return -residual_error_axis; + + const float f_corr = float(correction) / 255.0f; + const int32_t full_error_axis = -f_corr * distance_mm[axis] * planner.settings.axis_steps_per_mm[axis]; + return full_error_axis - residual_error_axis; +} + #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING) #include "../module/probe.h" diff --git a/Marlin/src/feature/backlash.h b/Marlin/src/feature/backlash.h index 17504cc78181..ef29012cb146 100644 --- a/Marlin/src/feature/backlash.h +++ b/Marlin/src/feature/backlash.h @@ -27,6 +27,12 @@ constexpr uint8_t all_on = 0xFF, all_off = 0x00; class Backlash { +private: + static axis_bits_t last_direction_bits; + #ifdef BACKLASH_SMOOTHING_MM + static xyz_long_t residual_error; + #endif + public: #if ENABLED(BACKLASH_GCODE) static xyz_float_t distance_mm; @@ -71,7 +77,8 @@ class Backlash { return has_measurement(X_AXIS) || has_measurement(Y_AXIS) || has_measurement(Z_AXIS); } - void add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const axis_bits_t dm, block_t * const block); + static void add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const axis_bits_t dm, block_t * const block); + static int32_t applied_steps(const AxisEnum axis); }; extern Backlash backlash; diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index d2e11f62fefe..37b264a3c55e 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1706,7 +1706,8 @@ void Planner::endstop_triggered(const AxisEnum axis) { } float Planner::triggered_position_mm(const AxisEnum axis) { - return stepper.triggered_position(axis) * mm_per_step[axis]; + const float result = DIFF_TERN(BACKLASH_COMPENSATION, stepper.triggered_position(axis), backlash.applied_steps(axis)); + return result * mm_per_step[axis]; } void Planner::finish_and_disable() { @@ -1728,8 +1729,8 @@ float Planner::get_axis_position_mm(const AxisEnum axis) { // Protect the access to the position. const bool was_enabled = stepper.suspend(); - const int32_t p1 = stepper.position(CORE_AXIS_1), - p2 = stepper.position(CORE_AXIS_2); + const int32_t p1 = DIFF_TERN(BACKLASH_COMPENSATION, stepper.position(CORE_AXIS_1), backlash.applied_steps(CORE_AXIS_1)), + p2 = DIFF_TERN(BACKLASH_COMPENSATION, stepper.position(CORE_AXIS_2), backlash.applied_steps(CORE_AXIS_2)); if (was_enabled) stepper.wake_up(); @@ -1738,7 +1739,7 @@ float Planner::get_axis_position_mm(const AxisEnum axis) { axis_steps = (axis == CORE_AXIS_2 ? CORESIGN(p1 - p2) : p1 + p2) * 0.5f; } else - axis_steps = stepper.position(axis); + axis_steps = DIFF_TERN(BACKLASH_COMPENSATION, stepper.position(axis), backlash.applied_steps(axis)); #elif EITHER(MARKFORGED_XY, MARKFORGED_YX) @@ -1755,11 +1756,12 @@ float Planner::get_axis_position_mm(const AxisEnum axis) { axis_steps = ((axis == CORE_AXIS_1) ? p1 - p2 : p2); } else - axis_steps = stepper.position(axis); + axis_steps = DIFF_TERN(BACKLASH_COMPENSATION, stepper.position(axis), backlash.applied_steps(axis)); #else axis_steps = stepper.position(axis); + TERN_(BACKLASH_COMPENSATION, axis_steps -= backlash.applied_steps(axis)); #endif @@ -2841,6 +2843,9 @@ void Planner::buffer_sync_block(TERN_(LASER_SYNCHRONOUS_M106_M107, uint8_t sync_ block->flag = sync_flag; block->position = position; + #if ENABLED(BACKLASH_COMPENSATION) + LOOP_LINEAR_AXES(axis) block->position[axis] += backlash.applied_steps((AxisEnum)axis); + #endif #if BOTH(HAS_FAN, LASER_SYNCHRONOUS_M106_M107) FANS_LOOP(i) block->fan_speed[i] = thermalManager.fan_speed[i]; @@ -3108,13 +3113,21 @@ void Planner::set_machine_position_mm(const abce_pos_t &abce) { LROUND(abce.k * settings.axis_steps_per_mm[K_AXIS]) ) ); + if (has_blocks_queued()) { //previous_nominal_speed_sqr = 0.0; // Reset planner junction speeds. Assume start from rest. //previous_speed.reset(); buffer_sync_block(); } - else - stepper.set_position(position); + else { + #if ENABLED(BACKLASH_COMPENSATION) + abce_long_t stepper_pos = position; + LOOP_LINEAR_AXES(axis) stepper_pos[axis] += backlash.applied_steps((AxisEnum)axis); + stepper.set_position(stepper_pos); + #else + stepper.set_position(position); + #endif + } } void Planner::set_position_mm(const xyze_pos_t &xyze) {