Browse Source

Planner block HOLD flag

Allows the Stepper ISR to wait until a given block is free for use. Allows Planner to plan the first move, which is split into two.
etagle 7 years ago
parent
commit
e0ca627033
2 changed files with 62 additions and 93 deletions
  1. 35
    82
      Marlin/src/module/planner.cpp
  2. 27
    11
      Marlin/src/module/planner.h

+ 35
- 82
Marlin/src/module/planner.cpp View File

@@ -92,6 +92,10 @@
92 92
   #include "../feature/power.h"
93 93
 #endif
94 94
 
95
+// Delay for delivery of first block to the stepper ISR, if the queue contains 2 or
96
+// fewer movements. The delay is measured in milliseconds, and must be less than 250ms
97
+#define BLOCK_DELAY_FOR_1ST_MOVE 50
98
+
95 99
 Planner planner;
96 100
 
97 101
   // public:
@@ -102,7 +106,8 @@ Planner planner;
102 106
 block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
103 107
 volatile uint8_t Planner::block_buffer_head,  // Index of the next block to be pushed
104 108
                  Planner::block_buffer_tail;  // Index of the busy block, if any
105
-uint16_t Planner::cleaning_buffer_counter;   // A counter to disable queuing of blocks
109
+uint16_t Planner::cleaning_buffer_counter;    // A counter to disable queuing of blocks
110
+uint8_t Planner::delay_before_delivering;     // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
106 111
 
107 112
 float Planner::max_feedrate_mm_s[XYZE_N],   // Max speeds in mm per second
108 113
       Planner::axis_steps_per_mm[XYZE_N],
@@ -222,6 +227,7 @@ void Planner::init() {
222 227
     bed_level_matrix.set_to_identity();
223 228
   #endif
224 229
   clear_block_buffer();
230
+  delay_before_delivering = 0;
225 231
 }
226 232
 
227 233
 #if ENABLED(BEZIER_JERK_CONTROL)
@@ -802,7 +808,8 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
802 808
   const bool was_enabled = STEPPER_ISR_ENABLED();
803 809
   if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
804 810
 
805
-  if (!TEST(block->flag, BLOCK_BIT_BUSY)) { // Don't update variables if block is busy.
811
+  // Don't update variables if block is busy: It is being interpreted by the planner
812
+  if (!TEST(block->flag, BLOCK_BIT_BUSY)) {
806 813
     block->accelerate_until = accelerate_steps;
807 814
     block->decelerate_after = accelerate_steps + plateau_steps;
808 815
     block->initial_rate = initial_rate;
@@ -1346,6 +1353,10 @@ void Planner::quick_stop() {
1346 1353
   // that is why we set head to tail!
1347 1354
   block_buffer_head = block_buffer_tail;
1348 1355
 
1356
+  // Restart the block delay for the first movement - As the queue was
1357
+  // forced to empty, there's no risk the ISR will touch this.
1358
+  delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
1359
+
1349 1360
   #if ENABLED(ULTRA_LCD)
1350 1361
     // Clear the accumulated runtime
1351 1362
     clear_block_buffer_runtime();
@@ -1374,12 +1385,6 @@ void Planner::endstop_triggered(const AxisEnum axis) {
1374 1385
   // Discard the active block that led to the trigger
1375 1386
   discard_current_block();
1376 1387
 
1377
-  // Discard the CONTINUED block, if any. Note the planner can only queue 1 continued
1378
-  // block after a previous non continued block, as the condition to queue them
1379
-  // is that there are no queued blocks at the time a new block is queued.
1380
-  const bool discard = has_blocks_queued() && TEST(block_buffer[block_buffer_tail].flag, BLOCK_BIT_CONTINUED);
1381
-  if (discard) discard_current_block();
1382
-
1383 1388
   // Reenable stepper ISR if it was enabled
1384 1389
   if (stepper_isr_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
1385 1390
 }
@@ -1467,6 +1472,16 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
1467 1472
     return true;
1468 1473
   }
1469 1474
 
1475
+  // If this is the first added movement, reload the delay, otherwise, cancel it.
1476
+  if (block_buffer_head == block_buffer_tail) {
1477
+    // If it was the first queued block, restart the 1st block delivery delay, to
1478
+    // give the planner an opportunity to queue more movements and plan them
1479
+    // As there are no queued movements, the Stepper ISR will not touch this
1480
+    // variable, so there is no risk setting this here (but it MUST be done
1481
+    // before the following line!!)
1482
+    delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
1483
+  }
1484
+
1470 1485
   // Move buffer head
1471 1486
   block_buffer_head = next_buffer_head;
1472 1487
 
@@ -2292,7 +2307,18 @@ void Planner::buffer_sync_block() {
2292 2307
   block->position[C_AXIS] = position[C_AXIS];
2293 2308
   block->position[E_AXIS] = position[E_AXIS];
2294 2309
 
2310
+  // If this is the first added movement, reload the delay, otherwise, cancel it.
2311
+  if (block_buffer_head == block_buffer_tail) {
2312
+    // If it was the first queued block, restart the 1st block delivery delay, to
2313
+    // give the planner an opportunity to queue more movements and plan them
2314
+    // As there are no queued movements, the Stepper ISR will not touch this
2315
+    // variable, so there is no risk setting this here (but it MUST be done
2316
+    // before the following line!!)
2317
+    delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
2318
+  }
2319
+
2295 2320
   block_buffer_head = next_buffer_head;
2321
+
2296 2322
   stepper.wake_up();
2297 2323
 } // buffer_sync_block()
2298 2324
 
@@ -2370,81 +2396,8 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con
2370 2396
     SERIAL_ECHOLNPGM(")");
2371 2397
   //*/
2372 2398
 
2373
-  // Always split the first move into two (if not homing or probing)
2374
-  if (!has_blocks_queued()) {
2375
-
2376
-    #define _BETWEEN(A) (position[_AXIS(A)] + target[_AXIS(A)]) >> 1
2377
-    const int32_t between[ABCE] = { _BETWEEN(A), _BETWEEN(B), _BETWEEN(C), _BETWEEN(E) };
2378
-
2379
-    #if HAS_POSITION_FLOAT
2380
-      #define _BETWEEN_F(A) (position_float[_AXIS(A)] + target_float[_AXIS(A)]) * 0.5
2381
-      const float between_float[ABCE] = { _BETWEEN_F(A), _BETWEEN_F(B), _BETWEEN_F(C), _BETWEEN_F(E) };
2382
-    #endif
2383
-
2384
-    // The new head value is not assigned yet
2385
-    uint8_t buffer_head = 0;
2386
-    bool added = false;
2387
-
2388
-    uint8_t next_buffer_head;
2389
-    block_t *block = get_next_free_block(next_buffer_head, 2);
2390
-
2391
-    // Fill the block with the specified movement
2399
+  // Queue the movement
2392 2400
     if (
2393
-      _populate_block(block, true, between
2394
-        #if HAS_POSITION_FLOAT
2395
-          , between_float
2396
-        #endif
2397
-        , fr_mm_s, extruder, millimeters * 0.5
2398
-      )
2399
-    ) {
2400
-      // Movement accepted - Point to the next reserved block
2401
-      block = &block_buffer[next_buffer_head];
2402
-
2403
-      // Store into the new to be stored head
2404
-      buffer_head = next_buffer_head;
2405
-      added = true;
2406
-
2407
-      // And advance the pointer to the next unused slot
2408
-      next_buffer_head = next_block_index(next_buffer_head);
2409
-    }
2410
-
2411
-    // Fill the second part of the block with the 2nd part of the movement
2412
-    if (
2413
-      _populate_block(block, true, target
2414
-        #if HAS_POSITION_FLOAT
2415
-          , target_float
2416
-        #endif
2417
-        , fr_mm_s, extruder, millimeters * 0.5
2418
-      )
2419
-    ) {
2420
-      // Movement accepted - If this block is a continuation
2421
-      // of the previous one, mark it as such
2422
-      if (added) SBI(block->flag, BLOCK_BIT_CONTINUED);
2423
-
2424
-      // Store into the new to be stored head
2425
-      buffer_head = next_buffer_head;
2426
-      added = true;
2427
-    }
2428
-
2429
-    // If any of the movements was added
2430
-    if (added) {
2431
-
2432
-      // Move buffer head and add all the blocks that were filled
2433
-      // successfully to the movement queue.
2434
-      block_buffer_head = buffer_head;
2435
-
2436
-      // Update the position (only when a move was queued)
2437
-      static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!");
2438
-      COPY(position, target);
2439
-      #if HAS_POSITION_FLOAT
2440
-        COPY(position_float, target_float);
2441
-      #endif
2442
-
2443
-      // Recalculate and optimize trapezoidal speed profiles
2444
-      recalculate();
2445
-    }
2446
-  }
2447
-  else if (
2448 2401
     !_buffer_steps(target
2449 2402
       #if HAS_POSITION_FLOAT
2450 2403
         , target_float

+ 27
- 11
Marlin/src/module/planner.h View File

@@ -54,7 +54,7 @@ enum BlockFlagBit : char {
54 54
   // from a safe speed (in consideration of jerking from zero speed).
55 55
   BLOCK_BIT_NOMINAL_LENGTH,
56 56
 
57
-  // The block is busy
57
+  // The block is busy, being interpreted by the stepper ISR
58 58
   BLOCK_BIT_BUSY,
59 59
 
60 60
   // The block is segment 2+ of a longer move
@@ -176,7 +176,8 @@ class Planner {
176 176
     static block_t block_buffer[BLOCK_BUFFER_SIZE];
177 177
     static volatile uint8_t block_buffer_head,      // Index of the next block to be pushed
178 178
                             block_buffer_tail;      // Index of the busy block, if any
179
-    static int16_t cleaning_buffer_counter;         // A counter to disable queuing of blocks
179
+    static uint16_t cleaning_buffer_counter;        // A counter to disable queuing of blocks
180
+    static uint8_t delay_before_delivering;         // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
180 181
 
181 182
     #if ENABLED(DISTINCT_E_FACTORS)
182 183
       static uint8_t last_extruder;                 // Respond to extruder change
@@ -634,25 +635,40 @@ class Planner {
634 635
      * WARNING: Called from Stepper ISR context!
635 636
      */
636 637
     static block_t* get_current_block() {
637
-      if (has_blocks_queued()) {
638
-        block_t * const block = &block_buffer[block_buffer_tail];
639 638
 
640
-        // If the block has no trapezoid calculated, it's unsafe to execute.
641
-        if (movesplanned() > 1) {
642
-          const block_t * const next = &block_buffer[next_block_index(block_buffer_tail)];
643
-          if (TEST(block->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE))
644
-            return NULL;
639
+      // Get the number of moves in the planner queue so far
640
+      uint8_t nr_moves = movesplanned();
641
+
642
+      // If there are any moves queued ...
643
+      if (nr_moves) {
644
+
645
+        // If there is still delay of delivery of blocks running, decrement it
646
+        if (delay_before_delivering) {
647
+          --delay_before_delivering;
648
+          // If the number of movements queued is less than 3, and there is still time
649
+          //  to wait, do not deliver anything
650
+          if (nr_moves < 3 && delay_before_delivering) return NULL;
651
+          delay_before_delivering = 0;
645 652
         }
646
-        else if (TEST(block->flag, BLOCK_BIT_RECALCULATE))
647
-          return NULL;
653
+
654
+        // If we are here, there is no excuse to deliver the block
655
+        block_t * const block = &block_buffer[block_buffer_tail];
656
+
657
+        // No trapezoid calculated? Don't execute yet.
658
+        if ( TEST(block->flag, BLOCK_BIT_RECALCULATE)
659
+          || (movesplanned() > 1 && TEST(block_buffer[next_block_index(block_buffer_tail)].flag, BLOCK_BIT_RECALCULATE))
660
+        ) return NULL;
648 661
 
649 662
         #if ENABLED(ULTRA_LCD)
650 663
           block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.
651 664
         #endif
665
+
666
+        // Mark the block as busy, so the planner does not attempt to replan it
652 667
         SBI(block->flag, BLOCK_BIT_BUSY);
653 668
         return block;
654 669
       }
655 670
       else {
671
+        // The queue became empty
656 672
         #if ENABLED(ULTRA_LCD)
657 673
           clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
658 674
         #endif

Loading…
Cancel
Save