3DPrinting/MarlinFirmware
0
0
Fork 0
mirror of https://github.com/MarlinFirmware/Marlin.git synced 2025-01-23 01:58:59 +00:00
Code Issues Releases Wiki Activity

⚡️ Fix motion smoothness (#27013)

Browse source
This commit is contained in:
Mihai 2024-05-17 04:48:21 +03:00 committed by GitHub
parent ef0bd975cf
commit 2fd7c2b865
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
4 changed files with 55 additions and 46 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

40
Marlin/src/module/planner.cpp
View file

@ -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) { 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), uint32_t initial_rate = LROUND(block->nominal_rate * entry_factor),
final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second) 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
NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE)); // 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)); 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 ANY(S_CURVE_ACCELERATION, LIN_ADVANCE)
// If we have some plateau time, the cruise rate will be the nominal rate // 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 #endif
// Steps for acceleration, plateau and deceleration // Steps for acceleration, plateau and deceleration
int32_t plateau_steps = block->step_event_count; int32_t plateau_steps = block->step_event_count,
uint32_t accelerate_steps = 0, accelerate_steps = 0,
decelerate_steps = 0; decelerate_steps = 0;
const int32_t accel = block->acceleration_steps_per_s2; const int32_t accel = block->acceleration_steps_per_s2;
float inverse_accel = 0.0f; 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, const float half_inverse_accel = 0.5f * inverse_accel,
nominal_rate_sq = FLOAT_SQ(block->nominal_rate), nominal_rate_sq = FLOAT_SQ(block->nominal_rate),
// Steps required for acceleration, deceleration to/from nominal rate // Steps required for acceleration, deceleration to/from nominal rate
decelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(final_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)); 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); 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 // Steps between acceleration and deceleration, if any
plateau_steps -= accelerate_steps + decelerate_steps; 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 // Calculate accel / braking time in order to reach the final_rate exactly
// at the end of this block. // at the end of this block.
if (plateau_steps < 0) { if (plateau_steps < 0) {
accelerate_steps_float = CEIL((block->step_event_count + accelerate_steps_float - decelerate_steps_float) * 0.5f); accelerate_steps = LROUND((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); LIMIT(accelerate_steps, 0, int32_t(block->step_event_count));
decelerate_steps = block->step_event_count - accelerate_steps; decelerate_steps = block->step_event_count - accelerate_steps;
#if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE)
// We won't reach the cruising rate. Let's calculate the speed we will reach // 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 #endif
} }
} }
@ -855,8 +861,8 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
#endif #endif
// Store new block parameters // Store new block parameters
block->accelerate_until = accelerate_steps; block->accelerate_before = accelerate_steps;
block->decelerate_after = block->step_event_count - decelerate_steps; block->decelerate_start = block->step_event_count - decelerate_steps;
block->initial_rate = initial_rate; block->initial_rate = initial_rate;
#if ENABLED(S_CURVE_ACCELERATION) #if ENABLED(S_CURVE_ACCELERATION)
block->acceleration_time = acceleration_time; 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->step_event_count = num_steps;
block->initial_rate = block->final_rate = block->nominal_rate = last_page_step_rate; // steps/s block->initial_rate = block->final_rate = block->nominal_rate = last_page_step_rate; // steps/s
block->accelerate_until = 0; block->accelerate_before = 0;
block->decelerate_after = block->step_event_count; block->decelerate_start = block->step_event_count;
// Will be set to last direction later if directional format. // Will be set to last direction later if directional format.
block->direction_bits.reset(); block->direction_bits.reset();

4
Marlin/src/module/planner.h
View file

@ -238,8 +238,8 @@ typedef struct PlannerBlock {
#endif #endif
// Settings for the trapezoid generator // Settings for the trapezoid generator
uint32_t accelerate_until, // The index of the step event on which to stop acceleration uint32_t accelerate_before, // The index of the step event where cruising starts
decelerate_after; // The index of the step event on which to start decelerating decelerate_start; // The index of the step event on which to start decelerating
#if ENABLED(S_CURVE_ACCELERATION) #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 uint32_t cruise_rate, // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase

53
Marlin/src/module/stepper.cpp
View file

@ -58,10 +58,16 @@
* *
* time -----> * time ----->
* *
* The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates * The speed over time graph forms a TRAPEZOID. The slope of acceleration is calculated by
* first block->accelerate_until step_events_completed, then keeps going at constant speed until * v = u + t
* step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset. * where 't' is the accumulated timer values of the steps so far.
* The slope of acceleration is calculated using v = u + at 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 #endif
// In timer_ticks
uint32_t Stepper::acceleration_time, Stepper::deceleration_time; uint32_t Stepper::acceleration_time, Stepper::deceleration_time;
#if MULTISTEPPING_LIMIT > 1 #if MULTISTEPPING_LIMIT > 1
@ -224,8 +231,8 @@ xyze_long_t Stepper::delta_error{0};
xyze_long_t Stepper::advance_dividend{0}; xyze_long_t Stepper::advance_dividend{0};
uint32_t Stepper::advance_divisor = 0, uint32_t Stepper::advance_divisor = 0,
Stepper::step_events_completed = 0, // The number of step events executed in the current block 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::accelerate_before, // The count at which to start cruising
Stepper::decelerate_after, // The count at which to start decelerating Stepper::decelerate_start, // The count at which to start decelerating
Stepper::step_event_count; // The total event count for the current block Stepper::step_event_count; // The total event count for the current block
#if ANY(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER) #if ANY(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER)
@ -2403,7 +2410,7 @@ hal_timer_t Stepper::block_phase_isr() {
// Step events not completed yet... // Step events not completed yet...
// Are we in acceleration phase ? // 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) #if ENABLED(S_CURVE_ACCELERATION)
// Get the next speed to use (Jerk limited!) // 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 // step_rate to timer interval and steps per stepper isr
interval = calc_multistep_timer_interval(acc_step_rate << oversampling_factor); interval = calc_multistep_timer_interval(acc_step_rate << oversampling_factor);
acceleration_time += interval; acceleration_time += interval;
deceleration_time = 0; // Reset since we're doing acceleration first.
#if ENABLED(NONLINEAR_EXTRUSION) #if ENABLED(NONLINEAR_EXTRUSION)
calc_nonlinear_e(acc_step_rate << oversampling_factor); calc_nonlinear_e(acc_step_rate << oversampling_factor);
@ -2456,30 +2464,24 @@ hal_timer_t Stepper::block_phase_isr() {
#endif #endif
} }
// Are we in Deceleration phase ? // Are we in Deceleration phase ?
else if (step_events_completed > decelerate_after) { else if (step_events_completed >= decelerate_start) {
uint32_t step_rate; uint32_t step_rate;
#if ENABLED(S_CURVE_ACCELERATION) #if ENABLED(S_CURVE_ACCELERATION)
// If this is the 1st time we process the 2nd half of the trapezoid... // If this is the 1st time we process the 2nd half of the trapezoid...
if (!bezier_2nd_half) { if (!bezier_2nd_half) {
// Initialize the Bézier speed curve // Initialize the Bézier speed curve
_calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse); _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse);
bezier_2nd_half = true; 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
// Calculate the next speed to use step_rate = deceleration_time < current_block->deceleration_time
step_rate = deceleration_time < current_block->deceleration_time ? _eval_bezier_curve(deceleration_time)
? _eval_bezier_curve(deceleration_time) : current_block->final_rate;
: current_block->final_rate;
}
#else #else
// Using the old trapezoidal control // Using the old trapezoidal control
step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate); 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; step_rate = acc_step_rate - step_rate;
NOLESS(step_rate, current_block->final_rate); NOLESS(step_rate, current_block->final_rate);
} }
@ -2542,6 +2544,9 @@ hal_timer_t Stepper::block_phase_isr() {
if (ticks_nominal == 0) { if (ticks_nominal == 0) {
// step_rate to timer interval and loops for the nominal speed // step_rate to timer interval and loops for the nominal speed
ticks_nominal = calc_multistep_timer_interval(current_block->nominal_rate << oversampling_factor); 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) #if ENABLED(NONLINEAR_EXTRUSION)
calc_nonlinear_e(current_block->nominal_rate << oversampling_factor); 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 flags for all moving axes, accounting for kinematics
set_axis_moved_for_current_block(); set_axis_moved_for_current_block();
// No acceleration / deceleration time elapsed so far
acceleration_time = deceleration_time = 0;
#if ENABLED(ADAPTIVE_STEP_SMOOTHING) #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
// Nonlinear Extrusion needs at least 2x oversampling to permit increase of E step rate // Nonlinear Extrusion needs at least 2x oversampling to permit increase of E step rate
// Otherwise assume no axis smoothing (via oversampling) // Otherwise assume no axis smoothing (via oversampling)
@ -2720,8 +2722,8 @@ hal_timer_t Stepper::block_phase_isr() {
step_events_completed = 0; step_events_completed = 0;
// Compute the acceleration and deceleration points // Compute the acceleration and deceleration points
accelerate_until = current_block->accelerate_until << oversampling_factor; accelerate_before = current_block->accelerate_before << oversampling_factor;
decelerate_after = current_block->decelerate_after << oversampling_factor; decelerate_start = current_block->decelerate_start << oversampling_factor;
TERN_(MIXING_EXTRUDER, mixer.stepper_setup(current_block->b_color)); 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 // Calculate the initial timer interval
interval = calc_multistep_timer_interval(current_block->initial_rate << oversampling_factor); 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) #if ENABLED(NONLINEAR_EXTRUSION)
calc_nonlinear_e(current_block->initial_rate << oversampling_factor); calc_nonlinear_e(current_block->initial_rate << oversampling_factor);

4
Marlin/src/module/stepper.h
View file

@ -391,8 +391,8 @@ class Stepper {
static xyze_long_t advance_dividend; static xyze_long_t advance_dividend;
static uint32_t advance_divisor, static uint32_t advance_divisor,
step_events_completed, // The number of step events executed in the current block step_events_completed, // The number of step events executed in the current block
accelerate_until, // The point from where we need to stop acceleration accelerate_before, // The count at which to start cruising
decelerate_after, // The point from where we need to start decelerating decelerate_start, // The count at which to start decelerating
step_event_count; // The total event count for the current block step_event_count; // The total event count for the current block
#if ANY(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER) #if ANY(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER)