Преглед изворни кода

Simplify backlash compensation code. (#12813)

- 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.
Marcio Teixeira пре 6 година
родитељ
комит
6a8fb0f25f
2 измењених фајлова са 21 додато и 27 уклоњено
  1. 20
    26
      Marlin/src/module/planner.cpp
  2. 1
    1
      Marlin/src/module/planner.h

+ 20
- 26
Marlin/src/module/planner.cpp Прегледај датотеку

@@ -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

+ 1
- 1
Marlin/src/module/planner.h Прегледај датотеку

@@ -339,7 +339,7 @@ class Planner {
339 339
     #endif
340 340
 
341 341
     #if ENABLED(BACKLASH_COMPENSATION)
342
-      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]);
342
+      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);
343 343
     #endif
344 344
 
345 345
   public:

Loading…
Откажи
Сачувај