From e0ca627033504dd7bf8d9b87ce9ec526ee792276 Mon Sep 17 00:00:00 2001 From: etagle Date: Sun, 13 May 2018 00:49:54 -0300 Subject: [PATCH] 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. --- Marlin/src/module/planner.cpp | 117 ++++++++++------------------------ Marlin/src/module/planner.h | 38 +++++++---- 2 files changed, 62 insertions(+), 93 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index d301971cf..78634f799 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -92,6 +92,10 @@ #include "../feature/power.h" #endif +// Delay for delivery of first block to the stepper ISR, if the queue contains 2 or +// fewer movements. The delay is measured in milliseconds, and must be less than 250ms +#define BLOCK_DELAY_FOR_1ST_MOVE 50 + Planner planner; // public: @@ -102,7 +106,8 @@ Planner planner; block_t Planner::block_buffer[BLOCK_BUFFER_SIZE]; volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed Planner::block_buffer_tail; // Index of the busy block, if any -uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks +uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks +uint8_t Planner::delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second Planner::axis_steps_per_mm[XYZE_N], @@ -222,6 +227,7 @@ void Planner::init() { bed_level_matrix.set_to_identity(); #endif clear_block_buffer(); + delay_before_delivering = 0; } #if ENABLED(BEZIER_JERK_CONTROL) @@ -802,7 +808,8 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e const bool was_enabled = STEPPER_ISR_ENABLED(); if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT(); - if (!TEST(block->flag, BLOCK_BIT_BUSY)) { // Don't update variables if block is busy. + // Don't update variables if block is busy: It is being interpreted by the planner + if (!TEST(block->flag, BLOCK_BIT_BUSY)) { block->accelerate_until = accelerate_steps; block->decelerate_after = accelerate_steps + plateau_steps; block->initial_rate = initial_rate; @@ -1346,6 +1353,10 @@ void Planner::quick_stop() { // that is why we set head to tail! block_buffer_head = block_buffer_tail; + // Restart the block delay for the first movement - As the queue was + // forced to empty, there's no risk the ISR will touch this. + delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE; + #if ENABLED(ULTRA_LCD) // Clear the accumulated runtime clear_block_buffer_runtime(); @@ -1374,12 +1385,6 @@ void Planner::endstop_triggered(const AxisEnum axis) { // Discard the active block that led to the trigger discard_current_block(); - // Discard the CONTINUED block, if any. Note the planner can only queue 1 continued - // block after a previous non continued block, as the condition to queue them - // is that there are no queued blocks at the time a new block is queued. - const bool discard = has_blocks_queued() && TEST(block_buffer[block_buffer_tail].flag, BLOCK_BIT_CONTINUED); - if (discard) discard_current_block(); - // Reenable stepper ISR if it was enabled if (stepper_isr_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); } @@ -1467,6 +1472,16 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE] return true; } + // If this is the first added movement, reload the delay, otherwise, cancel it. + if (block_buffer_head == block_buffer_tail) { + // If it was the first queued block, restart the 1st block delivery delay, to + // give the planner an opportunity to queue more movements and plan them + // As there are no queued movements, the Stepper ISR will not touch this + // variable, so there is no risk setting this here (but it MUST be done + // before the following line!!) + delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE; + } + // Move buffer head block_buffer_head = next_buffer_head; @@ -2292,7 +2307,18 @@ void Planner::buffer_sync_block() { block->position[C_AXIS] = position[C_AXIS]; block->position[E_AXIS] = position[E_AXIS]; + // If this is the first added movement, reload the delay, otherwise, cancel it. + if (block_buffer_head == block_buffer_tail) { + // If it was the first queued block, restart the 1st block delivery delay, to + // give the planner an opportunity to queue more movements and plan them + // As there are no queued movements, the Stepper ISR will not touch this + // variable, so there is no risk setting this here (but it MUST be done + // before the following line!!) + delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE; + } + block_buffer_head = next_buffer_head; + stepper.wake_up(); } // buffer_sync_block() @@ -2370,81 +2396,8 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con SERIAL_ECHOLNPGM(")"); //*/ - // Always split the first move into two (if not homing or probing) - if (!has_blocks_queued()) { - - #define _BETWEEN(A) (position[_AXIS(A)] + target[_AXIS(A)]) >> 1 - const int32_t between[ABCE] = { _BETWEEN(A), _BETWEEN(B), _BETWEEN(C), _BETWEEN(E) }; - - #if HAS_POSITION_FLOAT - #define _BETWEEN_F(A) (position_float[_AXIS(A)] + target_float[_AXIS(A)]) * 0.5 - const float between_float[ABCE] = { _BETWEEN_F(A), _BETWEEN_F(B), _BETWEEN_F(C), _BETWEEN_F(E) }; - #endif - - // The new head value is not assigned yet - uint8_t buffer_head = 0; - bool added = false; - - uint8_t next_buffer_head; - block_t *block = get_next_free_block(next_buffer_head, 2); - - // Fill the block with the specified movement + // Queue the movement if ( - _populate_block(block, true, between - #if HAS_POSITION_FLOAT - , between_float - #endif - , fr_mm_s, extruder, millimeters * 0.5 - ) - ) { - // Movement accepted - Point to the next reserved block - block = &block_buffer[next_buffer_head]; - - // Store into the new to be stored head - buffer_head = next_buffer_head; - added = true; - - // And advance the pointer to the next unused slot - next_buffer_head = next_block_index(next_buffer_head); - } - - // Fill the second part of the block with the 2nd part of the movement - if ( - _populate_block(block, true, target - #if HAS_POSITION_FLOAT - , target_float - #endif - , fr_mm_s, extruder, millimeters * 0.5 - ) - ) { - // Movement accepted - If this block is a continuation - // of the previous one, mark it as such - if (added) SBI(block->flag, BLOCK_BIT_CONTINUED); - - // Store into the new to be stored head - buffer_head = next_buffer_head; - added = true; - } - - // If any of the movements was added - if (added) { - - // Move buffer head and add all the blocks that were filled - // successfully to the movement queue. - block_buffer_head = buffer_head; - - // Update the position (only when a move was queued) - static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!"); - COPY(position, target); - #if HAS_POSITION_FLOAT - COPY(position_float, target_float); - #endif - - // Recalculate and optimize trapezoidal speed profiles - recalculate(); - } - } - else if ( !_buffer_steps(target #if HAS_POSITION_FLOAT , target_float diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 9dc203501..34288c14d 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -54,7 +54,7 @@ enum BlockFlagBit : char { // from a safe speed (in consideration of jerking from zero speed). BLOCK_BIT_NOMINAL_LENGTH, - // The block is busy + // The block is busy, being interpreted by the stepper ISR BLOCK_BIT_BUSY, // The block is segment 2+ of a longer move @@ -176,7 +176,8 @@ class Planner { static block_t block_buffer[BLOCK_BUFFER_SIZE]; static volatile uint8_t block_buffer_head, // Index of the next block to be pushed block_buffer_tail; // Index of the busy block, if any - static int16_t cleaning_buffer_counter; // A counter to disable queuing of blocks + static uint16_t cleaning_buffer_counter; // A counter to disable queuing of blocks + static uint8_t delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks #if ENABLED(DISTINCT_E_FACTORS) static uint8_t last_extruder; // Respond to extruder change @@ -634,25 +635,40 @@ class Planner { * WARNING: Called from Stepper ISR context! */ static block_t* get_current_block() { - if (has_blocks_queued()) { - block_t * const block = &block_buffer[block_buffer_tail]; - // If the block has no trapezoid calculated, it's unsafe to execute. - if (movesplanned() > 1) { - const block_t * const next = &block_buffer[next_block_index(block_buffer_tail)]; - if (TEST(block->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE)) - return NULL; + // Get the number of moves in the planner queue so far + uint8_t nr_moves = movesplanned(); + + // If there are any moves queued ... + if (nr_moves) { + + // If there is still delay of delivery of blocks running, decrement it + if (delay_before_delivering) { + --delay_before_delivering; + // If the number of movements queued is less than 3, and there is still time + // to wait, do not deliver anything + if (nr_moves < 3 && delay_before_delivering) return NULL; + delay_before_delivering = 0; } - else if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) - return NULL; + + // If we are here, there is no excuse to deliver the block + block_t * const block = &block_buffer[block_buffer_tail]; + + // No trapezoid calculated? Don't execute yet. + if ( TEST(block->flag, BLOCK_BIT_RECALCULATE) + || (movesplanned() > 1 && TEST(block_buffer[next_block_index(block_buffer_tail)].flag, BLOCK_BIT_RECALCULATE)) + ) return NULL; #if ENABLED(ULTRA_LCD) 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. #endif + + // Mark the block as busy, so the planner does not attempt to replan it SBI(block->flag, BLOCK_BIT_BUSY); return block; } else { + // The queue became empty #if ENABLED(ULTRA_LCD) clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero. #endif