From cac742009c1b44f3789e6bd8c400d9ee5763521b Mon Sep 17 00:00:00 2001
From: tombrazier <68918209+tombrazier@users.noreply.github.com>
Date: Sat, 4 Nov 2023 04:12:33 +0000
Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20Backlash=20Compensation=20?=
=?UTF-8?q?layer=20shift=20(#26392)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Marlin/src/feature/backlash.cpp | 54 ++++++++++++++++++++++-----------
Marlin/src/feature/backlash.h | 2 +-
Marlin/src/module/planner.cpp | 2 +-
3 files changed, 39 insertions(+), 19 deletions(-)
diff --git a/Marlin/src/feature/backlash.cpp b/Marlin/src/feature/backlash.cpp
index c6eb0d33f30..07fa7725a06 100644
--- a/Marlin/src/feature/backlash.cpp
+++ b/Marlin/src/feature/backlash.cpp
@@ -63,25 +63,25 @@ Backlash backlash;
* spread over multiple segments, smoothing out artifacts even more.
*/
-void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const AxisBits dm, block_t * const block) {
+void Backlash::add_correction_steps(const xyze_long_t &dist, const AxisBits dm, block_t * const block) {
AxisBits changed_dir = last_direction_bits ^ dm;
// Ignore direction change unless steps are taken in that direction
#if DISABLED(CORE_BACKLASH) || ANY(MARKFORGED_XY, MARKFORGED_YX)
- if (!da) changed_dir.x = false;
- if (!db) changed_dir.y = false;
- if (!dc) changed_dir.z = false;
+ if (!dist.a) changed_dir.x = false;
+ if (!dist.b) changed_dir.y = false;
+ if (!dist.c) changed_dir.z = false;
#elif CORE_IS_XY
- if (!(da + db)) changed_dir.x = false;
- if (!(da - db)) changed_dir.y = false;
- if (!dc) changed_dir.z = false;
+ if (!(dist.a + dist.b)) changed_dir.x = false;
+ if (!(dist.a - dist.b)) changed_dir.y = false;
+ if (!dist.c) changed_dir.z = false;
#elif CORE_IS_XZ
- if (!(da + dc)) changed_dir.x = false;
- if (!(da - dc)) changed_dir.z = false;
- if (!db) changed_dir.y = false;
+ if (!(dist.a + dist.c)) changed_dir.x = false;
+ if (!(dist.a - dist.c)) changed_dir.z = false;
+ if (!dist.b) changed_dir.y = false;
#elif CORE_IS_YZ
- if (!(db + dc)) changed_dir.y = false;
- if (!(db - dc)) changed_dir.z = false;
- if (!da) changed_dir.x = false;
+ if (!(dist.b + dist.c)) changed_dir.y = false;
+ if (!(dist.b - dist.c)) changed_dir.z = false;
+ if (!dist.a) changed_dir.x = false;
#endif
last_direction_bits ^= changed_dir;
@@ -97,7 +97,15 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
const float f_corr = float(correction) / all_on;
+ bool changed = false;
+ float millimeters_delta = 0.0f;
+ #if IS_KINEMATIC
+ float sqr_stepper_space_mm = 0.0f;
+ #endif
+
LOOP_NUM_AXES(axis) {
+ TERN_(IS_KINEMATIC, sqr_stepper_space_mm += sq(dist[axis] * planner.mm_per_step[axis]));
+
if (distance_mm[axis]) {
const bool forward = dm[axis];
@@ -107,8 +115,6 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
// Decide how much of the residual error to correct in this segment
int32_t error_correction = residual_error[axis];
- if (forward == (error_correction < 0))
- error_correction = 0; // Don't take up any backlash in this segment, as it would subtract steps
#ifdef BACKLASH_SMOOTHING_MM
if (error_correction && smoothing_mm != 0) {
@@ -118,9 +124,18 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
}
#endif
+ // Don't correct backlash in the opposite direction to movement on this axis and for accuracy in
+ // updating block->millimeters, don't add too many steps to the movement on this axis
+ if (forward)
+ LIMIT(error_correction, 0, dist[axis]);
+ else
+ LIMIT(error_correction, dist[axis], 0);
+
// This correction reduces the residual error and adds block steps
if (error_correction) {
+ changed = true;
block->steps[axis] += ABS(error_correction);
+ millimeters_delta += dist[axis] * error_correction * sq(planner.mm_per_step[axis]);
#if ENABLED(CORE_BACKLASH)
switch (axis) {
case CORE_AXIS_1:
@@ -142,6 +157,11 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
}
}
}
+
+ // If backlash correction steps were added modify block->millimeters with a linear approximation
+ // See https://github.com/MarlinFirmware/Marlin/pull/26392
+ if (changed)
+ block->millimeters += TERN(IS_KINEMATIC, millimeters_delta * block->millimeters / sqr_stepper_space_mm, millimeters_delta / block->millimeters);
}
int32_t Backlash::get_applied_steps(const AxisEnum axis) {
@@ -151,8 +171,8 @@ int32_t Backlash::get_applied_steps(const AxisEnum axis) {
const int32_t residual_error_axis = residual_error[axis];
- // At startup it is assumed the last move was forwards. So the applied
- // steps will always be a non-positive number.
+ // At startup it is assumed the last move was forward.
+ // So the applied steps will always be negative.
if (forward) return -residual_error_axis;
diff --git a/Marlin/src/feature/backlash.h b/Marlin/src/feature/backlash.h
index 14c0fe20e37..593e51b9d0f 100644
--- a/Marlin/src/feature/backlash.h
+++ b/Marlin/src/feature/backlash.h
@@ -72,7 +72,7 @@ public:
return has_measurement(X_AXIS) || has_measurement(Y_AXIS) || has_measurement(Z_AXIS);
}
- static void add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const AxisBits dm, block_t * const block);
+ static void add_correction_steps(const xyze_long_t &dist, const AxisBits dm, block_t * const block);
static int32_t get_applied_steps(const AxisEnum axis);
#if ENABLED(BACKLASH_GCODE)
diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp
index 1414a314457..8e9021b0306 100644
--- a/Marlin/src/module/planner.cpp
+++ b/Marlin/src/module/planner.cpp
@@ -2175,7 +2175,7 @@ bool Planner::_populate_block(
* A correction function is permitted to add steps to an axis, it
* should *never* remove steps!
*/
- TERN_(BACKLASH_COMPENSATION, backlash.add_correction_steps(dist.a, dist.b, dist.c, dm, block));
+ TERN_(BACKLASH_COMPENSATION, backlash.add_correction_steps(dist, dm, block));
}
TERN_(HAS_EXTRUDERS, block->steps.e = esteps);