sravan/Marlin-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

⚡️ Add / enforce min_entry_speed_sqr (#27089)

Browse Source
This commit is contained in:
Mihai
2024-05-15 23:01:56 +03:00
committed by GitHub
parent bbdad79ce5
commit 6423b8031d
2 changed files with 47 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
Marlin/src/module/planner.cpp
View File

@@ -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) {
// 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);
// 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.
// 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;
}
@@ -2566,9 +2537,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
@@ -2764,8 +2739,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;
@@ -2807,8 +2781,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;
}
@@ -2838,28 +2811,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
Marlin/src/module/planner.h
View File

@@ -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;
@@ -224,6 +216,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
@@ -255,7 +248,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