diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index c21230f662..a752ba0178 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -794,12 +794,17 @@ block_t* Planner::get_current_block() { */ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t entry_factor, const_float_t exit_factor) { - uint32_t initial_rate = CEIL(block->nominal_rate * entry_factor), - final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second) + uint32_t initial_rate = LROUND(block->nominal_rate * entry_factor), + final_rate = LROUND(block->nominal_rate * exit_factor); // (steps per second) - // Limit minimal step rate (Otherwise the timer will overflow.) + // Legacy check against supposed timer overflow. However Stepper::calc_timer_interval() already + // should protect against it. But removing results in less smooth motion for switching direction + // moves. This is because the current discrete stepping math diverges from physical motion under + // constant acceleration when acceleration_steps_per_s2 is large compared to initial/final_rate. NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE)); NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE)); + NOMORE(initial_rate, block->nominal_rate); + NOMORE(final_rate, block->nominal_rate); #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) // If we have some plateau time, the cruise rate will be the nominal rate @@ -807,9 +812,9 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t #endif // Steps for acceleration, plateau and deceleration - int32_t plateau_steps = block->step_event_count; - uint32_t accelerate_steps = 0, - decelerate_steps = 0; + int32_t plateau_steps = block->step_event_count, + accelerate_steps = 0, + decelerate_steps = 0; const int32_t accel = block->acceleration_steps_per_s2; float inverse_accel = 0.0f; @@ -818,10 +823,11 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t const float half_inverse_accel = 0.5f * inverse_accel, nominal_rate_sq = sq(float(block->nominal_rate)), // Steps required for acceleration, deceleration to/from nominal rate - decelerate_steps_float = half_inverse_accel * (nominal_rate_sq - sq(float(final_rate))); - float accelerate_steps_float = half_inverse_accel * (nominal_rate_sq - sq(float(initial_rate))); + decelerate_steps_float = half_inverse_accel * (nominal_rate_sq - sq(float(final_rate))), + accelerate_steps_float = half_inverse_accel * (nominal_rate_sq - sq(float(initial_rate))); + // Aims to fully reach nominal and final rates accelerate_steps = CEIL(accelerate_steps_float); - decelerate_steps = FLOOR(decelerate_steps_float); + decelerate_steps = CEIL(decelerate_steps_float); // Steps between acceleration and deceleration, if any plateau_steps -= accelerate_steps + decelerate_steps; @@ -831,13 +837,13 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t // Calculate accel / braking time in order to reach the final_rate exactly // at the end of this block. if (plateau_steps < 0) { - accelerate_steps_float = CEIL((block->step_event_count + accelerate_steps_float - decelerate_steps_float) * 0.5f); - accelerate_steps = _MIN(uint32_t(_MAX(accelerate_steps_float, 0)), block->step_event_count); + accelerate_steps = LROUND((block->step_event_count + accelerate_steps_float - decelerate_steps_float) * 0.5f); + LIMIT(accelerate_steps, 0, int32_t(block->step_event_count)); decelerate_steps = block->step_event_count - accelerate_steps; #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) // We won't reach the cruising rate. Let's calculate the speed we will reach - cruise_rate = final_speed(initial_rate, accel, accelerate_steps); + NOMORE(cruise_rate, final_speed(initial_rate, accel, accelerate_steps)); #endif } } diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 6426c7f4a2..0e564b0df6 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -193,6 +193,7 @@ bool Stepper::abort_current_block; ; #endif +// In timer_ticks uint32_t Stepper::acceleration_time, Stepper::deceleration_time; #if MULTISTEPPING_LIMIT > 1 @@ -2299,7 +2300,7 @@ hal_timer_t Stepper::block_phase_isr() { // Step events not completed yet... // Are we in acceleration phase ? - if (step_events_completed <= accelerate_until) { // Calculate new timer value + if (step_events_completed < accelerate_until) { // Calculate new timer value #if ENABLED(S_CURVE_ACCELERATION) // Get the next speed to use (Jerk limited!) @@ -2316,6 +2317,7 @@ hal_timer_t Stepper::block_phase_isr() { // step_rate to timer interval and steps per stepper isr interval = calc_multistep_timer_interval(acc_step_rate << oversampling_factor); acceleration_time += interval; + deceleration_time = 0; // Reset since we're doing acceleration first. #if ENABLED(NONLINEAR_EXTRUSION) calc_nonlinear_e(acc_step_rate << oversampling_factor); @@ -2355,30 +2357,24 @@ hal_timer_t Stepper::block_phase_isr() { #endif } // Are we in Deceleration phase ? - else if (step_events_completed > decelerate_after) { + else if (step_events_completed >= decelerate_after) { uint32_t step_rate; #if ENABLED(S_CURVE_ACCELERATION) - // If this is the 1st time we process the 2nd half of the trapezoid... if (!bezier_2nd_half) { // Initialize the Bézier speed curve _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse); bezier_2nd_half = true; - // The first point starts at cruise rate. Just save evaluation of the Bézier curve - step_rate = current_block->cruise_rate; } - else { - // Calculate the next speed to use - step_rate = deceleration_time < current_block->deceleration_time - ? _eval_bezier_curve(deceleration_time) - : current_block->final_rate; - } - + // Calculate the next speed to use + step_rate = deceleration_time < current_block->deceleration_time + ? _eval_bezier_curve(deceleration_time) + : current_block->final_rate; #else // Using the old trapezoidal control step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate); - if (step_rate < acc_step_rate) { // Still decelerating? + if (step_rate < acc_step_rate) { step_rate = acc_step_rate - step_rate; NOLESS(step_rate, current_block->final_rate); } @@ -2442,6 +2438,9 @@ hal_timer_t Stepper::block_phase_isr() { if (ticks_nominal == 0) { // step_rate to timer interval and loops for the nominal speed ticks_nominal = calc_multistep_timer_interval(current_block->nominal_rate << oversampling_factor); + // Prepare for deceleration + IF_DISABLED(S_CURVE_ACCELERATION, acc_step_rate = current_block->nominal_rate); + deceleration_time = ticks_nominal / 2; #if ENABLED(NONLINEAR_EXTRUSION) calc_nonlinear_e(current_block->nominal_rate << oversampling_factor); @@ -2640,9 +2639,6 @@ hal_timer_t Stepper::block_phase_isr() { ); axis_did_move = didmove; - // No acceleration / deceleration time elapsed so far - acceleration_time = deceleration_time = 0; - #if ENABLED(ADAPTIVE_STEP_SMOOTHING) // Nonlinear Extrusion needs at least 2x oversampling to permit increase of E step rate // Otherwise assume no axis smoothing (via oversampling) @@ -2783,7 +2779,8 @@ hal_timer_t Stepper::block_phase_isr() { // Calculate the initial timer interval interval = calc_multistep_timer_interval(current_block->initial_rate << oversampling_factor); - acceleration_time += interval; + // Initialize ac/deceleration time as if half the time passed. + acceleration_time = deceleration_time = interval / 2; #if ENABLED(NONLINEAR_EXTRUSION) calc_nonlinear_e(current_block->initial_rate << oversampling_factor);