From 2fd7c2b8652a3250e1eb1dbfd96991b4b11f6257 Mon Sep 17 00:00:00 2001 From: Mihai <299015+mh-dm@users.noreply.github.com> Date: Fri, 17 May 2024 04:48:21 +0300 Subject: [PATCH] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Fix=20motion=20smoothness?= =?UTF-8?q?=20(#27013)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/module/planner.cpp | 40 +++++++++++++++----------- Marlin/src/module/planner.h | 4 +-- Marlin/src/module/stepper.cpp | 53 ++++++++++++++++++----------------- Marlin/src/module/stepper.h | 4 +-- 4 files changed, 55 insertions(+), 46 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 6b4f63fb00..8f11f4ec18 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -796,12 +796,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.) - NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE)); + // Legacy check against supposed timer overflow. However Stepper::calc_timer_interval() already + // 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. + 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 @@ -809,9 +814,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; @@ -820,10 +825,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 = FLOAT_SQ(block->nominal_rate), // Steps required for acceleration, deceleration to/from nominal rate - decelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(final_rate)); - float accelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(initial_rate)); + decelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(final_rate)), + accelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(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; @@ -833,13 +839,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 } } @@ -855,8 +861,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; @@ -3158,8 +3164,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 a06dd50e04..fd26b4340e 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -238,8 +238,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 90254ff947..c2674480d5 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 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. + * + * 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. */ /** @@ -193,6 +199,7 @@ bool Stepper::abort_current_block; ; #endif +// In timer_ticks uint32_t Stepper::acceleration_time, Stepper::deceleration_time; #if MULTISTEPPING_LIMIT > 1 @@ -224,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) @@ -2403,7 +2410,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!) @@ -2420,6 +2427,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); @@ -2456,30 +2464,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_start) { 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); } @@ -2542,6 +2544,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); @@ -2664,9 +2669,6 @@ hal_timer_t Stepper::block_phase_isr() { // Set flags for all moving axes, accounting for kinematics set_axis_moved_for_current_block(); - // 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) @@ -2720,8 +2722,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)); @@ -2807,7 +2809,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); diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 43d1012f10..82b41290bf 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)