@ -104,11 +104,12 @@ Planner planner;
* A ring buffer of moves described in steps * A ring buffer of moves described in steps
*/ */
block_t Planner : : block_buffer [ BLOCK_BUFFER_SIZE ] ; 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_nonbusy , // Index of the first non-busy block
uint16_t Planner : : cleaning_buffer_counter ; // A counter to disable queuing of blocks
Planner : : block_buffer_planned , // Index of the optimally planned block
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_tail ; // Index of the busy block, if any
Planner : : block_buffer_planned ; // Index of the optimally planned block
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
uint32_t Planner : : max_acceleration_mm_per_s2 [ XYZE_N ] , // (mm/s^2) M201 XYZE
uint32_t Planner : : max_acceleration_mm_per_s2 [ XYZE_N ] , // (mm/s^2) M201 XYZE
Planner : : max_acceleration_steps_per_s2 [ XYZE_N ] , // (steps/s^2) Derived from mm_per_s2
Planner : : max_acceleration_steps_per_s2 [ XYZE_N ] , // (steps/s^2) Derived from mm_per_s2
@ -240,7 +241,6 @@ 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 ;
} }
@ -703,6 +703,12 @@ void Planner::init() {
/**
/**
* Calculate trapezoid parameters , multiplying the entry - and exit - speeds * Calculate trapezoid parameters , multiplying the entry - and exit - speeds
* by the provided factors . * by the provided factors .
* *
* # # # # # # # # # # # # VERY IMPORTANT # # # # # # # # # # # #
* NOTE that the PRECONDITION to call this function is that the block is
* NOT BUSY and it is marked as RECALCULATE . That WARRANTIES the Stepper ISR
* is not and will not use the block while we modify it , so it is safe to
* alter its values .
*/ */
void Planner : : calculate_trapezoid_for_block ( block_t * const block , const float & entry_factor , const float & exit_factor ) { void Planner : : calculate_trapezoid_for_block ( block_t * const block , const float & entry_factor , const float & exit_factor ) {
@ -744,9 +750,6 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
cruise_rate = block - > nominal_rate ; cruise_rate = block - > nominal_rate ;
# endif # endif
// block->accelerate_until = accelerate_steps;
// block->decelerate_after = accelerate_steps+plateau_steps;
# if ENABLED(S_CURVE_ACCELERATION) # if ENABLED(S_CURVE_ACCELERATION)
// Jerk controlled speed requires to express speed versus time, NOT steps
// Jerk controlled speed requires to express speed versus time, NOT steps
uint32_t acceleration_time = ( ( float ) ( cruise_rate - initial_rate ) / accel ) * ( STEPPER_TIMER_RATE ) , uint32_t acceleration_time = ( ( float ) ( cruise_rate - initial_rate ) / accel ) * ( STEPPER_TIMER_RATE ) ,
@ -755,32 +758,20 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
// And to offload calculations from the ISR, we also calculate the inverse of those times here
// And to offload calculations from the ISR, we also calculate the inverse of those times here
uint32_t acceleration_time_inverse = get_period_inverse ( acceleration_time ) ; uint32_t acceleration_time_inverse = get_period_inverse ( acceleration_time ) ;
uint32_t deceleration_time_inverse = get_period_inverse ( deceleration_time ) ; uint32_t deceleration_time_inverse = get_period_inverse ( deceleration_time ) ;
# endif # endif
// Fill variables used by the stepper in a critical section
// Store new block parameters
const bool was_enabled = STEPPER_ISR_ENABLED ( ) ; block - > accelerate_until = accelerate_steps ;
if ( was_enabled ) DISABLE_STEPPER_DRIVER_INTERRUPT ( ) ; block - > decelerate_after = accelerate_steps + plateau_steps ;
block - > initial_rate = initial_rate ;
// Don't update variables if block is busy; it is being interpreted by the planner.
# if ENABLED(S_CURVE_ACCELERATION)
// If this happens, there's a problem... The block speed is inconsistent. Some values
block - > acceleration_time = acceleration_time ;
// have already been updated, but the Stepper ISR is already using the block. Fortunately,
block - > deceleration_time = deceleration_time ;
// the values being used by the Stepper ISR weren't touched, so just stop here...
block - > acceleration_time_inverse = acceleration_time_inverse ;
// TODO: There may be a way to update a running block, depending on the stepper ISR position.
block - > deceleration_time_inverse = deceleration_time_inverse ;
if ( ! TEST ( block - > flag , BLOCK_BIT_BUSY ) ) { block - > cruise_rate = cruise_rate ;
block - > accelerate_until = accelerate_steps ; # endif
block - > decelerate_after = accelerate_steps + plateau_steps ; block - > final_rate = final_rate ;
block - > initial_rate = initial_rate ;
# if ENABLED(S_CURVE_ACCELERATION)
block - > acceleration_time = acceleration_time ;
block - > deceleration_time = deceleration_time ;
block - > acceleration_time_inverse = acceleration_time_inverse ;
block - > deceleration_time_inverse = deceleration_time_inverse ;
block - > cruise_rate = cruise_rate ;
# endif
block - > final_rate = final_rate ;
}
if ( was_enabled ) ENABLE_STEPPER_DRIVER_INTERRUPT ( ) ;
} }
/* PLANNER SPEED DEFINITION
/* PLANNER SPEED DEFINITION
@ -831,7 +822,7 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
streaming operating conditions . Use for planning optimizations by avoiding recomputing parts of the 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 , 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 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 . this requirement when encountered by the Planner : : 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 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 line segments , like G2 / 3 arcs or complex curves , may seem to move slow . This is because there simply isn ' t
@ -875,8 +866,19 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t * const
// ISR does not consume the block before being recalculated
// ISR does not consume the block before being recalculated
SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ; SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
// Set the new entry speed
// But there is an inherent race condition here, as the block may have
current - > entry_speed_sqr = new_entry_speed_sqr ; // become BUSY just before being marked RECALCULATE, so check for that!
if ( stepper . is_block_busy ( current ) ) {
// Block became busy. Clear the RECALCULATE flag (no point in
// recalculating BUSY blocks). And don't set its speed, as it can't
// be updated at this time.
CBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
}
else {
// Block is not BUSY so this is ahead of the Stepper ISR:
// Just Set the new entry speed.
current - > entry_speed_sqr = new_entry_speed_sqr ;
}
} }
} }
} }
@ -902,12 +904,11 @@ void Planner::reverse_pass() {
// Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
// 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.
// 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.
// NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
block_t * current ;
const block_t * next = NULL ; const block_t * next = NULL ;
while ( block_index ! = planned_block_index ) { while ( block_index ! = planned_block_index ) {
// Perform the reverse pass
// Perform the reverse pass
current = & block_buffer [ block_index ] ; block_t * current = & block_buffer [ block_index ] ;
// Only consider non sync blocks
// Only consider non sync blocks
if ( ! TEST ( current - > flag , BLOCK_BIT_SYNC_POSITION ) ) { if ( ! TEST ( current - > flag , BLOCK_BIT_SYNC_POSITION ) ) {
@ -917,6 +918,18 @@ void Planner::reverse_pass() {
// Advance to the next
// Advance to the next
block_index = prev_block_index ( block_index ) ; block_index = prev_block_index ( block_index ) ;
// The ISR could advance the block_buffer_planned while we were doing the reverse pass.
// We must try to avoid using an already consumed block as the last one - So follow
// changes to the pointer and make sure to limit the loop to the currently busy block
while ( planned_block_index ! = block_buffer_planned ) {
// If we reached the busy block or an already processed block, break the loop now
if ( block_index = = planned_block_index ) return ;
// Advance the pointer, following the busy block
planned_block_index = next_block_index ( planned_block_index ) ;
}
} }
} }
@ -940,11 +953,24 @@ void Planner::forward_pass_kernel(const block_t* const previous, block_t* const
// so the stepper ISR does not consume the block before being recalculated
// so the stepper ISR does not consume the block before being recalculated
SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ; SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
// Always <= max_entry_speed_sqr. Backward pass sets this.
// But there is an inherent race condition here, as the block maybe
current - > entry_speed_sqr = new_entry_speed_sqr ; // Always <= max_entry_speed_sqr. Backward pass sets this.
// became BUSY, just before it was marked as RECALCULATE, so check
// if that is the case!
if ( stepper . is_block_busy ( current ) ) {
// Block became busy. Clear the RECALCULATE flag (no point in
// recalculating BUSY blocks and don't set its speed, as it can't
// be updated at this time.
CBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
}
else {
// Block is not BUSY, we won the race against the Stepper ISR:
// Always <= max_entry_speed_sqr. Backward pass sets this.
current - > entry_speed_sqr = new_entry_speed_sqr ; // Always <= max_entry_speed_sqr. Backward pass sets this.
// Set optimal plan pointer.
// Set optimal plan pointer.
block_buffer_planned = block_index ; block_buffer_planned = block_index ;
}
} }
} }
@ -981,7 +1007,13 @@ void Planner::forward_pass() {
// Skip SYNC blocks
// Skip SYNC blocks
if ( ! TEST ( current - > flag , BLOCK_BIT_SYNC_POSITION ) ) { if ( ! TEST ( current - > flag , BLOCK_BIT_SYNC_POSITION ) ) {
forward_pass_kernel ( previous , current , block_index ) ; // If there's no previous block or the previous block is not
// BUSY (thus, modifiable) run the forward_pass_kernel. Otherwise,
// the previous block became BUSY, so assume the current block's
// entry speed can't be altered (since that would also require
// updating the exit speed of the previous block).
if ( ! previous | | ! stepper . is_block_busy ( previous ) )
forward_pass_kernel ( previous , current , block_index ) ;
previous = current ; previous = current ;
} }
// Advance to the previous
// Advance to the previous
@ -996,16 +1028,15 @@ void Planner::forward_pass() {
*/ */
void Planner : : recalculate_trapezoids ( ) { void Planner : : recalculate_trapezoids ( ) {
// The tail may be changed by the ISR so get a local copy.
// 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 ,
head_block_index = block_buffer_head ;
// As there could be a sync block in the head of the queue, and the next loop must not
// Since there could be a sync block in the head of the queue, and the
// recalculate the head block (as it needs to be specially handled), scan backwards until
// next loop must not recalculate the head block (as it needs to be
// we find the first non SYNC block
// specially handled), scan backwards to the first non-SYNC block.
uint8_t head_block_index = block_buffer_head ;
while ( head_block_index ! = block_index ) { while ( head_block_index ! = block_index ) {
// Go back (head always point to the first free block)
// Go back (head always point to the first free block)
uint8_t prev_index = prev_block_index ( head_block_index ) ; const uint8_t prev_index = prev_block_index ( head_block_index ) ;
// Get the pointer to the block
// Get the pointer to the block
block_t * prev = & block_buffer [ prev_index ] ; block_t * prev = & block_buffer [ prev_index ] ;
@ -1015,7 +1046,7 @@ void Planner::recalculate_trapezoids() {
// Examine the previous block. This and all following are SYNC blocks
// Examine the previous block. This and all following are SYNC blocks
head_block_index = prev_index ; head_block_index = prev_index ;
} ; }
// Go from the tail (currently executed block) to the first block, without including it)
// Go from the tail (currently executed block) to the first block, without including it)
block_t * current = NULL , * next = NULL ; block_t * current = NULL , * next = NULL ;
@ -1037,17 +1068,24 @@ void Planner::recalculate_trapezoids() {
// RECALCULATE yet, but the next one is. That's the reason for the following line.
// RECALCULATE yet, but the next one is. That's the reason for the following line.
SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ; SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
// But there is an inherent race condition here, as the block maybe
const float current_nominal_speed = SQRT ( current - > nominal_speed_sqr ) , // became BUSY, just before it was marked as RECALCULATE, so check
nomr = 1.0 / current_nominal_speed ; // if that is the case!
calculate_trapezoid_for_block ( current , current_entry_speed * nomr , next_entry_speed * nomr ) ; if ( ! stepper . is_block_busy ( current ) ) {
# if ENABLED(LIN_ADVANCE) // Block is not BUSY, we won the race against the Stepper ISR:
if ( current - > use_advance_lead ) {
const float comp = current - > e_D_ratio * extruder_advance_K * axis_steps_per_mm [ E_AXIS ] ; // NOTE: Entry and exit factors always > 0 by all previous logic operations.
current - > max_adv_steps = current_nominal_speed * comp ; const float current_nominal_speed = SQRT ( current - > nominal_speed_sqr ) ,
current - > final_adv_steps = next_entry_speed * comp ; nomr = 1.0 / current_nominal_speed ;
} calculate_trapezoid_for_block ( current , current_entry_speed * nomr , next_entry_speed * nomr ) ;
# endif # if ENABLED(LIN_ADVANCE)
if ( current - > use_advance_lead ) {
const float comp = current - > e_D_ratio * extruder_advance_K * axis_steps_per_mm [ E_AXIS ] ;
current - > max_adv_steps = current_nominal_speed * comp ;
current - > final_adv_steps = next_entry_speed * comp ;
}
# endif
}
// Reset current only to ensure next trapezoid is computed - The
// Reset current only to ensure next trapezoid is computed - The
// stepper is free to use the block from now on.
// stepper is free to use the block from now on.
@ -1070,16 +1108,23 @@ void Planner::recalculate_trapezoids() {
// marked as RECALCULATE yet. That's the reason for the following line.
// marked as RECALCULATE yet. That's the reason for the following line.
SBI ( next - > flag , BLOCK_BIT_RECALCULATE ) ; SBI ( next - > flag , BLOCK_BIT_RECALCULATE ) ;
const float next_nominal_speed = SQRT ( next - > nominal_speed_sqr ) , // But there is an inherent race condition here, as the block maybe
nomr = 1.0 / next_nominal_speed ; // became BUSY, just before it was marked as RECALCULATE, so check
calculate_trapezoid_for_block ( next , next_entry_speed * nomr , ( MINIMUM_PLANNER_SPEED ) * nomr ) ; // if that is the case!
# if ENABLED(LIN_ADVANCE) if ( ! stepper . is_block_busy ( current ) ) {
if ( next - > use_advance_lead ) { // Block is not BUSY, we won the race against the Stepper ISR:
const float comp = next - > e_D_ratio * extruder_advance_K * axis_steps_per_mm [ E_AXIS ] ;
next - > max_adv_steps = next_nominal_speed * comp ; const float next_nominal_speed = SQRT ( next - > nominal_speed_sqr ) ,
next - > final_adv_steps = ( MINIMUM_PLANNER_SPEED ) * comp ; nomr = 1.0 / next_nominal_speed ;
} calculate_trapezoid_for_block ( next , next_entry_speed * nomr , ( MINIMUM_PLANNER_SPEED ) * nomr ) ;
# endif # if ENABLED(LIN_ADVANCE)
if ( next - > use_advance_lead ) {
const float comp = next - > e_D_ratio * extruder_advance_K * axis_steps_per_mm [ E_AXIS ] ;
next - > max_adv_steps = next_nominal_speed * comp ;
next - > final_adv_steps = ( MINIMUM_PLANNER_SPEED ) * comp ;
}
# endif
}
// Reset next only to ensure its trapezoid is computed - The stepper is free to use
// Reset next only to ensure its trapezoid is computed - The stepper is free to use
// the block from now on.
// the block from now on.
@ -1423,7 +1468,7 @@ void Planner::quick_stop() {
if ( was_enabled ) DISABLE_STEPPER_DRIVER_INTERRUPT ( ) ; if ( was_enabled ) DISABLE_STEPPER_DRIVER_INTERRUPT ( ) ;
// Drop all queue entries
// Drop all queue entries
block_buffer_planned = block_buffer_head = block_buffer_tail ; block_buffer_nonbusy = 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.
@ -1906,7 +1951,8 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
// Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
float inverse_secs = fr_mm_s * inverse_millimeters ; float inverse_secs = fr_mm_s * inverse_millimeters ;
const uint8_t moves_queued = movesplanned ( ) ; // Get the number of non busy movements in queue (non busy means that they can be altered)
const uint8_t moves_queued = nonbusy_movesplanned ( ) ;
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
# if ENABLED(SLOWDOWN) || ENABLED(ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT) # if ENABLED(SLOWDOWN) || ENABLED(ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)
Eduardo José Tagle
6 years ago
Scott Lahteine