Skip to content

Commit

Permalink
🐛 Fix backlash applied steps when config changes (MarlinFirmware#23826)
Browse files Browse the repository at this point in the history 
Followup to MarlinFirmware#23814
  • Loading branch information
@tombrazier
tombrazier authored Mar 1, 2022
1 parent 15de14d commit 6b7868d
Show file tree
Hide file tree
Showing 8 changed files with 167 additions and 117 deletions.
76 changes: 46 additions & 30 deletions Marlin/src/feature/backlash.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,7 @@
#include "../module/planner.h"

axis_bits_t Backlash::last_direction_bits;
#ifdef BACKLASH_SMOOTHING_MM
xyz_long_t Backlash::residual_error{0};
#endif
xyz_long_t Backlash::residual_error{0};

#ifdef BACKLASH_DISTANCE_MM
#if ENABLED(BACKLASH_GCODE)
Expand All @@ -43,7 +41,7 @@ axis_bits_t Backlash::last_direction_bits;
#endif

#if ENABLED(BACKLASH_GCODE)
uint8_t Backlash::correction = (BACKLASH_CORRECTION) * 0xFF;
uint8_t Backlash::correction = (BACKLASH_CORRECTION) * all_on;
#ifdef BACKLASH_SMOOTHING_MM
float Backlash::smoothing_mm = BACKLASH_SMOOTHING_MM;
#endif
Expand Down Expand Up @@ -87,43 +85,36 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
#endif
last_direction_bits ^= changed_dir;

if (correction == 0) return;
if (!correction && !residual_error) return;

#ifdef BACKLASH_SMOOTHING_MM
// The segment proportion is a value greater than 0.0 indicating how much residual_error
// is corrected for in this segment. The contribution is based on segment length and the
// smoothing distance. Since the computation of this proportion involves a floating point
// division, defer computation until needed.
float segment_proportion = 0;
#else
// No direction change, no correction.
if (!changed_dir) return;
// No leftover residual error from segment to segment
xyz_long_t residual_error{0};
#endif

const float f_corr = float(correction) / 255.0f;
const float f_corr = float(correction) / all_on;

LOOP_LINEAR_AXES(axis) {
if (distance_mm[axis]) {
const bool reversing = TEST(dm,axis);
const bool reverse = TEST(dm, axis);

// When an axis changes direction, add axis backlash to the residual error
if (TEST(changed_dir, axis))
residual_error[axis] += (reversing ? -f_corr : f_corr) * distance_mm[axis] * planner.settings.axis_steps_per_mm[axis];
residual_error[axis] += (reverse ? -f_corr : f_corr) * distance_mm[axis] * planner.settings.axis_steps_per_mm[axis];

// Decide how much of the residual error to correct in this segment
int32_t error_correction = residual_error[axis];
if (reverse != (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) {
// 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);
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
// Take up a portion of the residual_error in this segment
if (segment_proportion == 0) segment_proportion = _MIN(1.0f, block->millimeters / smoothing_mm);
error_correction = CEIL(segment_proportion * error_correction);
}
#endif

Expand Down Expand Up @@ -153,27 +144,52 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
}
}

int32_t Backlash::applied_steps(const AxisEnum axis) {
int32_t Backlash::get_applied_steps(const AxisEnum axis) {
if (axis >= LINEAR_AXES) return 0;

const bool reversing = TEST(last_direction_bits, axis);
const bool reverse = 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
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.

if (!reversing) return -residual_error_axis;
if (!reverse) return -residual_error_axis;

const float f_corr = float(correction) / 255.0f;
const float f_corr = float(correction) / all_on;
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;
}

class Backlash::StepAdjuster {
xyz_long_t applied_steps;
public:
StepAdjuster() {
LOOP_LINEAR_AXES(axis) applied_steps[axis] = backlash.get_applied_steps((AxisEnum)axis);
}
~StepAdjuster() {
// after backlash compensation parameter changes, ensure applied step count does not change
LOOP_LINEAR_AXES(axis) residual_error[axis] += backlash.get_applied_steps((AxisEnum)axis) - applied_steps[axis];
}
};

void Backlash::set_correction_uint8(const uint8_t v) {
StepAdjuster adjuster;
correction = v;
}

void Backlash::set_distance_mm(const AxisEnum axis, const float v) {
StepAdjuster adjuster;
distance_mm[axis] = v;
}

#ifdef BACKLASH_SMOOTHING_MM
void Backlash::set_smoothing_mm(const float v) {
StepAdjuster adjuster;
smoothing_mm = v;
}
#endif

#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)

#include "../module/probe.h"
Expand Down
46 changes: 29 additions & 17 deletions Marlin/src/feature/backlash.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,41 +24,36 @@
#include "../inc/MarlinConfigPre.h"
#include "../module/planner.h"

constexpr uint8_t all_on = 0xFF, all_off = 0x00;

class Backlash {
public:
static constexpr uint8_t all_on = 0xFF, all_off = 0x00;

private:
static axis_bits_t last_direction_bits;
#ifdef BACKLASH_SMOOTHING_MM
static xyz_long_t residual_error;
#endif
static xyz_long_t residual_error;

public:
#if ENABLED(BACKLASH_GCODE)
static xyz_float_t distance_mm;
static uint8_t correction;
static xyz_float_t distance_mm;
#ifdef BACKLASH_SMOOTHING_MM
static float smoothing_mm;
#endif

static void set_correction(const_float_t v) { correction = _MAX(0, _MIN(1.0, v)) * all_on; }
static float get_correction() { return float(ui8_to_percent(correction)) / 100.0f; }
#else
static constexpr uint8_t correction = (BACKLASH_CORRECTION) * 0xFF;
static constexpr uint8_t correction = (BACKLASH_CORRECTION) * all_on;
static const xyz_float_t distance_mm;
#ifdef BACKLASH_SMOOTHING_MM
static constexpr float smoothing_mm = BACKLASH_SMOOTHING_MM;
#endif
#endif

#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
private:
static xyz_float_t measured_mm;
static xyz_uint8_t measured_count;
public:
static void measure_with_probe();
static xyz_float_t measured_mm;
static xyz_uint8_t measured_count;
#endif

class StepAdjuster;

public:
static float get_measurement(const AxisEnum a) {
UNUSED(a);
// Return the measurement averaged over all readings
Expand All @@ -78,7 +73,24 @@ class Backlash {
}

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);
static int32_t get_applied_steps(const AxisEnum axis);

#if ENABLED(BACKLASH_GCODE)
static void set_correction_uint8(const uint8_t v);
static uint8_t get_correction_uint8() { return correction; }
static void set_correction(const float v) { set_correction_uint8(_MAX(0, _MIN(1.0, v)) * all_on + 0.5f); }
static float get_correction() { return float(get_correction_uint8()) / all_on; }
static void set_distance_mm(const AxisEnum axis, const float v);
static float get_distance_mm(const AxisEnum axis) {return distance_mm[axis];}
#ifdef BACKLASH_SMOOTHING_MM
static void set_smoothing_mm(const float v);
static float get_smoothing_mm() {return smoothing_mm;}
#endif
#endif

#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
static void measure_with_probe();
#endif
};

extern Backlash backlash;
60 changes: 37 additions & 23 deletions Marlin/src/gcode/calibrate/G425.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,13 +105,27 @@ struct measurements_t {
};

#if ENABLED(BACKLASH_GCODE)
#define TEMPORARY_BACKLASH_CORRECTION(value) REMEMBER(tbst, backlash.correction, value)
class restorer_correction {
const uint8_t val_;
public:
restorer_correction(const uint8_t temp_val) : val_(backlash.get_correction_uint8()) { backlash.set_correction_uint8(temp_val); }
~restorer_correction() { backlash.set_correction_uint8(val_); }
};

#define TEMPORARY_BACKLASH_CORRECTION(value) restorer_correction restorer_tbst(value)
#else
#define TEMPORARY_BACKLASH_CORRECTION(value)
#endif

#if ENABLED(BACKLASH_GCODE) && defined(BACKLASH_SMOOTHING_MM)
#define TEMPORARY_BACKLASH_SMOOTHING(value) REMEMBER(tbsm, backlash.smoothing_mm, value)
class restorer_smoothing {
const float val_;
public:
restorer_smoothing(const float temp_val) : val_(backlash.get_smoothing_mm()) { backlash.set_smoothing_mm(temp_val); }
~restorer_smoothing() { backlash.set_smoothing_mm(val_); }
};

#define TEMPORARY_BACKLASH_SMOOTHING(value) restorer_smoothing restorer_tbsm(value)
#else
#define TEMPORARY_BACKLASH_SMOOTHING(value)
#endif
Expand Down Expand Up @@ -524,53 +538,53 @@ inline void calibrate_backlash(measurements_t &m, const float uncertainty) {

{
// New scope for TEMPORARY_BACKLASH_CORRECTION
TEMPORARY_BACKLASH_CORRECTION(all_off);
TEMPORARY_BACKLASH_CORRECTION(backlash.all_off);
TEMPORARY_BACKLASH_SMOOTHING(0.0f);

probe_sides(m, uncertainty);

#if ENABLED(BACKLASH_GCODE)

#if HAS_X_CENTER
backlash.distance_mm.x = (m.backlash[LEFT] + m.backlash[RIGHT]) / 2;
backlash.set_distance_mm(X_AXIS, (m.backlash[LEFT] + m.backlash[RIGHT]) / 2);
#elif ENABLED(CALIBRATION_MEASURE_LEFT)
backlash.distance_mm.x = m.backlash[LEFT];
backlash.set_distance_mm(X_AXIS, m.backlash[LEFT]);
#elif ENABLED(CALIBRATION_MEASURE_RIGHT)
backlash.distance_mm.x = m.backlash[RIGHT];
backlash.set_distance_mm(X_AXIS, m.backlash[RIGHT]);
#endif

#if HAS_Y_CENTER
backlash.distance_mm.y = (m.backlash[FRONT] + m.backlash[BACK]) / 2;
backlash.set_distance_mm(Y_AXIS, (m.backlash[FRONT] + m.backlash[BACK]) / 2);
#elif ENABLED(CALIBRATION_MEASURE_FRONT)
backlash.distance_mm.y = m.backlash[FRONT];
backlash.set_distance_mm(Y_AXIS, m.backlash[FRONT]);
#elif ENABLED(CALIBRATION_MEASURE_BACK)
backlash.distance_mm.y = m.backlash[BACK];
backlash.set_distance_mm(Y_AXIS, m.backlash[BACK]);
#endif

TERN_(HAS_Z_AXIS, if (AXIS_CAN_CALIBRATE(Z)) backlash.distance_mm.z = m.backlash[TOP]);
TERN_(HAS_Z_AXIS, if (AXIS_CAN_CALIBRATE(Z)) backlash.set_distance_mm(Z_AXIS, m.backlash[TOP]));

#if HAS_I_CENTER
backlash.distance_mm.i = (m.backlash[IMINIMUM] + m.backlash[IMAXIMUM]) / 2;
backlash.set_distance_mm(I_AXIS, (m.backlash[IMINIMUM] + m.backlash[IMAXIMUM]) / 2);
#elif ENABLED(CALIBRATION_MEASURE_IMIN)
backlash.distance_mm.i = m.backlash[IMINIMUM];
backlash.set_distance_mm(I_AXIS, m.backlash[IMINIMUM]);
#elif ENABLED(CALIBRATION_MEASURE_IMAX)
backlash.distance_mm.i = m.backlash[IMAXIMUM];
backlash.set_distance_mm(I_AXIS, m.backlash[IMAXIMUM]);
#endif

#if HAS_J_CENTER
backlash.distance_mm.j = (m.backlash[JMINIMUM] + m.backlash[JMAXIMUM]) / 2;
backlash.set_distance_mm(J_AXIS, (m.backlash[JMINIMUM] + m.backlash[JMAXIMUM]) / 2);
#elif ENABLED(CALIBRATION_MEASURE_JMIN)
backlash.distance_mm.j = m.backlash[JMINIMUM];
backlash.set_distance_mm(J_AXIS, m.backlash[JMINIMUM]);
#elif ENABLED(CALIBRATION_MEASURE_JMAX)
backlash.distance_mm.j = m.backlash[JMAXIMUM];
backlash.set_distance_mm(J_AXIS, m.backlash[JMAXIMUM]);
#endif

#if HAS_K_CENTER
backlash.distance_mm.k = (m.backlash[KMINIMUM] + m.backlash[KMAXIMUM]) / 2;
backlash.set_distance_mm(K_AXIS, (m.backlash[KMINIMUM] + m.backlash[KMAXIMUM]) / 2);
#elif ENABLED(CALIBRATION_MEASURE_KMIN)
backlash.distance_mm.k = m.backlash[KMINIMUM];
backlash.set_distance_mm(K_AXIS, m.backlash[KMINIMUM]);
#elif ENABLED(CALIBRATION_MEASURE_KMAX)
backlash.distance_mm.k = m.backlash[KMAXIMUM];
backlash.set_distance_mm(K_AXIS, m.backlash[KMAXIMUM]);
#endif

#endif // BACKLASH_GCODE
Expand All @@ -581,7 +595,7 @@ inline void calibrate_backlash(measurements_t &m, const float uncertainty) {
// allowed directions to take up any backlash
{
// New scope for TEMPORARY_BACKLASH_CORRECTION
TEMPORARY_BACKLASH_CORRECTION(all_on);
TEMPORARY_BACKLASH_CORRECTION(backlash.all_on);
TEMPORARY_BACKLASH_SMOOTHING(0.0f);
const xyz_float_t move = LINEAR_AXIS_ARRAY(
AXIS_CAN_CALIBRATE(X) * 3, AXIS_CAN_CALIBRATE(Y) * 3, AXIS_CAN_CALIBRATE(Z) * 3,
Expand Down Expand Up @@ -611,7 +625,7 @@ inline void update_measurements(measurements_t &m, const AxisEnum axis) {
* - Call calibrate_backlash() beforehand for best accuracy
*/
inline void calibrate_toolhead(measurements_t &m, const float uncertainty, const uint8_t extruder) {
TEMPORARY_BACKLASH_CORRECTION(all_on);
TEMPORARY_BACKLASH_CORRECTION(backlash.all_on);
TEMPORARY_BACKLASH_SMOOTHING(0.0f);

TERN(HAS_MULTI_HOTEND, set_nozzle(m, extruder), UNUSED(extruder));
Expand Down Expand Up @@ -648,7 +662,7 @@ inline void calibrate_toolhead(measurements_t &m, const float uncertainty, const
* uncertainty in - How far away from the object to begin probing
*/
inline void calibrate_all_toolheads(measurements_t &m, const float uncertainty) {
TEMPORARY_BACKLASH_CORRECTION(all_on);
TEMPORARY_BACKLASH_CORRECTION(backlash.all_on);
TEMPORARY_BACKLASH_SMOOTHING(0.0f);

HOTEND_LOOP() calibrate_toolhead(m, uncertainty, e);
Expand All @@ -674,7 +688,7 @@ inline void calibrate_all() {

TERN_(HAS_HOTEND_OFFSET, reset_hotend_offsets());

TEMPORARY_BACKLASH_CORRECTION(all_on);
TEMPORARY_BACKLASH_CORRECTION(backlash.all_on);
TEMPORARY_BACKLASH_SMOOTHING(0.0f);

// Do a fast and rough calibration of the toolheads
Expand Down
Loading

0 comments on commit 6b7868d

Please sign in to comment.