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