@ -150,33 +150,31 @@ void Planner::init() {
* by the provided factors .
* by the provided factors .
*/
*/
void Planner : : calculate_trapezoid_for_block ( block_t * block , float entry_factor , float exit_factor ) {
void Planner : : calculate_trapezoid_for_block ( block_t * block , float entry_factor , float exit_factor ) {
u nsigned long initial_rate = ceil ( block - > nominal_rate * entry_factor ) ,
u int32_t initial_rate = ceil ( block - > nominal_rate * entry_factor ) ,
final_rate = ceil ( block - > nominal_rate * exit_factor ) ; // (steps per second)
final_rate = ceil ( block - > nominal_rate * exit_factor ) ; // (steps per second)
// Limit minimal step rate (Otherwise the timer will overflow.)
// Limit minimal step rate (Otherwise the timer will overflow.)
NOLESS ( initial_rate , 120 ) ;
NOLESS ( initial_rate , 120 ) ;
NOLESS ( final_rate , 120 ) ;
NOLESS ( final_rate , 120 ) ;
long accel = block - > acceleration_steps_per_s2 ;
int32_t accel = block - > acceleration_steps_per_s2 ,
int32_t accelerate_steps = ceil ( estimate_acceleration_distance ( initial_rate , block - > nominal_rate , accel ) ) ;
accelerate_steps = ceil ( estimate_acceleration_distance ( initial_rate , block - > nominal_rate , accel ) ) ,
int32_t decelerate_steps = floor ( estimate_acceleration_distance ( block - > nominal_rate , final_rate , - accel ) ) ;
decelerate_steps = floor ( estimate_acceleration_distance ( block - > nominal_rate , final_rate , - accel ) ) ,
plateau_steps = block - > step_event_count - accelerate_steps - decelerate_steps ;
// Calculate the size of Plateau of Nominal Rate.
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 accel and start braking
// have to use intersection_distance() to calculate when to abort accel 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 = ceil ( intersection_distance ( initial_rate , final_rate , accel , block - > step_event_count ) ) ;
accelerate_steps = ceil ( intersection_distance ( initial_rate , final_rate , accel , block - > step_event_count ) ) ;
accelerate_steps = max ( accelerate_steps , 0 ) ; // Check limits due to numerical round-off
NOLESS ( accelerate_steps , 0 ) ; // Check limits due to numerical round-off
accelerate_steps = min ( ( uint32_t ) accelerate_steps , block - > step_event_count ) ; //(We can cast here to unsigned, because the above line ensures that we are above zero)
accelerate_steps = min ( ( uint32_t ) accelerate_steps , block - > step_event_count ) ; //(We can cast here to unsigned, because the above line ensures that we are above zero)
plateau_steps = 0 ;
plateau_steps = 0 ;
}
}
# if ENABLED(ADVANCE)
# if ENABLED(ADVANCE)
volatile long initial_advance = block - > advance * sq ( entry_factor ) ;
volatile int32_t initial_advance = block - > advance * sq ( entry_factor ) ,
volatile long final_advance = block - > advance * sq ( exit_factor ) ;
final_advance = block - > advance * sq ( exit_factor ) ;
# endif // ADVANCE
# endif // ADVANCE
// block->accelerate_until = accelerate_steps;
// block->accelerate_until = accelerate_steps;
@ -266,7 +264,7 @@ void Planner::forward_pass_kernel(block_t* previous, block_t* current) {
// If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
// If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
if ( ! previous - > nominal_length_flag ) {
if ( ! previous - > nominal_length_flag ) {
if ( previous - > entry_speed < current - > entry_speed ) {
if ( previous - > entry_speed < current - > entry_speed ) {
double entry_speed = min ( current - > entry_speed ,
float entry_speed = min ( current - > entry_speed ,
max_allowable_speed ( - previous - > acceleration , previous - > entry_speed , previous - > millimeters ) ) ;
max_allowable_speed ( - previous - > acceleration , previous - > entry_speed , previous - > millimeters ) ) ;
// Check for junction speed change
// Check for junction speed change
if ( current - > entry_speed ! = entry_speed ) {
if ( current - > entry_speed ! = entry_speed ) {
@ -982,15 +980,13 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
# endif
# endif
// Calculate and limit speed in mm/sec for each axis
// Calculate and limit speed in mm/sec for each axis
float current_speed [ NUM_AXIS ] ;
float current_speed [ NUM_AXIS ] , speed_factor = 1.0 ; // factor <1 decreases speed
float speed_factor = 1.0 ; //factor <=1 do decrease speed
LOOP_XYZE ( i ) {
LOOP_XYZE ( i ) {
current_speed [ i ] = delta_mm [ i ] * inverse_mm_s ;
float cs = fabs ( current_speed [ i ] = delta_mm [ i ] * inverse_mm_s ) ;
float cs = fabs ( current_speed [ i ] ) , mf = max_feedrate_mm_s [ i ] ;
if ( cs > max_feedrate_mm_s [ i ] ) NOMORE ( speed_factor , max_feedrate_mm_s [ i ] / cs ) ;
if ( cs > mf ) speed_factor = min ( speed_factor , mf / cs ) ;
}
}
// Max seg ement time in u s.
// Max seg ment time in µ s.
# ifdef XY_FREQUENCY_LIMIT
# ifdef XY_FREQUENCY_LIMIT
// Check and limit the xy direction change frequency
// Check and limit the xy direction change frequency
@ -1024,7 +1020,7 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
min_xy_segment_time = min ( max_x_segment_time , max_y_segment_time ) ;
min_xy_segment_time = min ( max_x_segment_time , max_y_segment_time ) ;
if ( min_xy_segment_time < MAX_FREQ_TIME ) {
if ( min_xy_segment_time < MAX_FREQ_TIME ) {
float low_sf = speed_factor * min_xy_segment_time / ( MAX_FREQ_TIME ) ;
float low_sf = speed_factor * min_xy_segment_time / ( MAX_FREQ_TIME ) ;
speed_factor = min ( speed_factor , low_sf ) ;
NOMORE ( speed_factor , low_sf ) ;
}
}
# endif // XY_FREQUENCY_LIMIT
# endif // XY_FREQUENCY_LIMIT
@ -1091,8 +1087,7 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
if ( cos_theta > - 0.95 ) {
if ( cos_theta > - 0.95 ) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
// 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.
double sin_theta_d2 = sqrt ( 0.5 * ( 1.0 - cos_theta ) ) ; // Trig half angle identity. Always positive.
vmax_junction = min ( vmax_junction ,
NOMORE ( vmax_junction , sqrt ( block - > acceleration * junction_deviation * sin_theta_d2 / ( 1.0 - sin_theta_d2 ) ) ) ;
sqrt ( block - > acceleration * junction_deviation * sin_theta_d2 / ( 1.0 - sin_theta_d2 ) ) ) ;
}
}
}
}
}
}
@ -1125,7 +1120,7 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
block - > max_entry_speed = vmax_junction ;
block - > max_entry_speed = vmax_junction ;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
// 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 ) ;
float v_allowable = max_allowable_speed ( - block - > acceleration , MINIMUM_PLANNER_SPEED , block - > millimeters ) ;
block - > entry_speed = min ( vmax_junction , v_allowable ) ;
block - > entry_speed = min ( vmax_junction , v_allowable ) ;
// Initialize planner efficiency flags
// Initialize planner efficiency flags