From b0fdbede9c98e8b807dd4a7a291b1b668c1a8e15 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 <thinkyhead@users.noreply.github.com>
---
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 24c0f2ca0c1..23458cf86cb 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 17504cc7818..ef29012cb14 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 @@ public:
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 d2e11f62fef..37b264a3c55 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) {