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.

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

Add and enforce min_entry_speed_sqr #27089

Merged
merged 1 commit into from
May 15, 2024
Merged

Add and enforce min_entry_speed_sqr #27089

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
127 changes: 46 additions & 81 deletions Marlin/src/module/planner.cpp
View file
Original file line number Diff line number Diff line change
@@ -784,7 +784,9 @@ block_t* Planner::get_current_block() {

/**
* Calculate trapezoid parameters, multiplying the entry- and exit-speeds
* by the provided factors.
* by the provided factors. Requires that initial_rate and final_rate are
* no less than sqrt(block->acceleration_steps_per_s2 / 2), which is ensured
* through minimum_planner_speed_sqr in _populate_block().
**
* ############ VERY IMPORTANT ############
* NOTE that the PRECONDITION to call this function is that the block is
@@ -940,7 +942,7 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
* neighboring blocks.
* b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed)
* with a maximum allowable deceleration over the block travel distance.
* c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero).
* c. The last (or newest appended) block is planned from safe_exit_speed_sqr.
* 2. Go over every block in chronological (forward) order and dial down junction speed values if
* a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable
* acceleration over the block travel distance.
@@ -996,29 +998,13 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t

// The kernel called by recalculate() when scanning the plan from last to first entry.
void Planner::reverse_pass_kernel(block_t * const current, const block_t * const next, const_float_t safe_exit_speed_sqr) {
if (current) {
// If entry speed is already at the maximum entry speed, and there was no change of speed
// in the next block, there is no need to recheck. Block is cruising and there is no need to
// compute anything for this block,
// If not, block entry speed needs to be recalculated to ensure maximum possible planned speed.
const float max_entry_speed_sqr = current->max_entry_speed_sqr;

// Compute maximum entry speed decelerating over the current block from its exit speed.
// If not at the maximum entry speed, or the previous block entry speed changed
if (current->entry_speed_sqr != max_entry_speed_sqr || (next && next->flag.recalculate)) {

// If nominal length true, max junction speed is guaranteed to be reached.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.

const float next_entry_speed_sqr = next ? next->entry_speed_sqr : safe_exit_speed_sqr,
new_entry_speed_sqr = current->flag.nominal_length
? max_entry_speed_sqr
: _MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next_entry_speed_sqr, current->millimeters));
// We need to recalculate only for the last block added or if next->entry_speed_sqr changed.
if (!next || next->flag.recalculate) {
// And only if we're not already at max entry speed.
if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
const float next_entry_speed_sqr = next ? next->entry_speed_sqr : safe_exit_speed_sqr;
float new_entry_speed_sqr = max_allowable_speed_sqr(-current->acceleration, next_entry_speed_sqr, current->millimeters);
NOMORE(new_entry_speed_sqr, current->max_entry_speed_sqr);
if (current->entry_speed_sqr != new_entry_speed_sqr) {

// Need to recalculate the block speed - Mark it now, so the stepper
@@ -1094,41 +1080,26 @@ void Planner::reverse_pass(const_float_t safe_exit_speed_sqr) {

// The kernel called by recalculate() when scanning the plan from first to last entry.
void Planner::forward_pass_kernel(const block_t * const previous, block_t * const current, const uint8_t block_index) {
if (previous) {
// If the previous block is an acceleration block, too short to complete the full speed
// change, adjust the entry speed accordingly. Entry speeds have already been reset,
// maximized, and reverse-planned. If nominal length is set, max junction speed is
// guaranteed to be reached. No need to recheck.
if (!previous->flag.nominal_length && previous->entry_speed_sqr < current->entry_speed_sqr) {

// Compute the maximum allowable speed
const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);

// If true, current block is full-acceleration and we can move the planned pointer forward.
// Check against previous speed only on current->entry_speed_sqr changes (or if first time).
if (current->flag.recalculate) {
// If the previous block is accelerating check if it's too short to complete the full speed
// change then adjust the entry speed accordingly. Entry speeds have already been maximized.
if (previous->entry_speed_sqr < current->entry_speed_sqr) {
float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);

// If true, previous block is full-acceleration and we can move the planned pointer forward.
if (new_entry_speed_sqr < current->entry_speed_sqr) {
// Current entry speed limited by full acceleration from previous entry speed.
// Make sure entry speed not lower than minimum_planner_speed_sqr.
NOLESS(new_entry_speed_sqr, current->min_entry_speed_sqr);
current->entry_speed_sqr = new_entry_speed_sqr;

// Mark we need to recompute the trapezoidal shape, and do it now,
// so the stepper ISR does not consume the block before being recalculated
current->flag.recalculate = true;

// But there is an inherent race condition here, as the block maybe
// became BUSY, just before it was marked as RECALCULATE, so check
// if that is the case!
if (stepper.is_block_busy(current)) {
// Block became busy. Clear the RECALCULATE flag (no point in
// recalculating BUSY blocks and don't set its speed, as it can't
// be updated at this time.
current->flag.recalculate = false;
}
else {
// Block is not BUSY, we won the race against the Stepper ISR:

// Always <= max_entry_speed_sqr. Backward pass sets this.
current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.

// Set optimal plan pointer.
block_buffer_planned = block_index;
}
// Set optimal plan pointer.
block_buffer_planned = block_index;
}
else {
// Previous entry speed has been maximized.
block_buffer_planned = prev_block_index(block_index);
}
}

@@ -1170,7 +1141,7 @@ void Planner::forward_pass() {
// the previous block became BUSY, so assume the current block's
// entry speed can't be altered (since that would also require
// updating the exit speed of the previous block).
if (!previous || !stepper.is_block_busy(previous))
if (previous && !stepper.is_block_busy(previous))
forward_pass_kernel(previous, block, block_index);
previous = block;
}
@@ -2565,9 +2536,13 @@ bool Planner::_populate_block(
}
#endif

// The minimum possible speed is the average speed for
// the first / last step at current acceleration limit
// Formula for the average speed over a 1 step worth of distance if starting from zero and
// accelerating at the current limit. Since we can only change the speed every step this is a
// good lower limit for the entry and exit speeds. Note that for calculate_trapezoid_for_block()
// to work correctly, this must be accurately set and propagated.
minimum_planner_speed_sqr = 0.5f * block->acceleration / steps_per_mm;
// Go straight to/from nominal speed if block->acceleration is too high for it.
NOMORE(minimum_planner_speed_sqr, sq(block->nominal_speed));

float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2

@@ -2763,8 +2738,7 @@ bool Planner::_populate_block(
// Get the lowest speed
vmax_junction_sqr = _MIN(vmax_junction_sqr, sq(block->nominal_speed), sq(previous_nominal_speed));
}
else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
vmax_junction_sqr = 0;
else vmax_junction_sqr = minimum_planner_speed_sqr;

prev_unit_vec = unit_vec;

@@ -2806,8 +2780,7 @@ bool Planner::_populate_block(

xyze_float_t speed_diff = current_speed;
float vmax_junction;
const bool start_from_zero = !moves_queued || UNEAR_ZERO(previous_nominal_speed);
if (start_from_zero) {
if (!moves_queued || UNEAR_ZERO(previous_nominal_speed)) {
// Limited by a jerk to/from full halt.
vmax_junction = block->nominal_speed;
}
@@ -2837,28 +2810,20 @@ bool Planner::_populate_block(
}
vmax_junction_sqr = sq(vmax_junction * v_factor);

if (start_from_zero) minimum_planner_speed_sqr = vmax_junction_sqr;

#endif // CLASSIC_JERK

// High acceleration limits override low jerk/junction deviation limits (as fixing trapezoids
// or reducing acceleration introduces too much complexity and/or too much compute)
NOLESS(vmax_junction_sqr, minimum_planner_speed_sqr);

// Max entry speed of this block equals the max exit speed of the previous block.
block->max_entry_speed_sqr = vmax_junction_sqr;

// Initialize block entry speed. Compute based on deceleration to sqrt(minimum_planner_speed_sqr).
const float v_allowable_sqr = max_allowable_speed_sqr(-block->acceleration, minimum_planner_speed_sqr, block->millimeters);

// Start with the minimum allowed speed
// Set entry speed. The reverse and forward passes will optimize it later.
block->entry_speed_sqr = minimum_planner_speed_sqr;
// Set min entry speed. Rarely it could be higher than the previous nominal speed but that's ok.
block->min_entry_speed_sqr = minimum_planner_speed_sqr;

// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->flag.set_nominal(sq(block->nominal_speed) <= v_allowable_sqr);
block->flag.recalculate = true;

// Update previous path unit_vector and nominal speed
previous_speed = current_speed;
11 changes: 2 additions & 9 deletions Marlin/src/module/planner.h
View file
Original file line number Diff line number Diff line change
@@ -111,11 +111,6 @@ enum BlockFlagBit {
// Recalculate trapezoids on entry junction. For optimization.
BLOCK_BIT_RECALCULATE,

// Nominal speed always reached.
// i.e., The segment is long enough, so the nominal speed is reachable if accelerating
// from a safe speed (in consideration of jerking from zero speed).
BLOCK_BIT_NOMINAL_LENGTH,

// The block is segment 2+ of a longer move
BLOCK_BIT_CONTINUED,

@@ -142,8 +137,6 @@ typedef struct {
struct {
bool recalculate:1;

bool nominal_length:1;

bool continued:1;

bool sync_position:1;
@@ -166,7 +159,6 @@ typedef struct {
void apply(const uint8_t f) volatile { bits |= f; }
void apply(const BlockFlagBit b) volatile { SBI(bits, b); }
void reset(const BlockFlagBit b) volatile { bits = _BV(b); }
void set_nominal(const bool n) volatile { recalculate = true; if (n) nominal_length = true; }

} block_flags_t;

@@ -223,6 +215,7 @@ typedef struct PlannerBlock {
// Fields used by the motion planner to manage acceleration
float nominal_speed, // The nominal speed for this block in (mm/sec)
entry_speed_sqr, // Entry speed at previous-current junction in (mm/sec)^2
min_entry_speed_sqr, // Minimum allowable junction entry speed in (mm/sec)^2
max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2
millimeters, // The total travel of this block in mm
acceleration; // acceleration mm/sec^2
@@ -254,7 +247,7 @@ typedef struct PlannerBlock {
acceleration_time_inverse, // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used
deceleration_time_inverse;
#else
uint32_t acceleration_rate; // The acceleration rate used for acceleration calculation
uint32_t acceleration_rate; // Acceleration rate in (2^24 steps)/timer_ticks*s
#endif

AxisBits direction_bits; // Direction bits set for this block, where 1 is negative motion
Loading