MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Simplify backlash compensation code. (#12813)

Browse Source 
- Use `TEST(dm,axis)` to determine directions instead of doing comparisons.
- Remove recomputation of `millimeters` and `delta_mm` since backlash compensation should not affect the distance over which material is extruded.
This commit is contained in:
Marcio Teixeira 2019-01-04 12:30:56 -07:00 committed by Scott Lahteine
parent a59d3d4323
commit 6a8fb0f25f
2 changed files with 21 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
Marlin/src/module/planner.cpp
View File

@ -236,7 +236,6 @@ void Planner::init() {
}
#if ENABLED(S_CURVE_ACCELERATION)
#ifdef __AVR__
/**
* This routine returns 0x1000000 / d, getting the inverse as fast as possible.
@ -1570,7 +1569,7 @@ void Planner::synchronize() {
#endif
#endif
void Planner::add_backlash_correction_steps(const int32_t da, const int32_t db, const int32_t dc, const uint8_t dm, block_t * const block, float (&delta_mm)[ABCE]) {
void Planner::add_backlash_correction_steps(const int32_t da, const int32_t db, const int32_t dc, const uint8_t dm, block_t * const block) {
static uint8_t last_direction_bits;
uint8_t changed_dir = last_direction_bits ^ dm;
// Ignore direction change if no steps are taken in that direction
@ -1598,25 +1597,21 @@ void Planner::synchronize() {
if (!changed_dir) return;
#endif
const bool positive[XYZ] = { da > 0, db > 0, dc > 0 };
#ifdef BACKLASH_SMOOTHING_MM
const bool non_zero[XYZ] = { da != 0, db != 0, dc != 0 };
#endif
bool made_adjustment = false;
LOOP_XYZ(axis) {
if (backlash_distance_mm[axis]) {
const bool reversing = TEST(dm,axis);
LOOP_XYZ(i) {
if (backlash_distance_mm[i]) {
// When an axis changes direction, add axis backlash to the residual error
if (TEST(changed_dir, i))
residual_error[i] += backlash_correction * (positive[i] ? 1.0f : -1.0f) * backlash_distance_mm[i] * planner.settings.axis_steps_per_mm[i];
if (TEST(changed_dir, axis))
residual_error[axis] += backlash_correction * (reversing ? -1.0f : 1.0f) * backlash_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[i];
int32_t error_correction = residual_error[axis];
#ifdef BACKLASH_SMOOTHING_MM
if (error_correction && backlash_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 (non_zero[i] && positive[i] == (error_correction > 0)) {
if (reversing == (error_correction < 0)) {
if (segment_proportion == 0)
segment_proportion = MIN(1.0f, block->millimeters / backlash_smoothing_mm);
error_correction *= segment_proportion;
@ -1627,17 +1622,11 @@ void Planner::synchronize() {
#endif
// Making a correction reduces the residual error and modifies delta_mm
if (error_correction) {
block->steps[i] += ABS(error_correction);
residual_error[i] -= error_correction;
delta_mm[i] = (positive[i] ? 1.0f : -1.0f) * block->steps[i] * steps_to_mm[i];
made_adjustment = true;
block->steps[axis] += ABS(error_correction);
residual_error[axis] -= error_correction;
}
}
}
// If any of the axes were adjusted, recompute block->millimeters
if (made_adjustment)
block->millimeters = SQRT(sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_AXIS]));
}
#endif // BACKLASH_COMPENSATION
@ -1889,11 +1878,17 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
#endif
);
/**
* At this point at least one of the axes has more steps than
* MIN_STEPS_PER_SEGMENT, ensuring the segment won't get dropped as
* zero-length. It's important to not apply corrections
* to blocks that would get dropped!
*
* A correction function is permitted to add steps to an axis, it
* should *never* remove steps!
*/
#if ENABLED(BACKLASH_COMPENSATION)
// If we make it here, at least one of the axes has more steps than
// MIN_STEPS_PER_SEGMENT, so the segment won't get dropped by Marlin
// and it is okay to add steps for backlash correction.
add_backlash_correction_steps(da, db, dc, dm, block, delta_mm);
add_backlash_correction_steps(da, db, dc, dm, block);
#endif
}
@ -2344,7 +2339,6 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2
#if ENABLED(JUNCTION_DEVIATION)
/**
* Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
* Let a circle be tangent to both previous and current path line segments, where the junction

2
Marlin/src/module/planner.h
View File

@ -339,7 +339,7 @@ class Planner {
#endif
#if ENABLED(BACKLASH_COMPENSATION)
static void add_backlash_correction_steps(const int32_t da, const int32_t db, const int32_t dc, const uint8_t dm, block_t * const block, float (&delta_mm)[ABCE]);
static void add_backlash_correction_steps(const int32_t da, const int32_t db, const int32_t dc, const uint8_t dm, block_t * const block);
#endif
public: