@ -83,6 +83,8 @@ unsigned long axis_steps_per_sqr_second[NUM_AXIS];
// The current position of the tool in absolute steps
// The current position of the tool in absolute steps
long position [ 4 ] ; //rescaled from extern when axis_steps_per_unit are changed by gcode
long position [ 4 ] ; //rescaled from extern when axis_steps_per_unit are changed by gcode
static float previous_speed [ 4 ] ; // Speed of previous path line segment
static float previous_nominal_speed ; // Nominal speed of previous path line segment
//===========================================================================
//===========================================================================
@ -92,12 +94,30 @@ static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for
static volatile unsigned char block_buffer_head ; // Index of the next block to be pushed
static volatile unsigned char block_buffer_head ; // Index of the next block to be pushed
static volatile unsigned char block_buffer_tail ; // Index of the block to process now
static volatile unsigned char block_buffer_tail ; // Index of the block to process now
// Used for the frequency limit
static unsigned char old_direction_bits = 0 ; // Old direction bits. Used for speed calculations
static long x_segment_time [ 3 ] = { 0 , 0 , 0 } ; // Segment times (in us). Used for speed calculations
static long y_segment_time [ 3 ] = { 0 , 0 , 0 } ;
// Returns the index of the next block in the ring buffer
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
static int8_t next_block_index ( int8_t block_index ) {
block_index + + ;
if ( block_index = = BLOCK_BUFFER_SIZE ) { block_index = 0 ; }
return ( block_index ) ;
}
// Returns the index of the previous block in the ring buffer
static int8_t prev_block_index ( int8_t block_index ) {
if ( block_index = = 0 ) { block_index = BLOCK_BUFFER_SIZE ; }
block_index - - ;
return ( block_index ) ;
}
//===========================================================================
//===========================================================================
//=============================functions ============================
//=============================functions ============================
//===========================================================================
//===========================================================================
# define ONE_MINUTE_OF_MICROSECONDS 60000000.0
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// given acceleration:
// given acceleration:
@ -128,43 +148,46 @@ inline float intersection_distance(float initial_rate, float final_rate, float a
// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
void calculate_trapezoid_for_block ( block_t * block , float entry_speed , float exit_speed ) { void calculate_trapezoid_for_block ( block_t * block , float entry_factor , float exit_factor ) {
if ( block - > busy = = true ) return ; // If block is busy then bail out.
long initial_rate = ceil ( block - > nominal_rate * entry_factor ) ; // (step/min)
float entry_factor = entry_speed / block - > nominal_speed ; long final_rate = ceil ( block - > nominal_rate * exit_factor ) ; // (step/min)
float exit_factor = exit_speed / block - > nominal_speed ;
long initial_rate = ceil ( block - > nominal_rate * entry_factor ) ;
long final_rate = ceil ( block - > nominal_rate * exit_factor ) ;
# ifdef ADVANCE
long initial_advance = block - > advance * entry_factor * entry_factor ;
long final_advance = block - > advance * exit_factor * exit_factor ;
# endif // ADVANCE
// Limit minimal step rate (Otherwise the timer will overflow.)
// Limit minimal step rate (Otherwise the timer will overflow.)
if ( initial_rate < 120 ) initial_rate = 120 ; if ( initial_rate < 120 ) { initial_rate = 120 ; }
if ( final_rate < 120 ) final_rate = 120 ; if ( final_rate < 120 ) { final_rate = 120 ; }
// Calculate the acceleration steps
long acceleration = block - > acceleration_st ; long acceleration = block - > acceleration_st ;
long accelerate_steps = estimate_acceleration_distance ( initial_rate , block - > nominal_rate , acceleration ) ; int32_t accelerate_steps =
long decelerate_steps = estimate_acceleration_distance ( final_rate , block - > nominal_rate , acceleration ) ; ceil ( estimate_acceleration_distance ( block - > initial_rate , block - > nominal_rate , acceleration ) ) ;
int32_t decelerate_steps =
floor ( estimate_acceleration_distance ( block - > nominal_rate , block - > final_rate , - acceleration ) ) ;
// Calculate the size of Plateau of Nominal Rate.
// Calculate the size of Plateau of Nominal Rate.
long plateau_steps = block - > step_event_count - accelerate_steps - decelerate_steps ; int32_t plateau_steps = block - > step_event_count - accelerate_steps - decelerate_steps ;
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
// have to use intersection_distance() to calculate when to abort acceleration and start braking
// have to use intersection_distance() to calculate when to abort acceleration and start braking
// in order to reach the final_rate exactly at the end of this block.
// in order to reach the final_rate exactly at the end of this block.
if ( plateau_steps < 0 ) { if ( plateau_steps < 0 ) {
accelerate_steps = intersection_distance ( initial_rate , final_rate , acceleration , block - > step_event_count ) ; accelerate_steps = ceil (
intersection_distance ( block - > initial_rate , block - > final_rate , acceleration , block - > step_event_count ) ) ;
accelerate_steps = max ( accelerate_steps , 0 ) ; // Check limits due to numerical round-off
accelerate_steps = min ( accelerate_steps , block - > step_event_count ) ;
plateau_steps = 0 ; plateau_steps = 0 ;
} }
long decelerate_after = accelerate_steps + plateau_steps ; # ifdef ADVANCE
long initial_advance = block - > advance * entry_factor * entry_factor ;
long final_advance = block - > advance * exit_factor * exit_factor ;
# endif // ADVANCE
// block->accelerate_until = accelerate_steps;
// block->decelerate_after = accelerate_steps+plateau_steps;
CRITICAL_SECTION_START ; // Fill variables used by the stepper in a critical section
CRITICAL_SECTION_START ; // Fill variables used by the stepper in a critical section
if ( block - > busy = = false ) { // Don't update variables if block is busy.
if ( block - > busy = = false ) { // Don't update variables if block is busy.
block - > accelerate_until = accelerate_steps ; block - > accelerate_until = accelerate_steps ;
block - > decelerate_after = decelerate_after ; block - > decelerate_after = accelerate_steps + plateau_steps ;
block - > initial_rate = initial_rate ; block - > initial_rate = initial_rate ;
block - > final_rate = final_rate ; block - > final_rate = final_rate ;
# ifdef ADVANCE # ifdef ADVANCE
@ -178,71 +201,40 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
// acceleration within the allotted distance.
// acceleration within the allotted distance.
inline float max_allowable_speed ( float acceleration , float target_velocity , float distance ) { inline float max_allowable_speed ( float acceleration , float target_velocity , float distance ) {
return sqrt ( target_velocity * target_velocity - 2 * acceleration * 60 * 60 * distance ) ; return sqrt ( target_velocity * target_velocity - 2 * acceleration * distance ) ;
} }
// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
// This method will calculate the junction jerk as the euclidean distance between the nominal
// This method will calculate the junction jerk as the euclidean distance between the nominal
// velocities of the respective blocks.
// velocities of the respective blocks.
inline float junction_jerk ( block_t * before , block_t * after ) { //inline float junction_jerk(block_t *before, block_t *after) {
return sqrt ( // return sqrt(
pow ( ( before - > speed_x - after - > speed_x ) , 2 ) + pow ( ( before - > speed_y - after - > speed_y ) , 2 ) ) ; // pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2));
} //}
// Return the safe speed which is max_jerk/2, e.g. the
// speed under which you cannot exceed max_jerk no matter what you do.
float safe_speed ( block_t * block ) {
float safe_speed ;
safe_speed = max_xy_jerk / 2 ;
if ( abs ( block - > speed_z ) > max_z_jerk / 2 )
safe_speed = max_z_jerk / 2 ;
if ( safe_speed > block - > nominal_speed )
safe_speed = block - > nominal_speed ;
return safe_speed ;
}
// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
void planner_reverse_pass_kernel ( block_t * previous , block_t * current , block_t * next ) { void planner_reverse_pass_kernel ( block_t * previous , block_t * current , block_t * next ) {
if ( ! current ) { if ( ! current ) { return ; }
return ;
} if ( next ) {
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
float entry_speed = current - > nominal_speed ; // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
float exit_factor ; // check for maximum allowable speed reductions to ensure maximum possible planned speed.
float exit_speed ; if ( current - > entry_speed ! = current - > max_entry_speed ) {
if ( next ) {
exit_speed = next - > entry_speed ; // If nominal length true, max junction speed is guaranteed to be reached. Only compute
} // for max allowable speed if block is decelerating and nominal length is false.
else { if ( ( ! current - > nominal_length_flag ) & & ( current - > max_entry_speed > next - > entry_speed ) ) {
exit_speed = safe_speed ( current ) ; current - > entry_speed = min ( current - > max_entry_speed ,
} max_allowable_speed ( - current - > acceleration , next - > entry_speed , current - > millimeters ) ) ;
} else {
// Calculate the entry_factor for the current block.
current - > entry_speed = current - > max_entry_speed ;
if ( previous ) {
// Reduce speed so that junction_jerk is within the maximum allowed
float jerk = junction_jerk ( previous , current ) ;
if ( ( previous - > steps_x = = 0 ) & & ( previous - > steps_y = = 0 ) ) {
entry_speed = safe_speed ( current ) ;
}
else if ( jerk > max_xy_jerk ) {
entry_speed = ( max_xy_jerk / jerk ) * entry_speed ;
}
if ( abs ( previous - > speed_z - current - > speed_z ) > max_z_jerk ) {
entry_speed = ( max_z_jerk / abs ( previous - > speed_z - current - > speed_z ) ) * entry_speed ;
}
// If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
if ( entry_speed > exit_speed ) {
float max_entry_speed = max_allowable_speed ( - current - > acceleration , exit_speed , current - > millimeters ) ;
if ( max_entry_speed < entry_speed ) {
entry_speed = max_entry_speed ;
} }
current - > recalculate_flag = true ;
} }
} } // Skip last block. Already initialized and set for recalculation.
else {
entry_speed = safe_speed ( current ) ;
}
// Store result
current - > entry_speed = entry_speed ;
} }
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
@ -251,33 +243,34 @@ void planner_reverse_pass() {
char block_index = block_buffer_head ; char block_index = block_buffer_head ;
if ( ( ( block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE ) & ( BLOCK_BUFFER_SIZE - 1 ) ) > 3 ) { if ( ( ( block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE ) & ( BLOCK_BUFFER_SIZE - 1 ) ) > 3 ) {
block_index = ( block_buffer_head - 3 ) & ( BLOCK_BUFFER_SIZE - 1 ) ; block_index = ( block_buffer_head - 3 ) & ( BLOCK_BUFFER_SIZE - 1 ) ;
block_t * block [ 5 ] = { block_t * block [ 3 ] = { NULL , NULL , NULL } ;
NULL , NULL , NULL , NULL , NULL } ;
while ( block_index ! = block_buffer_tail ) { while ( block_index ! = block_buffer_tail ) {
block_index = ( block_index - 1 ) & ( BLOCK_BUFFER_SIZE - 1 ) ; block_index = prev_block_index ( block_index ) ;
block [ 2 ] = block [ 1 ] ; block [ 2 ] = block [ 1 ] ;
block [ 1 ] = block [ 0 ] ; block [ 1 ] = block [ 0 ] ;
block [ 0 ] = & block_buffer [ block_index ] ; block [ 0 ] = & block_buffer [ block_index ] ;
planner_reverse_pass_kernel ( block [ 0 ] , block [ 1 ] , block [ 2 ] ) ; planner_reverse_pass_kernel ( block [ 0 ] , block [ 1 ] , block [ 2 ] ) ;
} }
planner_reverse_pass_kernel ( NULL , block [ 0 ] , block [ 1 ] ) ;
} }
} }
// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
void planner_forward_pass_kernel ( block_t * previous , block_t * current , block_t * next ) { void planner_forward_pass_kernel ( block_t * previous , block_t * current , block_t * next ) {
if ( ! current ) { if ( ! previous ) { return ; }
return ;
} // If the previous block is an acceleration block, but it is not long enough to complete the
if ( previous ) { // full speed change within the block, we need to adjust the entry speed accordingly. Entry
// If the previous block is an acceleration block, but it is not long enough to
// speeds have already been reset, maximized, and reverse planned by reverse planner.
// complete the full speed change within the block, we need to adjust out entry
// If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
// speed accordingly. Remember current->entry_factor equals the exit factor of
if ( ! previous - > nominal_length_flag ) {
// the previous block.
if ( previous - > entry_speed < current - > entry_speed ) {
if ( previous - > entry_speed < current - > entry_speed ) { double entry_speed = min ( current - > entry_speed ,
float max_entry_speed = max_allowable_speed ( - previous - > acceleration , previous - > entry_speed , previous - > millimeters ) ; max_allowable_speed ( - previous - > acceleration , previous - > entry_speed , previous - > millimeters ) ) ;
if ( max_entry_speed < current - > entry_speed ) {
current - > entry_speed = max_entry_speed ; // Check for junction speed change
if ( current - > entry_speed ! = entry_speed ) {
current - > entry_speed = entry_speed ;
current - > recalculate_flag = true ;
} }
} }
} }
@ -287,15 +280,14 @@ void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *n
// implements the forward pass.
// implements the forward pass.
void planner_forward_pass ( ) { void planner_forward_pass ( ) {
char block_index = block_buffer_tail ; char block_index = block_buffer_tail ;
block_t * block [ 3 ] = { block_t * block [ 3 ] = { NULL , NULL , NULL } ;
NULL , NULL , NULL } ;
while ( block_index ! = block_buffer_head ) { while ( block_index ! = block_buffer_head ) {
block [ 0 ] = block [ 1 ] ; block [ 0 ] = block [ 1 ] ;
block [ 1 ] = block [ 2 ] ; block [ 1 ] = block [ 2 ] ;
block [ 2 ] = & block_buffer [ block_index ] ; block [ 2 ] = & block_buffer [ block_index ] ;
planner_forward_pass_kernel ( block [ 0 ] , block [ 1 ] , block [ 2 ] ) ; planner_forward_pass_kernel ( block [ 0 ] , block [ 1 ] , block [ 2 ] ) ;
block_index = ( block_index + 1 ) & ( BLOCK_BUFFER_SIZE - 1 ) ; block_index = next_block_index ( block_index ) ;
} }
planner_forward_pass_kernel ( block [ 1 ] , block [ 2 ] , NULL ) ; planner_forward_pass_kernel ( block [ 1 ] , block [ 2 ] , NULL ) ;
} }
@ -304,18 +296,30 @@ void planner_forward_pass() {
// entry_factor for each junction. Must be called by planner_recalculate() after
// entry_factor for each junction. Must be called by planner_recalculate() after
// updating the blocks.
// updating the blocks.
void planner_recalculate_trapezoids ( ) { void planner_recalculate_trapezoids ( ) {
char block_index = block_buffer_tail ; int8_t block_index = block_buffer_tail ;
block_t * current ; block_t * current ;
block_t * next = NULL ; block_t * next = NULL ;
while ( block_index ! = block_buffer_head ) { while ( block_index ! = block_buffer_head ) {
current = next ; current = next ;
next = & block_buffer [ block_index ] ; next = & block_buffer [ block_index ] ;
if ( current ) { if ( current ) {
calculate_trapezoid_for_block ( current , current - > entry_speed , next - > entry_speed ) ; // Recalculate if current block entry or exit junction speed has changed.
if ( current - > recalculate_flag | | next - > recalculate_flag ) {
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
calculate_trapezoid_for_block ( current , current - > entry_speed / current - > nominal_speed ,
next - > entry_speed / current - > nominal_speed ) ;
current - > recalculate_flag = false ; // Reset current only to ensure next trapezoid is computed
}
} }
block_index = ( block_index + 1 ) & ( BLOCK_BUFFER_SIZE - 1 ) ; block_index = next_block_index ( block_index ) ;
}
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
if ( next ! = NULL ) {
calculate_trapezoid_for_block ( next , next - > entry_speed / next - > nominal_speed ,
MINIMUM_PLANNER_SPEED / next - > nominal_speed ) ;
next - > recalculate_flag = false ;
} }
calculate_trapezoid_for_block ( next , next - > entry_speed , safe_speed ( next ) ) ;
} }
// Recalculates the motion plan according to the following algorithm:
// Recalculates the motion plan according to the following algorithm:
@ -345,6 +349,11 @@ void plan_init() {
block_buffer_head = 0 ; block_buffer_head = 0 ;
block_buffer_tail = 0 ; block_buffer_tail = 0 ;
memset ( position , 0 , sizeof ( position ) ) ; // clear position
memset ( position , 0 , sizeof ( position ) ) ; // clear position
previous_speed [ 0 ] = 0.0 ;
previous_speed [ 1 ] = 0.0 ;
previous_speed [ 2 ] = 0.0 ;
previous_speed [ 3 ] = 0.0 ;
previous_nominal_speed = 0.0 ;
} }
@ -387,13 +396,15 @@ void check_axes_activity() {
if ( ( DISABLE_E ) & & ( e_active = = 0 ) ) disable_e ( ) ; if ( ( DISABLE_E ) & & ( e_active = = 0 ) ) disable_e ( ) ;
} }
float junction_deviation = 0.1 ;
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
// calculation the caller must also provide the physical length of the line in millimeters.
void plan_buffer_line ( const float & x , const float & y , const float & z , const float & e , float feed_rate ) void plan_buffer_line ( const float & x , const float & y , const float & z , const float & e , float feed_rate )
{ {
// Calculate the buffer head after we push this byte
// Calculate the buffer head after we push this byte
int next_buffer_head = ( block_buffer_head + 1 ) & ( BLOCK_BUFFER_SIZE - 1 ) ; int next_buffer_head = next_block_index ( block_buffer_head ) ;
// If the buffer is full: good! That means we are well ahead of the robot.
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
// Rest here until there is room in the buffer.
@ -426,9 +437,14 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
block - > step_event_count = max ( block - > steps_x , max ( block - > steps_y , max ( block - > steps_z , block - > steps_e ) ) ) ; block - > step_event_count = max ( block - > steps_x , max ( block - > steps_y , max ( block - > steps_z , block - > steps_e ) ) ) ;
// Bail if this is a zero-length block
// Bail if this is a zero-length block
if ( block - > step_event_count < = dropsegments ) { if ( block - > step_event_count < = dropsegments ) { return ; } ;
return ;
} ; // Compute direction bits for this block
block - > direction_bits = 0 ;
if ( target [ X_AXIS ] < position [ X_AXIS ] ) { block - > direction_bits | = ( 1 < < X_AXIS ) ; }
if ( target [ Y_AXIS ] < position [ Y_AXIS ] ) { block - > direction_bits | = ( 1 < < Y_AXIS ) ; }
if ( target [ Z_AXIS ] < position [ Z_AXIS ] ) { block - > direction_bits | = ( 1 < < Z_AXIS ) ; }
if ( target [ E_AXIS ] < position [ E_AXIS ] ) { block - > direction_bits | = ( 1 < < E_AXIS ) ; }
//enable active axes
//enable active axes
if ( block - > steps_x ! = 0 ) enable_x ( ) ; if ( block - > steps_x ! = 0 ) enable_x ( ) ;
@ -436,14 +452,23 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
if ( block - > steps_z ! = 0 ) enable_z ( ) ; if ( block - > steps_z ! = 0 ) enable_z ( ) ;
if ( block - > steps_e ! = 0 ) enable_e ( ) ; if ( block - > steps_e ! = 0 ) enable_e ( ) ;
float delta_x_mm = ( target [ X_AXIS ] - position [ X_AXIS ] ) / axis_steps_per_unit [ X_AXIS ] ; float delta_mm [ 4 ] ;
float delta_y_mm = ( target [ Y_AXIS ] - position [ Y_AXIS ] ) / axis_steps_per_unit [ Y_AXIS ] ; delta_mm [ X_AXIS ] = ( target [ X_AXIS ] - position [ X_AXIS ] ) / axis_steps_per_unit [ X_AXIS ] ;
float delta_z_mm = ( target [ Z_AXIS ] - position [ Z_AXIS ] ) / axis_steps_per_unit [ Z_AXIS ] ; delta_mm [ Y_AXIS ] = ( target [ Y_AXIS ] - position [ Y_AXIS ] ) / axis_steps_per_unit [ Y_AXIS ] ;
float delta_e_mm = ( target [ E_AXIS ] - position [ E_AXIS ] ) / axis_steps_per_unit [ E_AXIS ] ; delta_mm [ Z_AXIS ] = ( target [ Z_AXIS ] - position [ Z_AXIS ] ) / axis_steps_per_unit [ Z_AXIS ] ;
block - > millimeters = sqrt ( square ( delta_x_mm ) + square ( delta_y_mm ) + square ( delta_z_mm ) + square ( delta_e_mm ) ) ; delta_mm [ E_AXIS ] = ( target [ E_AXIS ] - position [ E_AXIS ] ) / axis_steps_per_unit [ E_AXIS ] ;
block - > millimeters = sqrt ( square ( delta_mm [ X_AXIS ] ) + square ( delta_mm [ Y_AXIS ] ) +
square ( delta_mm [ Z_AXIS ] ) ) ;
float inverse_millimeters = 1.0 / block - > millimeters ; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters ;
unsigned long microseconds ; block - > nominal_speed = block - > millimeters * inverse_second ; // (mm/sec) Always > 0
block - > nominal_rate = ceil ( block - > step_event_count * inverse_second ) ; // (step/sec) Always > 0
// unsigned long microseconds;
#if 0
if ( block - > steps_e = = 0 ) { if ( block - > steps_e = = 0 ) {
if ( feed_rate < mintravelfeedrate ) feed_rate = mintravelfeedrate ; if ( feed_rate < mintravelfeedrate ) feed_rate = mintravelfeedrate ;
} }
@ -466,75 +491,168 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
if ( microseconds < minsegmenttime ) microseconds = minsegmenttime ; if ( microseconds < minsegmenttime ) microseconds = minsegmenttime ;
} }
// END OF SLOW DOWN SECTION
// END OF SLOW DOWN SECTION
# endif
// Calculate speed in mm/sec for each axis
float current_speed [ 4 ] ;
for ( int i = 0 ; i < 4 ; i + + ) {
current_speed [ i ] = delta_mm [ i ] * inverse_second ;
}
// Calculate speed in mm/minute for each axis
// Limit speed per axis
float multiplier = 60.0 * 1000000.0 / microseconds ; float speed_factor = 1.0 ; //factor <=1 do decrease speed
block - > speed_z = delta_z_mm * multiplier ; for ( int i = 0 ; i < 4 ; i + + ) {
block - > speed_x = delta_x_mm * multiplier ; if ( abs ( current_speed [ i ] ) > max_feedrate [ i ] )
block - > speed_y = delta_y_mm * multiplier ; speed_factor = min ( speed_factor , max_feedrate [ i ] / abs ( current_speed [ i ] ) ) ;
block - > speed_e = delta_e_mm * multiplier ; }
// Max segement time in us.
// Limit speed per axis
# ifdef XY_FREQUENCY_LIMIT
float speed_factor = 1 ; //factor <=1 do decrease speed
# define MAX_FREQ_TIME (1000000.0 / XY_FREQUENCY_LIMIT)
if ( abs ( block - > speed_x ) > max_feedrate [ X_AXIS ] ) {
speed_factor = max_feedrate [ X_AXIS ] / abs ( block - > speed_x ) ; // Check and limit the xy direction change frequency
//if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; /is not need here because auf the init above
unsigned char direction_change = block - > direction_bits ^ old_direction_bits ;
old_direction_bits = block - > direction_bits ;
long segment_time = lround ( 1000000.0 / inverse_second ) ;
if ( ( direction_change & ( 1 < < X_AXIS ) ) = = 0 ) {
x_segment_time [ 0 ] + = segment_time ;
} }
if ( abs ( block - > speed_y ) > max_feedrate [ Y_AXIS ] ) { else {
float tmp_speed_factor = max_feedrate [ Y_AXIS ] / abs ( block - > speed_y ) ; x_segment_time [ 2 ] = x_segment_time [ 1 ] ;
if ( speed_factor > tmp_speed_factor ) speed_factor = tmp_speed_factor ; x_segment_time [ 1 ] = x_segment_time [ 0 ] ;
x_segment_time [ 0 ] = segment_time ;
} }
if ( abs ( block - > speed_z ) > max_feedrate [ Z_AXIS ] ) { if ( ( direction_change & ( 1 < < Y_AXIS ) ) = = 0 ) {
float tmp_speed_factor = max_feedrate [ Z_AXIS ] / abs ( block - > speed_z ) ; y_segment_time [ 0 ] + = segment_time ;
if ( speed_factor > tmp_speed_factor ) speed_factor = tmp_speed_factor ;
} }
if ( abs ( block - > speed_e ) > max_feedrate [ E_AXIS ] ) { else {
float tmp_speed_factor = max_feedrate [ E_AXIS ] / abs ( block - > speed_e ) ; y_segment_time [ 2 ] = y_segment_time [ 1 ] ;
if ( speed_factor > tmp_speed_factor ) speed_factor = tmp_speed_factor ; y_segment_time [ 1 ] = y_segment_time [ 0 ] ;
y_segment_time [ 0 ] = segment_time ;
}
long max_x_segment_time = max ( x_segment_time [ 0 ] , max ( x_segment_time [ 1 ] , x_segment_time [ 2 ] ) ) ;
long max_y_segment_time = max ( y_segment_time [ 0 ] , max ( y_segment_time [ 1 ] , y_segment_time [ 2 ] ) ) ;
long min_xy_segment_time = min ( max_x_segment_time , max_y_segment_time ) ;
if ( min_xy_segment_time < MAX_FREQ_TIME ) speed_factor = min ( speed_factor , ( float ) min_xy_segment_time / ( float ) MAX_FREQ_TIME ) ;
# endif
// Correct the speed
if ( speed_factor < 1.0 ) {
// Serial.print("speed factor : "); Serial.println(speed_factor);
for ( int i = 0 ; i < 4 ; i + + ) {
if ( abs ( current_speed [ i ] ) > max_feedrate [ i ] )
speed_factor = min ( speed_factor , max_feedrate [ i ] / abs ( current_speed [ i ] ) ) ;
// Serial.print("current_speed"); Serial.print(i); Serial.print(" : "); Serial.println(current_speed[i]);
}
for ( unsigned char i = 0 ; i < 4 ; i + + ) {
current_speed [ i ] * = speed_factor ;
}
block - > nominal_speed * = speed_factor ;
block - > nominal_rate * = speed_factor ;
} }
multiplier = multiplier * speed_factor ;
block - > speed_z = delta_z_mm * multiplier ; // Compute and limit the acceleration rate for the trapezoid generator.
block - > speed_x = delta_x_mm * multiplier ; float steps_per_mm = block - > step_event_count / block - > millimeters ;
block - > speed_y = delta_y_mm * multiplier ;
block - > speed_e = delta_e_mm * multiplier ;
block - > nominal_speed = block - > millimeters * multiplier ;
block - > nominal_rate = ceil ( block - > step_event_count * multiplier / 60 ) ;
if ( block - > nominal_rate < 120 )
block - > nominal_rate = 120 ;
block - > entry_speed = safe_speed ( block ) ;
// Compute the acceleration rate for the trapezoid generator.
float travel_per_step = block - > millimeters / block - > step_event_count ;
if ( block - > steps_x = = 0 & & block - > steps_y = = 0 & & block - > steps_z = = 0 ) { if ( block - > steps_x = = 0 & & block - > steps_y = = 0 & & block - > steps_z = = 0 ) {
block - > acceleration_st = ceil ( ( retract_acceleration ) / travel_per_step ) ; // convert to: acceleration steps/sec^2
block - > acceleration_st = ceil ( retract_acceleration * steps_per_mm ) ; // convert to: acceleration steps/sec^2
} }
else { else {
block - > acceleration_st = ceil ( ( acceleration ) / travel_per_step ) ; // convert to: acceleration steps/sec^2
block - > acceleration_st = ceil ( acceleration * steps_per_mm ) ; // convert to: acceleration steps/sec^2
float tmp_acceleration = ( float ) block - > acceleration_st / ( float ) block - > step_event_count ;
// Limit acceleration per axis
// Limit acceleration per axis
if ( ( tmp_acceleration * block - > steps_x ) > axis_steps_per_sqr_second [ X_AXIS ] ) { if ( ( ( float ) block - > acceleration_st * ( float ) block - > steps_x / ( float ) block - > step_event_count ) > axis_steps_per_sqr_second [ X_AXIS ] )
block - > acceleration_st = axis_steps_per_sqr_second [ X_AXIS ] ; block - > acceleration_st = axis_steps_per_sqr_second [ X_AXIS ] ;
tmp_acceleration = ( float ) block - > acceleration_st / ( float ) block - > step_event_count ; if ( ( ( float ) block - > acceleration_st * ( float ) block - > steps_y / ( float ) block - > step_event_count ) > axis_steps_per_sqr_second [ Y_AXIS ] )
}
if ( ( tmp_acceleration * block - > steps_y ) > axis_steps_per_sqr_second [ Y_AXIS ] ) {
block - > acceleration_st = axis_steps_per_sqr_second [ Y_AXIS ] ; block - > acceleration_st = axis_steps_per_sqr_second [ Y_AXIS ] ;
tmp_acceleration = ( float ) block - > acceleration_st / ( float ) block - > step_event_count ; if ( ( ( float ) block - > acceleration_st * ( float ) block - > steps_e / ( float ) block - > step_event_count ) > axis_steps_per_sqr_second [ E_AXIS ] )
}
if ( ( tmp_acceleration * block - > steps_e ) > axis_steps_per_sqr_second [ E_AXIS ] ) {
block - > acceleration_st = axis_steps_per_sqr_second [ E_AXIS ] ; block - > acceleration_st = axis_steps_per_sqr_second [ E_AXIS ] ;
tmp_acceleration = ( float ) block - > acceleration_st / ( float ) block - > step_event_count ; if ( ( ( float ) block - > acceleration_st * ( float ) block - > steps_z / ( float ) block - > step_event_count ) > axis_steps_per_sqr_second [ Z_AXIS ] )
}
if ( ( tmp_acceleration * block - > steps_z ) > axis_steps_per_sqr_second [ Z_AXIS ] ) {
block - > acceleration_st = axis_steps_per_sqr_second [ Z_AXIS ] ; block - > acceleration_st = axis_steps_per_sqr_second [ Z_AXIS ] ;
tmp_acceleration = ( float ) block - > acceleration_st / ( float ) block - > step_event_count ;
}
} }
block - > acceleration = block - > acceleration_st * travel_per_step ; block - > acceleration = block - > acceleration_st / steps_per_mm ;
block - > acceleration_rate = ( long ) ( ( float ) block - > acceleration_st * 8.388608 ) ; block - > acceleration_rate = ( long ) ( ( float ) block - > acceleration_st * 8.388608 ) ;
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec [ 3 ] ;
unit_vec [ X_AXIS ] = delta_mm [ X_AXIS ] * inverse_millimeters ;
unit_vec [ Y_AXIS ] = delta_mm [ Y_AXIS ] * inverse_millimeters ;
unit_vec [ Z_AXIS ] = delta_mm [ Z_AXIS ] * inverse_millimeters ;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED ; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ( ( block_buffer_head ! = block_buffer_tail ) & & ( previous_nominal_speed > 0.0 ) ) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec [ X_AXIS ] * unit_vec [ X_AXIS ]
- previous_unit_vec [ Y_AXIS ] * unit_vec [ Y_AXIS ]
- previous_unit_vec [ Z_AXIS ] * unit_vec [ Z_AXIS ] ;
// Skip and use default max junction speed for 0 degree acute junction.
if ( cos_theta < 0.95 ) {
vmax_junction = min ( previous_nominal_speed , block - > nominal_speed ) ;
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if ( cos_theta > - 0.95 ) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt ( 0.5 * ( 1.0 - cos_theta ) ) ; // Trig half angle identity. Always positive.
vmax_junction = min ( vmax_junction ,
sqrt ( block - > acceleration * junction_deviation * sin_theta_d2 / ( 1.0 - sin_theta_d2 ) ) ) ;
}
}
}
# endif
// Start with a safe speed
float vmax_junction = max_xy_jerk / 2 ;
if ( abs ( current_speed [ Z_AXIS ] ) > max_z_jerk / 2 )
vmax_junction = max_z_jerk / 2 ;
vmax_junction = min ( vmax_junction , block - > nominal_speed ) ;
if ( ( block_buffer_head ! = block_buffer_tail ) & & ( previous_nominal_speed > 0.0 ) ) {
float jerk = sqrt ( pow ( ( current_speed [ X_AXIS ] - previous_speed [ X_AXIS ] ) , 2 ) + pow ( ( current_speed [ Y_AXIS ] - previous_speed [ Y_AXIS ] ) , 2 ) ) ;
if ( ( previous_speed [ X_AXIS ] ! = 0.0 ) | | ( previous_speed [ Y_AXIS ] ! = 0.0 ) ) {
vmax_junction = block - > nominal_speed ;
}
if ( jerk > max_xy_jerk ) {
vmax_junction * = ( max_xy_jerk / jerk ) ;
}
if ( abs ( current_speed [ Z_AXIS ] - previous_speed [ Z_AXIS ] ) > max_z_jerk ) {
vmax_junction * = ( max_z_jerk / abs ( current_speed [ Z_AXIS ] - previous_speed [ Z_AXIS ] ) ) ;
}
}
block - > max_entry_speed = vmax_junction ;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed ( - block - > acceleration , MINIMUM_PLANNER_SPEED , block - > millimeters ) ;
block - > entry_speed = min ( vmax_junction , v_allowable ) ;
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if ( block - > nominal_speed < = v_allowable ) { block - > nominal_length_flag = true ; }
else { block - > nominal_length_flag = false ; }
block - > recalculate_flag = true ; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy ( previous_speed , current_speed , sizeof ( previous_speed ) ) ; // previous_speed[] = current_speed[]
previous_nominal_speed = block - > nominal_speed ;
# ifdef ADVANCE # ifdef ADVANCE
// Calculate advance rate
// Calculate advance rate
if ( ( block - > steps_e = = 0 ) | | ( block - > steps_x = = 0 & & block - > steps_y = = 0 & & block - > steps_z = = 0 ) ) { if ( ( block - > steps_e = = 0 ) | | ( block - > steps_x = = 0 & & block - > steps_y = = 0 & & block - > steps_z = = 0 ) ) {
@ -555,24 +673,11 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
} }
# endif // ADVANCE
# endif // ADVANCE
// compute a preliminary conservative acceleration trapezoid
float safespeed = safe_speed ( block ) ;
calculate_trapezoid_for_block ( block , safespeed , safespeed ) ;
// Compute direction bits for this block
block - > direction_bits = 0 ;
if ( target [ X_AXIS ] < position [ X_AXIS ] ) { calculate_trapezoid_for_block ( block , block - > entry_speed / block - > nominal_speed ,
block - > direction_bits | = ( 1 < < X_AXIS ) ; MINIMUM_PLANNER_SPEED / block - > nominal_speed ) ;
}
if ( target [ Y_AXIS ] < position [ Y_AXIS ] ) {
block - > direction_bits | = ( 1 < < Y_AXIS ) ;
}
if ( target [ Z_AXIS ] < position [ Z_AXIS ] ) {
block - > direction_bits | = ( 1 < < Z_AXIS ) ;
}
if ( target [ E_AXIS ] < position [ E_AXIS ] ) {
block - > direction_bits | = ( 1 < < E_AXIS ) ;
}
// Move buffer head
// Move buffer head
block_buffer_head = next_buffer_head ; block_buffer_head = next_buffer_head ;
@ -581,6 +686,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
memcpy ( position , target , sizeof ( target ) ) ; // position[] = target[]
memcpy ( position , target , sizeof ( target ) ) ; // position[] = target[]
planner_recalculate ( ) ; planner_recalculate ( ) ;
st_wake_up ( ) ; st_wake_up ( ) ;
} }
@ -590,5 +696,10 @@ void plan_set_position(const float &x, const float &y, const float &z, const flo
position [ Y_AXIS ] = lround ( y * axis_steps_per_unit [ Y_AXIS ] ) ; position [ Y_AXIS ] = lround ( y * axis_steps_per_unit [ Y_AXIS ] ) ;
position [ Z_AXIS ] = lround ( z * axis_steps_per_unit [ Z_AXIS ] ) ; position [ Z_AXIS ] = lround ( z * axis_steps_per_unit [ Z_AXIS ] ) ;
position [ E_AXIS ] = lround ( e * axis_steps_per_unit [ E_AXIS ] ) ; position [ E_AXIS ] = lround ( e * axis_steps_per_unit [ E_AXIS ] ) ;
previous_nominal_speed = 0.0 ; // Resets planner junction speeds. Assumes start from rest.
previous_speed [ 0 ] = 0.0 ;
previous_speed [ 1 ] = 0.0 ;
previous_speed [ 2 ] = 0.0 ;
previous_speed [ 3 ] = 0.0 ;
} }
Erik van der Zalm
13 years ago