@ -107,7 +107,8 @@ block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
volatile uint8_t Planner : : block_buffer_head , // Index of the next block to be pushed
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
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
uint8_t Planner : : delay_before_delivering , // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
Planner : : block_buffer_planned ; // Index of the optimally planned block
float Planner : : max_feedrate_mm_s [ XYZE_N ] , // Max speeds in mm per second
float Planner : : max_feedrate_mm_s [ XYZE_N ] , // Max speeds in mm per second
Planner : : axis_steps_per_mm [ XYZE_N ] ,
Planner : : axis_steps_per_mm [ XYZE_N ] ,
@ -227,6 +228,7 @@ void Planner::init() {
bed_level_matrix . set_to_identity ( ) ;
bed_level_matrix . set_to_identity ( ) ;
# endif
# endif
clear_block_buffer ( ) ;
clear_block_buffer ( ) ;
block_buffer_planned = 0 ;
delay_before_delivering = 0 ;
delay_before_delivering = 0 ;
} }
@ -825,6 +827,68 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
if ( was_enabled ) ENABLE_STEPPER_DRIVER_INTERRUPT ( ) ;
if ( was_enabled ) ENABLE_STEPPER_DRIVER_INTERRUPT ( ) ;
} }
/* PLANNER SPEED DEFINITION
+ - - - - - - - - + < - current - > nominal_speed
/ \
current - > entry_speed - > + \
| + < - next - > entry_speed ( aka exit speed )
+ - - - - - - - - - - - - - +
time - - >
Recalculates the motion plan according to the following basic guidelines :
1. Go over every feasible block sequentially in reverse order and calculate the junction speeds
( i . e . current - > entry_speed ) such that :
a . No junction speed exceeds the pre - computed maximum junction speed limit or nominal speeds of
neighboring blocks .
b . A block entry speed cannot exceed one reverse - computed from its exit speed ( next - > entry_speed )
with a maximum allowable deceleration over the block travel distance .
c . The last ( or newest appended ) block is planned from a complete stop ( an exit speed of zero ) .
2. Go over every block in chronological ( forward ) order and dial down junction speed values if
a . The exit speed exceeds the one forward - computed from its entry speed with the maximum allowable
acceleration over the block travel distance .
When these stages are complete , the planner will have maximized the velocity profiles throughout the all
of the planner blocks , where every block is operating at its maximum allowable acceleration limits . In
other words , for all of the blocks in the planner , the plan is optimal and no further speed improvements
are possible . If a new block is added to the buffer , the plan is recomputed according to the said
guidelines for a new optimal plan .
To increase computational efficiency of these guidelines , a set of planner block pointers have been
created to indicate stop - compute points for when the planner guidelines cannot logically make any further
changes or improvements to the plan when in normal operation and new blocks are streamed and added to the
planner buffer . For example , if a subset of sequential blocks in the planner have been planned and are
bracketed by junction velocities at their maximums ( or by the first planner block as well ) , no new block
added to the planner buffer will alter the velocity profiles within them . So we no longer have to compute
them . Or , if a set of sequential blocks from the first block in the planner ( or a optimal stop - compute
point ) are all accelerating , they are all optimal and can not be altered by a new block added to the
planner buffer , as this will only further increase the plan speed to chronological blocks until a maximum
junction velocity is reached . However , if the operational conditions of the plan changes from infrequently
used feed holds or feedrate overrides , the stop - compute pointers will be reset and the entire plan is
recomputed as stated in the general guidelines .
Planner buffer index mapping :
- block_buffer_tail : Points to the beginning of the planner buffer . First to be executed or being executed .
- block_buffer_head : Points to the buffer block after the last block in the buffer . Used to indicate whether
the buffer is full or empty . As described for standard ring buffers , this block is always empty .
- block_buffer_planned : Points to the first buffer block after the last optimally planned block for normal
streaming operating conditions . Use for planning optimizations by avoiding recomputing parts of the
planner buffer that don ' t change with the addition of a new block , as describe above . In addition ,
this block can never be less than block_buffer_tail and will always be pushed forward and maintain
this requirement when encountered by the plan_discard_current_block ( ) routine during a cycle .
NOTE : Since the planner only computes on what ' s in the planner buffer , some motions with lots of short
line segments , like G2 / 3 arcs or complex curves , may seem to move slow . This is because there simply isn ' t
enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then
decelerate to a complete stop at the end of the buffer , as stated by the guidelines . If this happens and
becomes an annoyance , there are a few simple solutions : ( 1 ) Maximize the machine acceleration . The planner
will be able to compute higher velocity profiles within the same combined distance . ( 2 ) Maximize line
motion ( s ) distance per block to a desired tolerance . The more combined distance the planner has to use ,
the faster it can go . ( 3 ) Maximize the planner buffer size . This also will increase the combined distance
for the planner to compute over . It also increases the number of computations the planner has to perform
to compute an optimal plan , so select carefully .
*/
// The kernel called by recalculate() when scanning the plan from last to first entry.
// The kernel called by recalculate() when scanning the plan from last to first entry.
void Planner : : reverse_pass_kernel ( block_t * const current , const block_t * const next ) { void Planner : : reverse_pass_kernel ( block_t * const current , const block_t * const next ) {
if ( current ) {
if ( current ) {
@ -851,6 +915,8 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t * const
: MIN ( max_entry_speed_sqr , max_allowable_speed_sqr ( - current - > acceleration , next ? next - > entry_speed_sqr : sq ( MINIMUM_PLANNER_SPEED ) , current - > millimeters ) ) ;
: MIN ( max_entry_speed_sqr , max_allowable_speed_sqr ( - current - > acceleration , next ? next - > entry_speed_sqr : sq ( MINIMUM_PLANNER_SPEED ) , current - > millimeters ) ) ;
if ( current - > entry_speed_sqr ! = new_entry_speed_sqr ) {
if ( current - > entry_speed_sqr ! = new_entry_speed_sqr ) {
current - > entry_speed_sqr = new_entry_speed_sqr ;
current - > entry_speed_sqr = new_entry_speed_sqr ;
// Need to recalculate the block speed
SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
}
}
}
}
@ -862,44 +928,72 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t * const
* Once in reverse and once forward . This implements the reverse pass .
* Once in reverse and once forward . This implements the reverse pass .
*/
*/
void Planner : : reverse_pass ( ) { void Planner : : reverse_pass ( ) {
if ( movesplanned ( ) > 2 ) {
// Initialize block index to the last block in the planner buffer.
const uint8_t endnr = next_block_index ( block_buffer_tail ) ; // tail is running. tail+1 shouldn't be altered because it's connected to the running block.
uint8_t block_index = prev_block_index ( block_buffer_head ) ;
uint8_t blocknr = prev_block_index ( block_buffer_head ) ;
// Read the index of the last buffer planned block.
// The ISR may change it so get a stable local copy.
uint8_t planned_block_index = block_buffer_planned ;
// If there was a race condition and block_buffer_planned was incremented
// or was pointing at the head (queue empty) break loop now and avoid
// planning already consumed blocks
if ( planned_block_index = = block_buffer_head ) return ;
// Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
// block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
// NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
block_t * current ;
const block_t * next = NULL ;
while ( block_index ! = planned_block_index ) {
// Perform the reverse pass
// Perform the reverse pass
block_t * current , * next = NULL ;
current = & block_buffer [ block_index ] ;
while ( blocknr ! = endnr ) {
// Perform the reverse pass - Only consider non sync blocks
// Only consider non sync blocks
current = & block_buffer [ blocknr ] ;
if ( ! TEST ( current - > flag , BLOCK_BIT_SYNC_POSITION ) ) {
if ( ! TEST ( current - > flag , BLOCK_BIT_SYNC_POSITION ) ) {
reverse_pass_kernel ( current , next ) ;
reverse_pass_kernel ( current , next ) ;
next = current ;
next = current ;
}
}
// Advance to the next
// Advance to the next
blocknr = prev_block_index ( blocknr ) ;
block_index = prev_block_index ( block_index ) ;
}
}
}
} }
// The kernel called by recalculate() when scanning the plan from first to last entry.
// The kernel called by recalculate() when scanning the plan from first to last entry.
void Planner : : forward_pass_kernel ( const block_t * const previous , block_t * const current { void Planner : : forward_pass_kernel ( const block_t * const previous , block_t * const current , uint8_t block_index ) {
if ( previous ) {
if ( previous ) {
// If the previous block is an acceleration block, too short to complete the full speed
// If the previous block is an acceleration block, too short to complete the full speed
// change, adjust the entry speed accordingly. Entry speeds have already been reset,
// change, adjust the entry speed accordingly. Entry speeds have already been reset,
// maximized, and reverse-planned. If nominal length is set, max junction speed is
// maximized, and reverse-planned. If nominal length is set, max junction speed is
// guaranteed to be reached. No need to recheck.
// guaranteed to be reached. No need to recheck.
if ( ! TEST ( previous - > flag , BLOCK_BIT_NOMINAL_LENGTH ) ) {
if ( ! TEST ( previous - > flag , BLOCK_BIT_NOMINAL_LENGTH ) & &
if ( previous - > entry_speed_sqr < current - > entry_speed_sqr ) {
previous - > entry_speed_sqr < current - > entry_speed_sqr ) {
// Compute the maximum allowable speed
// Compute the maximum allowable speed
const float new_entry_speed_sqr = max_allowable_speed_sqr ( - previous - > acceleration , previous - > entry_speed_sqr , previous - > millimeters ) ;
const float new_entry_speed_sqr = max_allowable_speed_sqr ( - previous - > acceleration , previous - > entry_speed_sqr , previous - > millimeters ) ;
// If true, current block is full-acceleration
if ( current - > entry_speed_sqr > new_entry_speed_sqr ) {
// If true, current block is full-acceleration and we can move the planned pointer forward.
if ( new_entry_speed_sqr < current - > entry_speed_sqr ) {
// Always <= max_entry_speed_sqr. Backward pass sets this.
// Always <= max_entry_speed_sqr. Backward pass sets this.
current - > entry_speed_sqr = new_entry_speed_sqr ;
current - > entry_speed_sqr = new_entry_speed_sqr ; // Always <= max_entry_speed_sqr. Backward pass sets this.
// Set optimal plan pointer.
block_buffer_planned = block_index ;
// And mark we need to recompute the trapezoidal shape
SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
}
}
}
}
}
// Any block set at its maximum entry speed also creates an optimal plan up to this
// point in the buffer. When the plan is bracketed by either the beginning of the
// buffer and a maximum entry speed or two maximum entry speeds, every block in between
// cannot logically be further improved. Hence, we don't have to recompute them anymore.
if ( current - > entry_speed_sqr = = current - > max_entry_speed_sqr )
block_buffer_planned = block_index ;
}
}
} }
@ -908,20 +1002,30 @@ void Planner::forward_pass_kernel(const block_t * const previous, block_t* const
* Once in reverse and once forward . This implements the forward pass .
* Once in reverse and once forward . This implements the forward pass .
*/
*/
void Planner : : forward_pass ( ) { void Planner : : forward_pass ( ) {
const uint8_t endnr = block_buffer_head ;
uint8_t blocknr = block_buffer_tail ;
// Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
// Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
// Begin at buffer planned pointer. Note that block_buffer_planned can be modified
// by the stepper ISR, so read it ONCE. It it guaranteed that block_buffer_planned
// will never lead head, so the loop is safe to execute. Also note that the forward
// pass will never modify the values at the tail.
uint8_t block_index = block_buffer_planned ;
block_t * current ;
const block_t * previous = NULL ;
while ( block_index ! = block_buffer_head ) {
// Perform the forward pass
// Perform the forward pass
block_t * current , * previous = NULL ;
current = & block_buffer [ block_index ] ;
while ( blocknr ! = endnr ) {
// Perform the forward pass - Only consider non-sync blocks
// Skip SYNC blocks
current = & block_buffer [ blocknr ] ;
if ( ! TEST ( current - > flag , BLOCK_BIT_SYNC_POSITION ) ) {
if ( ! TEST ( current - > flag , BLOCK_BIT_SYNC_POSITION ) ) {
forward_pass_kernel ( previous , current ) ;
forward_pass_kernel ( previous , current , block_index );
previous = current ;
previous = current ;
}
}
// Advance to the previous
// Advance to the previous
block nr = next_block_index ( blocknr ) ;
block _index = next_block_index ( block_index ) ;
}
}
} }
@ -931,6 +1035,7 @@ void Planner::forward_pass() {
* recalculate ( ) after updating the blocks .
* recalculate ( ) after updating the blocks .
*/
*/
void Planner : : recalculate_trapezoids ( ) { void Planner : : recalculate_trapezoids ( ) {
// The tail may be changed by the ISR so get a local copy.
uint8_t block_index = block_buffer_tail ;
uint8_t block_index = block_buffer_tail ;
// As there could be a sync block in the head of the queue, and the next loop must not
// As there could be a sync block in the head of the queue, and the next loop must not
@ -1004,33 +1109,14 @@ void Planner::recalculate_trapezoids() {
}
}
} }
/**
* Recalculate the motion plan according to the following algorithm :
*
* 1. Go over every block in reverse order . . .
*
* Calculate a junction speed reduction ( block_t . entry_factor ) so :
*
* a . The junction jerk is within the set limit , and
*
* b . No speed reduction within one block requires faster
* deceleration than the one , true constant acceleration .
*
* 2. Go over every block in chronological order . . .
*
* Dial down junction speed reduction values if :
* a . The speed increase within one block would require faster
* acceleration than the one , true constant acceleration .
*
* After that , all blocks will have an entry_factor allowing all speed changes to
* be performed using only the one , true constant acceleration , and where no junction
* jerk is jerkier than the set limit , Jerky . Finally it will :
*
* 3. Recalculate " trapezoids " for all blocks .
*/
void Planner : : recalculate ( ) { void Planner : : recalculate ( ) {
// Initialize block index to the last block in the planner buffer.
const uint8_t block_index = prev_block_index ( block_buffer_head ) ;
// If there is just one block, no planning can be done. Avoid it!
if ( block_index ! = block_buffer_planned ) {
reverse_pass ( ) ;
reverse_pass ( ) ;
forward_pass ( ) ;
forward_pass ( ) ;
}
recalculate_trapezoids ( ) ;
recalculate_trapezoids ( ) ;
} }
@ -1348,10 +1434,18 @@ void Planner::check_axes_activity() {
# endif // PLANNER_LEVELING
# endif // PLANNER_LEVELING
void Planner : : quick_stop ( ) { void Planner : : quick_stop ( ) {
// Remove all the queued blocks. Note that this function is NOT
// Remove all the queued blocks. Note that this function is NOT
// called from the Stepper ISR, so we must consider tail as readonly!
// called from the Stepper ISR, so we must consider tail as readonly!
// that is why we set head to tail!
// that is why we set head to tail - But there is a race condition that
block_buffer_head = block_buffer_tail ;
// must be handled: The tail could change between the read and the assignment
// so this must be enclosed in a critical section
const bool was_enabled = STEPPER_ISR_ENABLED ( ) ;
if ( was_enabled ) DISABLE_STEPPER_DRIVER_INTERRUPT ( ) ;
// Drop all queue entries
block_buffer_planned = block_buffer_head = block_buffer_tail ;
// Restart the block delay for the first movement - As the queue was
// Restart the block delay for the first movement - As the queue was
// forced to empty, there's no risk the ISR will touch this.
// forced to empty, there's no risk the ISR will touch this.
@ -1365,6 +1459,9 @@ void Planner::quick_stop() {
// Make sure to drop any attempt of queuing moves for at least 1 second
// Make sure to drop any attempt of queuing moves for at least 1 second
cleaning_buffer_counter = 1000 ;
cleaning_buffer_counter = 1000 ;
// Reenable Stepper ISR
if ( was_enabled ) ENABLE_STEPPER_DRIVER_INTERRUPT ( ) ;
// And stop the stepper ISR
// And stop the stepper ISR
stepper . quick_stop ( ) ;
stepper . quick_stop ( ) ;
} }
etagle
7 years ago
committed by