|
@@ -236,7 +236,6 @@ void Planner::init() {
|
236
|
236
|
}
|
237
|
237
|
|
238
|
238
|
#if ENABLED(S_CURVE_ACCELERATION)
|
239
|
|
-
|
240
|
239
|
#ifdef __AVR__
|
241
|
240
|
/**
|
242
|
241
|
* This routine returns 0x1000000 / d, getting the inverse as fast as possible.
|
|
@@ -1570,7 +1569,7 @@ void Planner::synchronize() {
|
1570
|
1569
|
#endif
|
1571
|
1570
|
#endif
|
1572
|
1571
|
|
1573
|
|
- 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]) {
|
|
1572
|
+ 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) {
|
1574
|
1573
|
static uint8_t last_direction_bits;
|
1575
|
1574
|
uint8_t changed_dir = last_direction_bits ^ dm;
|
1576
|
1575
|
// Ignore direction change if no steps are taken in that direction
|
|
@@ -1598,25 +1597,21 @@ void Planner::synchronize() {
|
1598
|
1597
|
if (!changed_dir) return;
|
1599
|
1598
|
#endif
|
1600
|
1599
|
|
1601
|
|
- const bool positive[XYZ] = { da > 0, db > 0, dc > 0 };
|
1602
|
|
- #ifdef BACKLASH_SMOOTHING_MM
|
1603
|
|
- const bool non_zero[XYZ] = { da != 0, db != 0, dc != 0 };
|
1604
|
|
- #endif
|
1605
|
|
- bool made_adjustment = false;
|
|
1600
|
+ LOOP_XYZ(axis) {
|
|
1601
|
+ if (backlash_distance_mm[axis]) {
|
|
1602
|
+ const bool reversing = TEST(dm,axis);
|
1606
|
1603
|
|
1607
|
|
- LOOP_XYZ(i) {
|
1608
|
|
- if (backlash_distance_mm[i]) {
|
1609
|
1604
|
// When an axis changes direction, add axis backlash to the residual error
|
1610
|
|
- if (TEST(changed_dir, i))
|
1611
|
|
- residual_error[i] += backlash_correction * (positive[i] ? 1.0f : -1.0f) * backlash_distance_mm[i] * planner.settings.axis_steps_per_mm[i];
|
|
1605
|
+ if (TEST(changed_dir, axis))
|
|
1606
|
+ residual_error[axis] += backlash_correction * (reversing ? -1.0f : 1.0f) * backlash_distance_mm[axis] * planner.settings.axis_steps_per_mm[axis];
|
1612
|
1607
|
|
1613
|
1608
|
// Decide how much of the residual error to correct in this segment
|
1614
|
|
- int32_t error_correction = residual_error[i];
|
|
1609
|
+ int32_t error_correction = residual_error[axis];
|
1615
|
1610
|
#ifdef BACKLASH_SMOOTHING_MM
|
1616
|
1611
|
if (error_correction && backlash_smoothing_mm != 0) {
|
1617
|
1612
|
// Take up a portion of the residual_error in this segment, but only when
|
1618
|
1613
|
// the current segment travels in the same direction as the correction
|
1619
|
|
- if (non_zero[i] && positive[i] == (error_correction > 0)) {
|
|
1614
|
+ if (reversing == (error_correction < 0)) {
|
1620
|
1615
|
if (segment_proportion == 0)
|
1621
|
1616
|
segment_proportion = MIN(1.0f, block->millimeters / backlash_smoothing_mm);
|
1622
|
1617
|
error_correction *= segment_proportion;
|
|
@@ -1627,17 +1622,11 @@ void Planner::synchronize() {
|
1627
|
1622
|
#endif
|
1628
|
1623
|
// Making a correction reduces the residual error and modifies delta_mm
|
1629
|
1624
|
if (error_correction) {
|
1630
|
|
- block->steps[i] += ABS(error_correction);
|
1631
|
|
- residual_error[i] -= error_correction;
|
1632
|
|
- delta_mm[i] = (positive[i] ? 1.0f : -1.0f) * block->steps[i] * steps_to_mm[i];
|
1633
|
|
- made_adjustment = true;
|
|
1625
|
+ block->steps[axis] += ABS(error_correction);
|
|
1626
|
+ residual_error[axis] -= error_correction;
|
1634
|
1627
|
}
|
1635
|
1628
|
}
|
1636
|
1629
|
}
|
1637
|
|
-
|
1638
|
|
- // If any of the axes were adjusted, recompute block->millimeters
|
1639
|
|
- if (made_adjustment)
|
1640
|
|
- block->millimeters = SQRT(sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_AXIS]));
|
1641
|
1630
|
}
|
1642
|
1631
|
#endif // BACKLASH_COMPENSATION
|
1643
|
1632
|
|
|
@@ -1889,11 +1878,17 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
1889
|
1878
|
#endif
|
1890
|
1879
|
);
|
1891
|
1880
|
|
|
1881
|
+ /**
|
|
1882
|
+ * At this point at least one of the axes has more steps than
|
|
1883
|
+ * MIN_STEPS_PER_SEGMENT, ensuring the segment won't get dropped as
|
|
1884
|
+ * zero-length. It's important to not apply corrections
|
|
1885
|
+ * to blocks that would get dropped!
|
|
1886
|
+ *
|
|
1887
|
+ * A correction function is permitted to add steps to an axis, it
|
|
1888
|
+ * should *never* remove steps!
|
|
1889
|
+ */
|
1892
|
1890
|
#if ENABLED(BACKLASH_COMPENSATION)
|
1893
|
|
- // If we make it here, at least one of the axes has more steps than
|
1894
|
|
- // MIN_STEPS_PER_SEGMENT, so the segment won't get dropped by Marlin
|
1895
|
|
- // and it is okay to add steps for backlash correction.
|
1896
|
|
- add_backlash_correction_steps(da, db, dc, dm, block, delta_mm);
|
|
1891
|
+ add_backlash_correction_steps(da, db, dc, dm, block);
|
1897
|
1892
|
#endif
|
1898
|
1893
|
}
|
1899
|
1894
|
|
|
@@ -2344,7 +2339,6 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
2344
|
2339
|
float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2
|
2345
|
2340
|
|
2346
|
2341
|
#if ENABLED(JUNCTION_DEVIATION)
|
2347
|
|
-
|
2348
|
2342
|
/**
|
2349
|
2343
|
* Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
|
2350
|
2344
|
* Let a circle be tangent to both previous and current path line segments, where the junction
|