Skip to content
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.

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

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

Merged
merged 9 commits into from
May 17, 2024
Merged
Changes from 6 commits
Commits
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
Original file line number Diff line number Diff line change
@@ -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) {
thinkyhead marked this conversation as resolved.
Show resolved Hide resolved

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;
@@ -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
}
}
@@ -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;
thinkyhead marked this conversation as resolved.
Show resolved Hide resolved
block->initial_rate = initial_rate;
#if ENABLED(S_CURVE_ACCELERATION)
block->acceleration_time = acceleration_time;
@@ -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();
4 changes: 2 additions & 2 deletions Marlin/src/module/planner.h
Original file line number Diff line number Diff line change
@@ -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
55 changes: 29 additions & 26 deletions Marlin/src/module/stepper.cpp
Original file line number Diff line number Diff line change
@@ -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
thinkyhead marked this conversation as resolved.
Show resolved Hide resolved
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)
@@ -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!)
@@ -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.
thinkyhead marked this conversation as resolved.
Show resolved Hide resolved

#if ENABLED(NONLINEAR_EXTRUSION)
calc_nonlinear_e(acc_step_rate << oversampling_factor);
@@ -2355,30 +2363,24 @@ hal_timer_t Stepper::block_phase_isr() {
#endif
thinkyhead marked this conversation as resolved.
Show resolved Hide resolved
}
// 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);
}
@@ -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);
@@ -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

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

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

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

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;
@@ -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)
@@ -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));

@@ -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;
thinkyhead marked this conversation as resolved.
Show resolved Hide resolved

#if ENABLED(NONLINEAR_EXTRUSION)
calc_nonlinear_e(current_block->initial_rate << oversampling_factor);
4 changes: 2 additions & 2 deletions Marlin/src/module/stepper.h
Original file line number Diff line number Diff line change
@@ -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)