@ -93,9 +93,7 @@ float Planner::min_feedrate_mm_s,
Planner : : acceleration , // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
Planner : : retract_acceleration , // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
Planner : : travel_acceleration , // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
Planner : : max_xy_jerk , // The largest speed change requiring no acceleration
Planner : : max_z_jerk ,
Planner : : max_e_jerk ,
Planner : : max_jerk [ XYZE ] , // The largest speed change requiring no acceleration
Planner : : min_travel_feedrate_mm_s ;
# if HAS_ABL
@ -152,33 +150,31 @@ void Planner::init() {
* by the provided factors .
*/
void Planner : : calculate_trapezoid_for_block ( block_t * block , float entry_factor , float exit_factor ) {
unsigned long initial_rate = ceil ( block - > nominal_rate * entry_factor ) ,
final_rate = ceil ( block - > nominal_rate * exit_factor ) ; // (steps per second)
uint32_t initial_rate = ceil ( block - > nominal_rate * entry_factor ) ,
final_rate = ceil ( block - > nominal_rate * exit_factor ) ; // (steps per second)
// Limit minimal step rate (Otherwise the timer will overflow.)
NOLESS ( initial_rate , 120 ) ;
NOLESS ( final_rate , 120 ) ;
long accel = block - > acceleration_steps_per_s2 ;
int32_t 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 ) ) ;
// Calculate the size of Plateau of Nominal Rate.
int32_t plateau_steps = block - > step_event_count - accelerate_steps - decelerate_steps ;
int32_t accel = block - > acceleration_steps_per_s2 ,
accelerate_steps = ceil ( estimate_acceleration_distance ( initial_rate , block - > nominal_rate , accel ) ) ,
decelerate_steps = floor ( estimate_acceleration_distance ( block - > nominal_rate , final_rate , - accel ) ) ,
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
// 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.
if ( plateau_steps < 0 ) {
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)
plateau_steps = 0 ;
}
# if ENABLED(ADVANCE)
volatile long initial_advance = block - > advance * sq ( entry_factor ) ;
volatile long final_advance = block - > advance * sq ( exit_factor ) ;
volatile int32_t initial_advance = block - > advance * sq ( entry_factor ) ,
final_advance = block - > advance * sq ( exit_factor ) ;
# endif // ADVANCE
// block->accelerate_until = accelerate_steps;
@ -268,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 ( ! previous - > nominal_length_flag ) {
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 ) ) ;
// Check for junction speed change
if ( current - > entry_speed ! = entry_speed ) {
@ -984,15 +980,13 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
# endif
// Calculate and limit speed in mm/sec for each axis
float current_speed [ NUM_AXIS ] ;
float speed_factor = 1.0 ; //factor <=1 do decrease speed
float current_speed [ NUM_AXIS ] , speed_factor = 1.0 ; // factor <1 decreases speed
LOOP_XYZE ( i ) {
current_speed [ i ] = delta_mm [ i ] * inverse_mm_s ;
float cs = fabs ( current_speed [ i ] ) , mf = max_feedrate_mm_s [ i ] ;
if ( cs > mf ) speed_factor = min ( speed_factor , mf / cs ) ;
float cs = fabs ( current_speed [ i ] = delta_mm [ i ] * inverse_mm_s ) ;
if ( cs > max_feedrate_mm_s [ i ] ) NOMORE ( speed_factor , max_feedrate_mm_s [ i ] / cs ) ;
}
// Max segement time in u s.
// Max segment time in µ s.
# ifdef XY_FREQUENCY_LIMIT
// Check and limit the xy direction change frequency
@ -1026,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 ) ;
if ( 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
@ -1093,45 +1087,40 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
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 ) ) ) ;
NOMORE ( 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 * 0.5 ,
vmax_junction_factor = 1.0 ,
mz2 = max_z_jerk * 0.5 ,
me2 = max_e_jerk * 0.5 ,
csz = current_speed [ Z_AXIS ] ,
cse = current_speed [ E_AXIS ] ;
if ( fabs ( csz ) > mz2 ) vmax_junction = min ( vmax_junction , mz2 ) ;
if ( fabs ( cse ) > me2 ) vmax_junction = min ( vmax_junction , me2 ) ;
vmax_junction = min ( vmax_junction , block - > nominal_speed ) ;
float vmax_junction = max_jerk [ X_AXIS ] * 0.5 , vmax_junction_factor = 1.0 ;
if ( max_jerk [ Y_AXIS ] * 0.5 < fabs ( current_speed [ Y_AXIS ] ) ) NOMORE ( vmax_junction , max_jerk [ Y_AXIS ] * 0.5 ) ;
if ( max_jerk [ Z_AXIS ] * 0.5 < fabs ( current_speed [ Z_AXIS ] ) ) NOMORE ( vmax_junction , max_jerk [ Z_AXIS ] * 0.5 ) ;
if ( max_jerk [ E_AXIS ] * 0.5 < fabs ( current_speed [ E_AXIS ] ) ) NOMORE ( vmax_junction , max_jerk [ E_AXIS ] * 0.5 ) ;
NOMORE ( vmax_junction , block - > nominal_speed ) ;
float safe_speed = vmax_junction ;
if ( ( moves_queued > 1 ) & & ( previous_nominal_speed > 0.0001 ) ) {
float dsx = current_speed [ X_AXIS ] - previous_speed [ X_AXIS ] ,
dsy = current_speed [ Y_AXIS ] - previous_speed [ Y_AXIS ] ,
dsz = fabs ( csz - previous_speed [ Z_AXIS ] ) ,
dse = fabs ( cse - previous_speed [ E_AXIS ] ) ,
jerk = HYPOT ( dsx , dsy ) ;
if ( moves_queued > 1 & & previous_nominal_speed > 0.0001 ) {
//if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block - > nominal_speed ;
//}
// if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block - > nominal_speed ;
// }
if ( jerk > max_xy_jerk ) vmax_junction_factor = max_xy_jerk / jerk ;
if ( dsz > max_z_jerk ) vmax_junction_factor = min ( vmax_junction_factor , max_z_jerk / dsz ) ;
if ( dse > max_e_jerk ) vmax_junction_factor = min ( vmax_junction_factor , max_e_jerk / dse ) ;
float dsx = fabs ( current_speed [ X_AXIS ] - previous_speed [ X_AXIS ] ) ,
dsy = fabs ( current_speed [ Y_AXIS ] - previous_speed [ Y_AXIS ] ) ,
dsz = fabs ( current_speed [ Z_AXIS ] - previous_speed [ Z_AXIS ] ) ,
dse = fabs ( current_speed [ E_AXIS ] - previous_speed [ E_AXIS ] ) ;
if ( dsx > max_jerk [ X_AXIS ] ) NOMORE ( vmax_junction_factor , max_jerk [ X_AXIS ] / dsx ) ;
if ( dsy > max_jerk [ Y_AXIS ] ) NOMORE ( vmax_junction_factor , max_jerk [ Y_AXIS ] / dsy ) ;
if ( dsz > max_jerk [ Z_AXIS ] ) NOMORE ( vmax_junction_factor , max_jerk [ Z_AXIS ] / dsz ) ;
if ( dse > max_jerk [ E_AXIS ] ) NOMORE ( vmax_junction_factor , max_jerk [ E_AXIS ] / dse ) ;
vmax_junction = min ( previous_nominal_speed , vmax_junction * vmax_junction_factor ) ; // Limit speed to max previous speed
}
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 ) ;
float v_allowable = max_allowable_speed ( - block - > acceleration , MINIMUM_PLANNER_SPEED , block - > millimeters ) ;
block - > entry_speed = min ( vmax_junction , v_allowable ) ;
// Initialize planner efficiency flags
@ -1173,7 +1162,7 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
}
else {
long acc_dist = estimate_acceleration_distance ( 0 , block - > nominal_rate , block - > acceleration_steps_per_s2 ) ;
float advance = ( ( STEPS_PER_CUBIC_MM_E ) * ( EXTRUDER_ADVANCE_K ) ) * HYPOT ( cse , EXTRUSION_AREA ) * 256 ;
float advance = ( ( STEPS_PER_CUBIC_MM_E ) * ( EXTRUDER_ADVANCE_K ) ) * HYPOT ( current_ spe ed [ E_AXIS ] , EXTRUSION_AREA ) * 256 ;
block - > advance = advance ;
block - > advance_rate = acc_dist ? advance / ( float ) acc_dist : 0 ;
}
Scott Lahteine
8 years ago
GitHub