jessikitty/Marlin
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Smoother motion by fixing calculating trapezoids and ISR stepping.

Browse Source 
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
This commit is contained in:
Mihail Dumitrescu
2024-04-16 13:06:43 +03:00
parent 02ba6f9f3a
commit 549a4f48f9
2 changed files with 32 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Marlin/src/module/planner.cpp
+18 -12
View File
@@ -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
}
}
Marlin/src/module/stepper.cpp
+14 -17
View File
@@ -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);