Przeglądaj źródła

Improve sync of stepper positions

Scott Lahteine 7 lat temu
rodzic
commit
af1950a63e

+ 2
- 6
Marlin/src/gcode/geometry/G92.cpp Wyświetl plik

@@ -33,8 +33,6 @@
33 33
  */
34 34
 void GcodeSuite::G92() {
35 35
 
36
-  stepper.synchronize();
37
-
38 36
   #if ENABLED(CNC_COORDINATE_SYSTEMS)
39 37
     switch (parser.subcode) {
40 38
       case 1:
@@ -94,10 +92,8 @@ void GcodeSuite::G92() {
94 92
       COPY(coordinate_system[active_coordinate_system], position_shift);
95 93
   #endif
96 94
 
97
-  if (didXYZ)
98
-    SYNC_PLAN_POSITION_KINEMATIC();
99
-  else if (didE)
100
-    sync_plan_position_e();
95
+  if    (didXYZ) SYNC_PLAN_POSITION_KINEMATIC();
96
+  else if (didE) sync_plan_position_e();
101 97
 
102 98
   report_current_position();
103 99
 }

+ 36
- 22
Marlin/src/module/planner.cpp Wyświetl plik

@@ -1382,15 +1382,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
1382 1382
   const float esteps_float = de * e_factor[extruder];
1383 1383
   const int32_t esteps = abs(esteps_float) + 0.5;
1384 1384
 
1385
-  // Calculate the buffer head after we push this byte
1386
-  const uint8_t next_buffer_head = next_block_index(block_buffer_head);
1387
-
1388
-  // If the buffer is full: good! That means we are well ahead of the robot.
1389
-  // Rest here until there is room in the buffer.
1390
-  while (block_buffer_tail == next_buffer_head) idle();
1391
-
1392
-  // Prepare to set up new block
1393
-  block_t* block = &block_buffer[block_buffer_head];
1385
+  // Wait for the next available block
1386
+  uint8_t next_buffer_head;
1387
+  block_t * const block = get_next_free_block(next_buffer_head);
1394 1388
 
1395 1389
   // Clear all flags, including the "busy" bit
1396 1390
   block->flag = 0x00;
@@ -2033,6 +2027,26 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
2033 2027
 } // _buffer_steps()
2034 2028
 
2035 2029
 /**
2030
+ * Planner::buffer_sync_block
2031
+ * Add a block to the buffer that just updates the position
2032
+ */
2033
+void Planner::buffer_sync_block() {
2034
+  // Wait for the next available block
2035
+  uint8_t next_buffer_head;
2036
+  block_t * const block = get_next_free_block(next_buffer_head);
2037
+
2038
+  block->steps[A_AXIS] = position[A_AXIS];
2039
+  block->steps[B_AXIS] = position[B_AXIS];
2040
+  block->steps[C_AXIS] = position[C_AXIS];
2041
+  block->steps[E_AXIS] = position[E_AXIS];
2042
+
2043
+  block->flag = BLOCK_FLAG_SYNC_POSITION;
2044
+
2045
+  block_buffer_head = next_buffer_head;
2046
+  stepper.wake_up();
2047
+} // buffer_sync_block()
2048
+
2049
+/**
2036 2050
  * Planner::buffer_segment
2037 2051
  *
2038 2052
  * Add a new linear movement to the buffer in axis units.
@@ -2160,19 +2174,19 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
2160 2174
   #else
2161 2175
     #define _EINDEX E_AXIS
2162 2176
   #endif
2163
-  const int32_t na = position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]),
2164
-                nb = position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]),
2165
-                nc = position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]),
2166
-                ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
2177
+  position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]),
2178
+  position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]),
2179
+  position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]),
2180
+  position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
2167 2181
   #if HAS_POSITION_FLOAT
2168
-    position_float[X_AXIS] = a;
2169
-    position_float[Y_AXIS] = b;
2170
-    position_float[Z_AXIS] = c;
2182
+    position_float[A_AXIS] = a;
2183
+    position_float[B_AXIS] = b;
2184
+    position_float[C_AXIS] = c;
2171 2185
     position_float[E_AXIS] = e;
2172 2186
   #endif
2173
-  stepper.set_position(na, nb, nc, ne);
2174 2187
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
2175 2188
   ZERO(previous_speed);
2189
+  buffer_sync_block();
2176 2190
 }
2177 2191
 
2178 2192
 void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
@@ -2220,23 +2234,23 @@ void Planner::set_position_mm(const AxisEnum axis, const float &v) {
2220 2234
   #if HAS_POSITION_FLOAT
2221 2235
     position_float[axis] = v;
2222 2236
   #endif
2223
-  stepper.set_position(axis, position[axis]);
2224 2237
   previous_speed[axis] = 0.0;
2238
+  buffer_sync_block();
2225 2239
 }
2226 2240
 
2227 2241
 // Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
2228 2242
 void Planner::reset_acceleration_rates() {
2229 2243
   #if ENABLED(DISTINCT_E_FACTORS)
2230
-    #define HIGHEST_CONDITION (i < E_AXIS || i == E_AXIS + active_extruder)
2244
+    #define AXIS_CONDITION (i < E_AXIS || i == E_AXIS + active_extruder)
2231 2245
   #else
2232
-    #define HIGHEST_CONDITION true
2246
+    #define AXIS_CONDITION true
2233 2247
   #endif
2234 2248
   uint32_t highest_rate = 1;
2235 2249
   LOOP_XYZE_N(i) {
2236 2250
     max_acceleration_steps_per_s2[i] = max_acceleration_mm_per_s2[i] * axis_steps_per_mm[i];
2237
-    if (HIGHEST_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]);
2251
+    if (AXIS_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]);
2238 2252
   }
2239
-  cutoff_long = 4294967295UL / highest_rate;
2253
+  cutoff_long = 4294967295UL / highest_rate; // 0xFFFFFFFFUL
2240 2254
 }
2241 2255
 
2242 2256
 // Recalculate position, steps_to_mm if axis_steps_per_mm changes!

+ 28
- 4
Marlin/src/module/planner.h Wyświetl plik

@@ -57,14 +57,18 @@ enum BlockFlagBit : char {
57 57
   BLOCK_BIT_BUSY,
58 58
 
59 59
   // The block is segment 2+ of a longer move
60
-  BLOCK_BIT_CONTINUED
60
+  BLOCK_BIT_CONTINUED,
61
+
62
+  // Sync the stepper counts from the block
63
+  BLOCK_BIT_SYNC_POSITION
61 64
 };
62 65
 
63 66
 enum BlockFlag : char {
64 67
   BLOCK_FLAG_RECALCULATE          = _BV(BLOCK_BIT_RECALCULATE),
65 68
   BLOCK_FLAG_NOMINAL_LENGTH       = _BV(BLOCK_BIT_NOMINAL_LENGTH),
66 69
   BLOCK_FLAG_BUSY                 = _BV(BLOCK_BIT_BUSY),
67
-  BLOCK_FLAG_CONTINUED            = _BV(BLOCK_BIT_CONTINUED)
70
+  BLOCK_FLAG_CONTINUED            = _BV(BLOCK_BIT_CONTINUED),
71
+  BLOCK_FLAG_SYNC_POSITION        = _BV(BLOCK_BIT_SYNC_POSITION)
68 72
 };
69 73
 
70 74
 /**
@@ -422,6 +426,20 @@ class Planner {
422 426
 
423 427
     #endif
424 428
 
429
+
430
+    /**
431
+     * Planner::get_next_free_block
432
+     *
433
+     * - Get the next head index (passed by reference)
434
+     * - Wait for a space to open up in the planner
435
+     * - Return the head block
436
+     */
437
+    FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head) {
438
+      next_buffer_head = next_block_index(block_buffer_head);
439
+      while (block_buffer_tail == next_buffer_head) idle(); // while (is_full)
440
+      return &block_buffer[block_buffer_head];
441
+    }
442
+
425 443
     /**
426 444
      * Planner::_buffer_steps
427 445
      *
@@ -440,6 +458,12 @@ class Planner {
440 458
     );
441 459
 
442 460
     /**
461
+     * Planner::buffer_sync_block
462
+     * Add a block to the buffer that just updates the position
463
+     */
464
+    static void buffer_sync_block();
465
+
466
+    /**
443 467
      * Planner::buffer_segment
444 468
      *
445 469
      * Add a new linear movement to the buffer in axis units.
@@ -518,7 +542,7 @@ class Planner {
518 542
     static void set_position_mm_kinematic(const float (&cart)[XYZE]);
519 543
     static void set_position_mm(const AxisEnum axis, const float &v);
520 544
     FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
521
-    FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(AxisEnum(E_AXIS), e); }
545
+    FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); }
522 546
 
523 547
     /**
524 548
      * Sync from the stepper positions. (e.g., after an interrupted move)
@@ -528,7 +552,7 @@ class Planner {
528 552
     /**
529 553
      * Does the buffer have any blocks queued?
530 554
      */
531
-    static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
555
+    FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
532 556
 
533 557
     /**
534 558
      * "Discard" the block and "release" the memory.

+ 11
- 20
Marlin/src/module/stepper.cpp Wyświetl plik

@@ -1217,6 +1217,16 @@ void Stepper::isr() {
1217 1217
     // Anything in the buffer?
1218 1218
     if ((current_block = planner.get_current_block())) {
1219 1219
 
1220
+      // Sync block? Sync the stepper counts and return
1221
+      while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
1222
+        _set_position(
1223
+          current_block->steps[A_AXIS], current_block->steps[B_AXIS],
1224
+          current_block->steps[C_AXIS], current_block->steps[E_AXIS]
1225
+        );
1226
+        planner.discard_current_block();
1227
+        if (!(current_block = planner.get_current_block())) return;
1228
+      }
1229
+
1220 1230
       // Initialize the trapezoid generator from the current block.
1221 1231
       static int8_t last_extruder = -1;
1222 1232
 
@@ -1976,12 +1986,7 @@ void Stepper::synchronize() { while (planner.has_blocks_queued() || cleaning_buf
1976 1986
  * This allows get_axis_position_mm to correctly
1977 1987
  * derive the current XYZ position later on.
1978 1988
  */
1979
-void Stepper::set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
1980
-
1981
-  synchronize(); // Bad to set stepper counts in the middle of a move
1982
-
1983
-  CRITICAL_SECTION_START;
1984
-
1989
+void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
1985 1990
   #if CORE_IS_XY
1986 1991
     // corexy positioning
1987 1992
     // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
@@ -2004,21 +2009,7 @@ void Stepper::set_position(const int32_t &a, const int32_t &b, const int32_t &c,
2004 2009
     count_position[Y_AXIS] = b;
2005 2010
     count_position[Z_AXIS] = c;
2006 2011
   #endif
2007
-
2008
-  count_position[E_AXIS] = e;
2009
-  CRITICAL_SECTION_END;
2010
-}
2011
-
2012
-void Stepper::set_position(const AxisEnum &axis, const int32_t &v) {
2013
-  CRITICAL_SECTION_START;
2014
-  count_position[axis] = v;
2015
-  CRITICAL_SECTION_END;
2016
-}
2017
-
2018
-void Stepper::set_e_position(const int32_t &e) {
2019
-  CRITICAL_SECTION_START;
2020 2012
   count_position[E_AXIS] = e;
2021
-  CRITICAL_SECTION_END;
2022 2013
 }
2023 2014
 
2024 2015
 /**

+ 26
- 3
Marlin/src/module/stepper.h Wyświetl plik

@@ -191,9 +191,32 @@ class Stepper {
191 191
     //
192 192
     // Set the current position in steps
193 193
     //
194
-    static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
195
-    static void set_position(const AxisEnum &a, const int32_t &v);
196
-    static void set_e_position(const int32_t &e);
194
+    static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
195
+
196
+    FORCE_INLINE static void _set_position(const AxisEnum a, const int32_t &v) { count_position[a] = v; }
197
+
198
+    FORCE_INLINE static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
199
+      synchronize();
200
+      CRITICAL_SECTION_START;
201
+      _set_position(a, b, c, e);
202
+      CRITICAL_SECTION_END;
203
+    }
204
+
205
+    static void set_position(const AxisEnum a, const int32_t &v) {
206
+      synchronize();
207
+      CRITICAL_SECTION_START;
208
+      count_position[a] = v;
209
+      CRITICAL_SECTION_END;
210
+    }
211
+
212
+    FORCE_INLINE static void _set_e_position(const int32_t &e) { count_position[E_AXIS] = e; }
213
+
214
+    static void set_e_position(const int32_t &e) {
215
+      synchronize();
216
+      CRITICAL_SECTION_START;
217
+      count_position[E_AXIS] = e;
218
+      CRITICAL_SECTION_END;
219
+    }
197 220
 
198 221
     //
199 222
     // Set direction bits for all steppers

Ładowanie…
Anuluj
Zapisz