MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Planner block HOLD flag

Browse Source 
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.
This commit is contained in:
etagle 2018-05-13 00:49:54 -03:00 committed by Scott Lahteine
parent a11eb50a3e
commit e0ca627033
2 changed files with 62 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
Marlin/src/module/planner.cpp
View File

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

38
Marlin/src/module/planner.h
View File

@ -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()) {
// 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;
}
// If we are here, there is no excuse to deliver the block
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;
}
else if (TEST(block->flag, BLOCK_BIT_RECALCULATE))
return NULL;
// 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