From 549a4f48f96aa021a1ab3b3fa75dd903f423bcd8 Mon Sep 17 00:00:00 2001 From: Mihail Dumitrescu Date: Tue, 16 Apr 2024 13:06:43 +0300 Subject: [PATCH 1/6] Smoother motion by fixing calculating trapezoids and ISR stepping. Fix rounding directions in calculate_trapezoid_for_block(). Fix off-by-ones errors in ac/deceleration steps in block_phase_isr. Half-initialize ac/deceleration_time to smooth the speed change shock that happens between segments, which is critical as jerk/deviation adds to this. The result is a smoother motion profile that follows the imposed acceleration limits with a well defined 0.5-1.5x error factor (or 2x if axis is starting from ~0). Errors are due to converting a real-valued motion profile into discrete numbers of steps. Fixes are general and improve S_CURVE_ACCELERATION too (no endorsement implied). Tested by looking at the generated step/dir impulses with a logic analyzer. Enjoy the smoother motion or use more aggresive acceleration/jerk/deviation values for faster prints. Also improves: #12491 --- Marlin/src/module/planner.cpp | 30 ++++++++++++++++++------------ Marlin/src/module/stepper.cpp | 31 ++++++++++++++----------------- 2 files changed, 32 insertions(+), 29 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index c21230f662b3..a752ba017828 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 6426c7f4a2e3..0e564b0df6b5 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); From 1f3f416bd23533e5c450ebb2289f41a5591db5b6 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Wed, 24 Apr 2024 18:51:23 -0500 Subject: [PATCH 2/6] semantics changed --- Marlin/src/module/planner.cpp | 8 ++++---- Marlin/src/module/planner.h | 4 ++-- Marlin/src/module/stepper.cpp | 28 +++++++++++++++++----------- Marlin/src/module/stepper.h | 4 ++-- 4 files changed, 25 insertions(+), 19 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index a752ba017828..dbe058d75fa9 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -859,8 +859,8 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t #endif // Store new block parameters - block->accelerate_until = accelerate_steps; - block->decelerate_after = block->step_event_count - decelerate_steps; + block->accelerate_before = accelerate_steps; + block->decelerate_start = block->step_event_count - decelerate_steps; block->initial_rate = initial_rate; #if ENABLED(S_CURVE_ACCELERATION) block->acceleration_time = acceleration_time; @@ -3196,8 +3196,8 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s block->step_event_count = num_steps; block->initial_rate = block->final_rate = block->nominal_rate = last_page_step_rate; // steps/s - block->accelerate_until = 0; - block->decelerate_after = block->step_event_count; + block->accelerate_before = 0; + block->decelerate_start = block->step_event_count; // Will be set to last direction later if directional format. block->direction_bits.reset(); diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index b7b1abbb6153..4bc3d3be04e9 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -244,8 +244,8 @@ typedef struct PlannerBlock { #endif // Settings for the trapezoid generator - uint32_t accelerate_until, // The index of the step event on which to stop acceleration - decelerate_after; // The index of the step event on which to start decelerating + uint32_t accelerate_before, // The index of the step event where cruising starts + decelerate_start; // The index of the step event on which to start decelerating #if ENABLED(S_CURVE_ACCELERATION) uint32_t cruise_rate, // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 0e564b0df6b5..6df7dcd4c211 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -58,10 +58,16 @@ * * time -----> * - * The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates - * first block->accelerate_until step_events_completed, then keeps going at constant speed until - * step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset. - * The slope of acceleration is calculated using v = u + at where t is the accumulated timer values of the steps so far. + * the shape the speed curve over time forms a TRAPEZOID. The slope of acceleration is calculated by + * v = u + t + * where 't' is the accumulated timer values of the steps so far. + * + * The Stepper ISR dynamically executes acceleration, deceleration, and cruising according to the block parameters. + * - Start at block->initial_rate. + * - Accelerate while step_events_completed < block->accelerate_before. + * - Cruise while step_events_completed < block->decelerate_start. + * - Decelerate after that, until all steps are completed. + * - Reset the trapezoid generator. */ /** @@ -225,8 +231,8 @@ xyze_long_t Stepper::delta_error{0}; xyze_long_t Stepper::advance_dividend{0}; uint32_t Stepper::advance_divisor = 0, Stepper::step_events_completed = 0, // The number of step events executed in the current block - Stepper::accelerate_until, // The count at which to stop accelerating - Stepper::decelerate_after, // The count at which to start decelerating + Stepper::accelerate_before, // The count at which to start cruising + Stepper::decelerate_start, // The count at which to start decelerating Stepper::step_event_count; // The total event count for the current block #if ANY(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER) @@ -2300,7 +2306,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_before) { // Calculate new timer value #if ENABLED(S_CURVE_ACCELERATION) // Get the next speed to use (Jerk limited!) @@ -2357,7 +2363,7 @@ 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_start) { uint32_t step_rate; #if ENABLED(S_CURVE_ACCELERATION) @@ -2462,7 +2468,7 @@ hal_timer_t Stepper::block_phase_isr() { */ #if ENABLED(LASER_POWER_TRAP) if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { - if (step_events_completed + 1 == accelerate_until) { + if (step_events_completed + 1 == accelerate_before) { if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { if (current_block->laser.trap_ramp_entry_incr > 0) { current_block->laser.trap_ramp_active_pwr = current_block->laser.power; @@ -2692,8 +2698,8 @@ hal_timer_t Stepper::block_phase_isr() { step_events_completed = 0; // Compute the acceleration and deceleration points - accelerate_until = current_block->accelerate_until << oversampling_factor; - decelerate_after = current_block->decelerate_after << oversampling_factor; + accelerate_before = current_block->accelerate_before << oversampling_factor; + decelerate_start = current_block->decelerate_start << oversampling_factor; TERN_(MIXING_EXTRUDER, mixer.stepper_setup(current_block->b_color)); diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 84f85391d2bf..d69d007366e3 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -391,8 +391,8 @@ class Stepper { static xyze_long_t advance_dividend; static uint32_t advance_divisor, step_events_completed, // The number of step events executed in the current block - accelerate_until, // The point from where we need to stop acceleration - decelerate_after, // The point from where we need to start decelerating + accelerate_before, // The count at which to start cruising + decelerate_start, // The count at which to start decelerating step_event_count; // The total event count for the current block #if ANY(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER) From b9c8e24f678efe29d750c59b42d5cd2d9b4f0d13 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Wed, 24 Apr 2024 19:06:51 -0500 Subject: [PATCH 3/6] NOLESS+NOMORE=LIMIT --- Marlin/src/module/planner.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index dbe058d75fa9..8f7a80bfaf80 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -801,10 +801,8 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t // 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); + LIMIT(initial_rate, uint32_t(MINIMAL_STEP_RATE), block->nominal_rate); + LIMIT(final_rate, uint32_t(MINIMAL_STEP_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 From 30ece3d9e7338f8c947c9a4116f30b08ed5fa0d6 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Wed, 24 Apr 2024 19:08:58 -0500 Subject: [PATCH 4/6] worms --- Marlin/src/module/stepper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 6df7dcd4c211..4edbd6a189c2 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -58,7 +58,7 @@ * * time -----> * - * the shape the speed curve over time forms a TRAPEZOID. The slope of acceleration is calculated by + * The speed over time graph forms a TRAPEZOID. The slope of acceleration is calculated by * v = u + t * where 't' is the accumulated timer values of the steps so far. * From 89d7e171466b3437dd288d8d6ba5f6c4f6025ac3 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Wed, 24 Apr 2024 19:11:16 -0500 Subject: [PATCH 5/6] worms! --- Marlin/src/module/planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 8f7a80bfaf80..5de6e535de75 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -798,7 +798,7 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t final_rate = LROUND(block->nominal_rate * exit_factor); // (steps per second) // 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 + // should protect against it. But removing this code produces judder in direction-switching // 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. LIMIT(initial_rate, uint32_t(MINIMAL_STEP_RATE), block->nominal_rate); From 2fb1a2b7a3ad12b60eb4123a609944f6fc1cb367 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 26 Apr 2024 19:24:03 -0500 Subject: [PATCH 6/6] correction --- Marlin/src/module/planner.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 5de6e535de75..1f3e4069aa3c 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -801,8 +801,10 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t // should protect against it. But removing this code produces judder in direction-switching // 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. - LIMIT(initial_rate, uint32_t(MINIMAL_STEP_RATE), block->nominal_rate); - LIMIT(final_rate, uint32_t(MINIMAL_STEP_RATE), block->nominal_rate); + NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE)); // Enforce the minimum speed + NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE)); + NOMORE(initial_rate, block->nominal_rate); // NOTE: The nominal rate may be less than MINIMAL_STEP_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