Skip to content

Smoother motion by fixing calculating trapezoids and ISR stepping. #27013

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 9 commits into from
May 17, 2024
Merged

Smoother motion by fixing calculating trapezoids and ISR stepping. #27013

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
549a4f4
Smoother motion by fixing calculating trapezoids and ISR stepping.
mh-dm Apr 16, 2024
1f3f416
semantics changed
thinkyhead Apr 24, 2024
b9c8e24
NOLESS+NOMORE=LIMIT
thinkyhead Apr 25, 2024
30ece3d
worms
thinkyhead Apr 25, 2024
89d7e17
worms!
thinkyhead Apr 25, 2024
2fb1a2b
correction
thinkyhead Apr 27, 2024
81686a3
Merge branch 'bugfix-2.1.x' into pr/27013
thinkyhead May 11, 2024
4b41cd8
Merge branch 'bugfix-2.1.x' into pr/27013
thinkyhead May 17, 2024
2519e73
Merge branch 'bugfix-2.1.x' into pr/27013
thinkyhead May 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 24 additions & 18 deletions Marlin/src/module/planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -794,22 +794,27 @@ 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)

// Limit minimal step rate (Otherwise the timer will overflow.)
NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE));
uint32_t initial_rate = LROUND(block->nominal_rate * entry_factor),
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 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
uint32_t cruise_rate = block->nominal_rate;
#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;
Expand All @@ -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;
Expand All @@ -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
}
}
Expand All @@ -853,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;
Expand Down Expand Up @@ -3190,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();
Expand Down
4 changes: 2 additions & 2 deletions Marlin/src/module/planner.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
55 changes: 29 additions & 26 deletions Marlin/src/module/stepper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*/

/**
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -2299,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!)
Expand All @@ -2316,6 +2323,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);
Expand Down Expand Up @@ -2355,30 +2363,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);
}
Expand Down Expand Up @@ -2442,6 +2444,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);
Expand All @@ -2463,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) {
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Well, the new naming did highlight this: I think it should be without the +1 but I'm not sure.

Copy link
Member

@thinkyhead thinkyhead Apr 27, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We'll have to analyze the intent of this line and look at its history. Previously it wanted to match accelerate_until - 1 which would be the second-to-last step of acceleration. Assuming that we want no change in behavior, and assuming that accelerate_before is now one more than accelerate_until, the actual correction would be step_events_completed + 2 == accelerate_before.

Again, we need to look closer at the original intent of the line, because, for one thing, step_events_completed is not incremented by 1, but by events_to_do which may or may not always line up correctly to match the exact index accelerate_before - n. Once we fully grok the intent of this line we can simply replace it with whatever best fits the updated code.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Let's see if @descipher has anything to add….

Copy link
Contributor

@descipher descipher Apr 27, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Well it's been quite some time since my head was deep inside this code! "first block->accelerate_until step_events_completed" is the key element, since the first block starts at 0 we will have value 1 after the ISR does the count increment, thus the < accelerate_until naming usage which is not the same meaning as accelerate_before, it only looks that way when viewing it as less than accelerate_until at the moment of comparison. However this comparison moment is the actual accelerate_until PIT and not before.

In regards to the +1 == if I recall correctly the reason for + 1 stems from an odd step count and the PIT when the laser would need to be turned off or incr/decr . It worked out that the counts were shifted toward the tail of the trap so the counts were consistently short on the decel side of the trap. This can be viewed by turning on debug where it will give you the actual step counts. It becomes obvious with debug on and the simulator is your friend for that ;) The laser power will be inc'd and dec'd closer to ideal with the +1. Keep in mind its not uniform in a real world print or laser burn, but in tests from a dead start to full stop the ramps should be equal and that's when this shifted count behavior was first observed. If there is a change in the counts/shaping now it should be re-evaluated again.

Copy link
Member

@thinkyhead thinkyhead Apr 27, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My worry is that with some moves it will never be that step_events_completed + 1 == accelerate_before during the block_phase_isr because AFAIK this code is not guaranteed to be executed after every step, but only when some new "block phase" is "needed." And, as mentioned, step_events_completed is not incremented by 1, but by events_to_do.

The best thing to do would be to test this in the simulator, not with laser enabled, but just with that comparison being done at that same point in the code to see how it matches up and whether we are in fact guaranteed to be able to scrutinize every step index. If we can, then the very last index of acceleration will be accelerate_before - 1 and we can apply that correction.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The best thing to do would be to test this in the simulator

As a reminder, it is possible to log signals in the simulator and export them to files that can be viewed in PulseView. This allows you to analyze stepper pulses without requiring a physical logic analyzer setup. I find that it provides a much faster feedback loop than working with actual hardware.

It won't work for hardware PWM signals like you would have for a laser, but for any software-timed pulses it works well.

Copy link
Contributor

@descipher descipher Apr 28, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

PR Sim compiling result:
Took a look at the simulation outputs and captured a logic vcd of (X|Y)_STEP_PIN it looks reasonable however there is still the count shifting towards the tail of the trap where we can observe the resulting faster decel step rate.
This is the X_STEP_PIN Y_STEP_PIN output point and the zoom is the X axis.
image
image
Here the start pulse interval is 3.187ms and the stop is 2.27ms. This is a dead stop to dead stop thus we should have equal sides on the trap. We have a significantly harder decel due to the shorter decel count calc.
@thinkyhead We can use ( + 1 >=) to ensure we set the laser power on a missed compare. The intent of this code is to set the power to the current_block planner calculated "Cruise" laser power value. Since this code only happens on the cruise else section it should have no serious negative impact with the addition of >. If we end up fixing the counts then the +1 can be removed. So why use == to save compute cycles, if we use > it would increase the compute load by unnecessarily reapplying the power level.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since there are LASER_POWER_TRAP specific issues I've created #27031 to address them. The issues addressed in a way that's independent of this change.

Now to address how this change affects laser: I looked over how LASER_POWER_TRAP uses ac/decelerate_steps and (with the #27031 fixes applied too) the resulting ramp up/down is smoother than before. Here's a comparison at the end of a deceleration when the laser is going to 0% power.

Before:
laser-before-dec
(just a note that my steppers are set to edge stepping)

After:
laser-smooth-dec

I've also looked at the acceleration->cruise transition. I'd say the difference is not significant / not above the noise in the pwm/logic analyzer capture: Last step 75.4 -> 80% versus 76.7% -> 80%.

Took a look at the simulation outputs and captured a logic vcd of (X|Y)_STEP_PIN it looks reasonable however there is still the count shifting towards the tail of the trap where we can observe the resulting faster decel step rate. [..]
Here the start pulse interval is 3.187ms and the stop is 2.27ms. This is a dead stop to dead stop thus we should have equal sides on the trap. We have a significantly harder decel due to the shorter decel count calc.

@descipher You are right about the absolute ideal being equal intervals. And that even with this change the first step is slower than the last step, or stated another way: the first step is at exactly initial_rate but the last step isn't yet at final_rate. Two answers:

  • the difference was way worse before this change as the decel count was even lower
  • trying to hit the absolute ideal profile results in a code complexity explosion. This is because the ac/deceleration_steps computation has to work for all the following types of segments: full acceleration, full deceleration, acceleration->deceleration, acceleration->cruise->deceleration (5 cases). And say we do that, and get the first step at exactly initial_rate and the last at exactly final_rate (=initial_rate of next segment). Now the full acceleration->full acceleration and the full deceleration->full deceleration transitions become jerky because when final_rate and initial_rate of the next segment are equal that means there's no acceleration happening between segments, i.e. we go full accel, 0 accel, full accel. Special casing the 5 cases into 5 cases transitions is just too much. Also the float->discrete steps forces us to accept an error somewhere. To avoid this complexity explosion the way this code is structured is to accept an error of ~1/2 of the ideal acceleration. This half error manifests as not hitting the final_rate exactly.

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;
Expand Down Expand Up @@ -2640,9 +2645,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)
Expand Down Expand Up @@ -2696,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));

Expand Down Expand Up @@ -2783,7 +2785,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);
Expand Down
4 changes: 2 additions & 2 deletions Marlin/src/module/stepper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down