@ -833,7 +833,7 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t* const n
// for max allowable speed if block is decelerating and nominal length is false.
// for max allowable speed if block is decelerating and nominal length is false.
const float new_entry_speed = ( TEST ( current - > flag , BLOCK_BIT_NOMINAL_LENGTH ) | | max_entry_speed < = next - > entry_speed )
const float new_entry_speed = ( TEST ( current - > flag , BLOCK_BIT_NOMINAL_LENGTH ) | | max_entry_speed < = next - > entry_speed )
? max_entry_speed
? max_entry_speed
: min ( max_entry_speed , max_allowable_speed ( - current - > acceleration , next - > entry_speed , current - > millimeters ) ) ;
: MIN ( max_entry_speed , max_allowable_speed ( - current - > acceleration , next - > entry_speed , current - > millimeters ) ) ;
if ( new_entry_speed ! = current - > entry_speed ) {
if ( new_entry_speed ! = current - > entry_speed ) {
current - > entry_speed = new_entry_speed ;
current - > entry_speed = new_entry_speed ;
SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
@ -859,7 +859,7 @@ void Planner::reverse_pass() {
// for max allowable speed if block is decelerating and nominal length is false.
// for max allowable speed if block is decelerating and nominal length is false.
const float new_entry_speed = TEST ( current - > flag , BLOCK_BIT_NOMINAL_LENGTH )
const float new_entry_speed = TEST ( current - > flag , BLOCK_BIT_NOMINAL_LENGTH )
? max_entry_speed
? max_entry_speed
: min ( max_entry_speed , max_allowable_speed ( - current - > acceleration , MINIMUM_PLANNER_SPEED , current - > millimeters ) ) ;
: MIN ( max_entry_speed , max_allowable_speed ( - current - > acceleration , MINIMUM_PLANNER_SPEED , current - > millimeters ) ) ;
if ( current - > entry_speed ! = new_entry_speed ) {
if ( current - > entry_speed ! = new_entry_speed ) {
current - > entry_speed = new_entry_speed ;
current - > entry_speed = new_entry_speed ;
SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
SBI ( current - > flag , BLOCK_BIT_RECALCULATE ) ;
@ -884,7 +884,7 @@ void Planner::forward_pass_kernel(const block_t* const previous, block_t* const
// 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 < current - > entry_speed ) {
if ( previous - > entry_speed < current - > entry_speed ) {
const float new_entry_speed = min ( current - > entry_speed , max_allowable_speed ( - previous - > acceleration , previous - > entry_speed , previous - > millimeters ) ) ;
const float new_entry_speed = MIN ( current - > entry_speed , max_allowable_speed ( - previous - > acceleration , previous - > entry_speed , previous - > millimeters ) ) ;
// Check for junction speed change
// Check for junction speed change
if ( current - > entry_speed ! = new_entry_speed ) {
if ( current - > entry_speed ! = new_entry_speed ) {
current - > entry_speed = new_entry_speed ;
current - > entry_speed = new_entry_speed ;
@ -1384,7 +1384,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
}
}
# endif // PREVENT_COLD_EXTRUSION
# endif // PREVENT_COLD_EXTRUSION
# if ENABLED(PREVENT_LENGTHY_EXTRUDE)
# if ENABLED(PREVENT_LENGTHY_EXTRUDE)
if ( labs ( de * e_factor [ extruder ] ) > ( int32_t ) axis_steps_per_mm [ E_AXIS_N ] * ( EXTRUDE_MAXLENGTH ) ) { // It's not important to get max. extrusion length in a precision < 1mm, so save some cycles and cast to int
if ( ABS ( de * e_factor [ extruder ] ) > ( int32_t ) axis_steps_per_mm [ E_AXIS_N ] * ( EXTRUDE_MAXLENGTH ) ) { // It's not important to get max. extrusion length in a precision < 1mm, so save some cycles and cast to int
position [ E_AXIS ] = target [ E_AXIS ] ; // Behave as if the move really took place, but ignore E part
position [ E_AXIS ] = target [ E_AXIS ] ; // Behave as if the move really took place, but ignore E part
# if HAS_POSITION_FLOAT
# if HAS_POSITION_FLOAT
position_float [ E_AXIS ] = target_float [ E_AXIS ] ;
position_float [ E_AXIS ] = target_float [ E_AXIS ] ;
@ -1425,7 +1425,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
if ( de < 0 ) SBI ( dm , E_AXIS ) ;
if ( de < 0 ) SBI ( dm , E_AXIS ) ;
const float esteps_float = de * e_factor [ extruder ] ;
const float esteps_float = de * e_factor [ extruder ] ;
const int32_t esteps = abs ( esteps_float ) + 0.5 ;
const int32_t esteps = ABS ( esteps_float ) + 0.5 ;
// Wait for the next available block
// Wait for the next available block
uint8_t next_buffer_head ;
uint8_t next_buffer_head ;
@ -1440,26 +1440,26 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// Number of steps for each axis
// Number of steps for each axis
// See http://www.corexy.com/theory.html
// See http://www.corexy.com/theory.html
# if CORE_IS_XY
# if CORE_IS_XY
block - > steps [ A_AXIS ] = labs ( da + db ) ;
block - > steps [ A_AXIS ] = ABS ( da + db ) ;
block - > steps [ B_AXIS ] = labs ( da - db ) ;
block - > steps [ B_AXIS ] = ABS ( da - db ) ;
block - > steps [ Z_AXIS ] = labs ( dc ) ;
block - > steps [ Z_AXIS ] = ABS ( dc ) ;
# elif CORE_IS_XZ
# elif CORE_IS_XZ
block - > steps [ A_AXIS ] = labs ( da + dc ) ;
block - > steps [ A_AXIS ] = ABS ( da + dc ) ;
block - > steps [ Y_AXIS ] = labs ( db ) ;
block - > steps [ Y_AXIS ] = ABS ( db ) ;
block - > steps [ C_AXIS ] = labs ( da - dc ) ;
block - > steps [ C_AXIS ] = ABS ( da - dc ) ;
# elif CORE_IS_YZ
# elif CORE_IS_YZ
block - > steps [ X_AXIS ] = labs ( da ) ;
block - > steps [ X_AXIS ] = ABS ( da ) ;
block - > steps [ B_AXIS ] = labs ( db + dc ) ;
block - > steps [ B_AXIS ] = ABS ( db + dc ) ;
block - > steps [ C_AXIS ] = labs ( db - dc ) ;
block - > steps [ C_AXIS ] = ABS ( db - dc ) ;
# elif IS_SCARA
# elif IS_SCARA
block - > steps [ A_AXIS ] = labs ( da ) ;
block - > steps [ A_AXIS ] = ABS ( da ) ;
block - > steps [ B_AXIS ] = labs ( db ) ;
block - > steps [ B_AXIS ] = ABS ( db ) ;
block - > steps [ Z_AXIS ] = labs ( dc ) ;
block - > steps [ Z_AXIS ] = ABS ( dc ) ;
# else
# else
// default non-h-bot planning
// default non-h-bot planning
block - > steps [ A_AXIS ] = labs ( da ) ;
block - > steps [ A_AXIS ] = ABS ( da ) ;
block - > steps [ B_AXIS ] = labs ( db ) ;
block - > steps [ B_AXIS ] = ABS ( db ) ;
block - > steps [ C_AXIS ] = labs ( dc ) ;
block - > steps [ C_AXIS ] = ABS ( dc ) ;
# endif
# endif
block - > steps [ E_AXIS ] = esteps ;
block - > steps [ E_AXIS ] = esteps ;
@ -1660,7 +1660,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
delta_mm [ E_AXIS ] = esteps_float * steps_to_mm [ E_AXIS_N ] ;
delta_mm [ E_AXIS ] = esteps_float * steps_to_mm [ E_AXIS_N ] ;
if ( block - > steps [ A_AXIS ] < MIN_STEPS_PER_SEGMENT & & block - > steps [ B_AXIS ] < MIN_STEPS_PER_SEGMENT & & block - > steps [ C_AXIS ] < MIN_STEPS_PER_SEGMENT ) {
if ( block - > steps [ A_AXIS ] < MIN_STEPS_PER_SEGMENT & & block - > steps [ B_AXIS ] < MIN_STEPS_PER_SEGMENT & & block - > steps [ C_AXIS ] < MIN_STEPS_PER_SEGMENT ) {
block - > millimeters = F ABS( delta_mm [ E_AXIS ] ) ;
block - > millimeters = ABS ( delta_mm [ E_AXIS ] ) ;
}
}
else if ( ! millimeters ) {
else if ( ! millimeters ) {
block - > millimeters = SQRT (
block - > millimeters = SQRT (
@ -1751,7 +1751,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// 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 ] , speed_factor = 1.0 ; // factor <1 decreases speed
float current_speed [ NUM_AXIS ] , speed_factor = 1.0 ; // factor <1 decreases speed
LOOP_XYZE ( i ) {
LOOP_XYZE ( i ) {
const float cs = F ABS( ( current_speed [ i ] = delta_mm [ i ] * inverse_secs ) ) ;
const float cs = ABS ( ( current_speed [ i ] = delta_mm [ i ] * inverse_secs ) ) ;
# if ENABLED(DISTINCT_E_FACTORS)
# if ENABLED(DISTINCT_E_FACTORS)
if ( i = = E_AXIS ) i + = extruder ;
if ( i = = E_AXIS ) i + = extruder ;
# endif
# endif
@ -1789,7 +1789,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
const uint32_t max_x_segment_time = MAX3 ( xs0 , xs1 , xs2 ) ,
const uint32_t max_x_segment_time = MAX3 ( xs0 , xs1 , xs2 ) ,
max_y_segment_time = MAX3 ( ys0 , ys1 , ys2 ) ,
max_y_segment_time = MAX3 ( ys0 , ys1 , ys2 ) ,
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_US ) {
if ( min_xy_segment_time < MAX_FREQ_TIME_US ) {
const float low_sf = speed_factor * min_xy_segment_time / ( MAX_FREQ_TIME_US ) ;
const float low_sf = speed_factor * min_xy_segment_time / ( MAX_FREQ_TIME_US ) ;
NOMORE ( speed_factor , low_sf ) ;
NOMORE ( speed_factor , low_sf ) ;
@ -1973,7 +1973,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
vmax_junction = MINIMUM_PLANNER_SPEED ;
vmax_junction = MINIMUM_PLANNER_SPEED ;
}
}
else {
else {
junction_cos_theta = max ( junction_cos_theta , - 0.999999 ) ; // Check for numerical round-off to avoid divide by zero.
junction_cos_theta = MAX ( junction_cos_theta , - 0.999999 ) ; // Check for numerical round-off to avoid divide by zero.
const float sin_theta_d2 = SQRT ( 0.5 * ( 1.0 - junction_cos_theta ) ) ; // Trig half angle identity. Always positive.
const float sin_theta_d2 = SQRT ( 0.5 * ( 1.0 - junction_cos_theta ) ) ; // Trig half angle identity. Always positive.
// TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the
// TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the
@ -2003,7 +2003,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
float safe_speed = block - > nominal_speed ;
float safe_speed = block - > nominal_speed ;
uint8_t limited = 0 ;
uint8_t limited = 0 ;
LOOP_XYZE ( i ) {
LOOP_XYZE ( i ) {
const float jerk = F ABS( current_speed [ i ] ) , maxj = max_jerk [ i ] ;
const float jerk = ABS ( current_speed [ i ] ) , maxj = max_jerk [ i ] ;
if ( jerk > maxj ) {
if ( jerk > maxj ) {
if ( limited ) {
if ( limited ) {
const float mjerk = maxj * block - > nominal_speed ;
const float mjerk = maxj * block - > nominal_speed ;
@ -2023,7 +2023,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
// The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
// Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
// Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
vmax_junction = min ( block - > nominal_speed , previous_nominal_speed ) ;
vmax_junction = MIN ( block - > nominal_speed , previous_nominal_speed ) ;
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
float v_factor = 1 ;
float v_factor = 1 ;
@ -2043,9 +2043,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
// Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
const float jerk = ( v_exit > v_entry )
const float jerk = ( v_exit > v_entry )
? // coasting axis reversal
? // coasting axis reversal
( ( v_entry > 0 | | v_exit < 0 ) ? ( v_exit - v_entry ) : max ( v_exit , - v_entry ) )
( ( v_entry > 0 | | v_exit < 0 ) ? ( v_exit - v_entry ) : MAX ( v_exit , - v_entry ) )
: // v_exit <= v_entry coasting axis reversal
: // v_exit <= v_entry coasting axis reversal
( ( v_entry < 0 | | v_exit > 0 ) ? ( v_entry - v_exit ) : max ( - v_exit , v_entry ) ) ;
( ( v_entry < 0 | | v_exit > 0 ) ? ( v_entry - v_exit ) : MAX ( - v_exit , v_entry ) ) ;
if ( jerk > max_jerk [ axis ] ) {
if ( jerk > max_jerk [ axis ] ) {
v_factor * = max_jerk [ axis ] / jerk ;
v_factor * = max_jerk [ axis ] / jerk ;
@ -2072,7 +2072,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
const float v_allowable = max_allowable_speed ( - block - > acceleration , MINIMUM_PLANNER_SPEED , block - > millimeters ) ;
const float v_allowable = max_allowable_speed ( - block - > acceleration , MINIMUM_PLANNER_SPEED , block - > millimeters ) ;
// If stepper ISR is disabled, this indicates buffer_segment wants to add a split block.
// If stepper ISR is disabled, this indicates buffer_segment wants to add a split block.
// In this case start with the max. allowed speed to avoid an interrupted first move.
// In this case start with the max. allowed speed to avoid an interrupted first move.
block - > entry_speed = STEPPER_ISR_ENABLED ( ) ? MINIMUM_PLANNER_SPEED : min ( vmax_junction , v_allowable ) ;
block - > entry_speed = STEPPER_ISR_ENABLED ( ) ? MINIMUM_PLANNER_SPEED : MIN ( vmax_junction , v_allowable ) ;
// Initialize planner efficiency flags
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.