bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

🐛 Fix backlash applied steps when config changes (#23826)

Browse Source 
Followup to #23814
This commit is contained in:
tombrazier
2022-03-01 22:14:52 +00:00
committed by Scott Lahteine
parent 2d648e4dd9
commit 41a51e9527
8 changed files with 167 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
Marlin/src/feature/backlash.cpp
View File

@ -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)
@ -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
@ -87,7 +85,7 @@ 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
@ -95,35 +93,28 @@ 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;
#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
@ -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"

46
Marlin/src/feature/backlash.h
View File

@ -24,27 +24,22 @@
#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;
@ -52,13 +47,13 @@ public:
#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
@ -78,7 +73,24 @@ public:
}
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;