浏览代码

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
 }
236
 }
237
 
237
 
238
 #if ENABLED(S_CURVE_ACCELERATION)
238
 #if ENABLED(S_CURVE_ACCELERATION)
239
-
240
   #ifdef __AVR__
239
   #ifdef __AVR__
241
     /**
240
     /**
242
      * This routine returns 0x1000000 / d, getting the inverse as fast as possible.
241
      * This routine returns 0x1000000 / d, getting the inverse as fast as possible.
1570
     #endif
1569
     #endif
1571
   #endif
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
     static uint8_t last_direction_bits;
1573
     static uint8_t last_direction_bits;
1575
     uint8_t changed_dir = last_direction_bits ^ dm;
1574
     uint8_t changed_dir = last_direction_bits ^ dm;
1576
     // Ignore direction change if no steps are taken in that direction
1575
     // Ignore direction change if no steps are taken in that direction
1598
       if (!changed_dir) return;
1597
       if (!changed_dir) return;
1599
     #endif
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
         // When an axis changes direction, add axis backlash to the residual error
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
         // Decide how much of the residual error to correct in this segment
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
         #ifdef BACKLASH_SMOOTHING_MM
1610
         #ifdef BACKLASH_SMOOTHING_MM
1616
           if (error_correction && backlash_smoothing_mm != 0) {
1611
           if (error_correction && backlash_smoothing_mm != 0) {
1617
             // Take up a portion of the residual_error in this segment, but only when
1612
             // Take up a portion of the residual_error in this segment, but only when
1618
             // the current segment travels in the same direction as the correction
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
               if (segment_proportion == 0)
1615
               if (segment_proportion == 0)
1621
                 segment_proportion = MIN(1.0f, block->millimeters / backlash_smoothing_mm);
1616
                 segment_proportion = MIN(1.0f, block->millimeters / backlash_smoothing_mm);
1622
               error_correction *= segment_proportion;
1617
               error_correction *= segment_proportion;
1627
         #endif
1622
         #endif
1628
         // Making a correction reduces the residual error and modifies delta_mm
1623
         // Making a correction reduces the residual error and modifies delta_mm
1629
         if (error_correction) {
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
 #endif // BACKLASH_COMPENSATION
1631
 #endif // BACKLASH_COMPENSATION
1643
 
1632
 
1889
         #endif
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
     #if ENABLED(BACKLASH_COMPENSATION)
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
     #endif
1892
     #endif
1898
   }
1893
   }
1899
 
1894
 
2344
   float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2
2339
   float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2
2345
 
2340
 
2346
   #if ENABLED(JUNCTION_DEVIATION)
2341
   #if ENABLED(JUNCTION_DEVIATION)
2347
-
2348
     /**
2342
     /**
2349
      * Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
2343
      * Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
2350
      * Let a circle be tangent to both previous and current path line segments, where the junction
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
     #endif
339
     #endif
340
 
340
 
341
     #if ENABLED(BACKLASH_COMPENSATION)
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
     #endif
343
     #endif
344
 
344
 
345
   public:
345
   public:

正在加载...
取消
保存