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

PR27035

Browse Source 
Smoother motion by fixing calculating trapezoids and ISR stepping.

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

Remove nominal_length, remove MINIMAL_STEP_RATE, add min_entry_speed_sqr, initial clean up of reverse_pass_kernel and forward_pass_kernel.

Removing MINIMAL_STEP_RATE allows for correct handling of moves with low acceleration, including fixing judder that's caused when the planner computes an entry speed based on minimum_planner_speed_sqr that's then promptly overriden by MINIMAL_STEP_RATE.

Added min_entry_speed_sqr to avoid a specific potential source of judder due to working with discrete steps rather continuous real-valued physics. The first step of any segment runs at initial_rate. If it is too low compared to acceleration_steps_per_s2 it will result in too much accumulated acceleration_time (see stepper.cpp) which will mean the following step will be at a much higher speed, and the speed change will significantly surpass the set acceleration_steps_per_s2 limit. Making sure we can match this limit is why we have minimum_planner_speed_sqr in the first place.

Optimal number of sqrts and trapezoid calculations.

Remove forward_pass(). Call forward_pass_kernel() from recalculate() instead.
Fix potential for large speed changes if planner falls behind.

group for clarity and review

as described

combined float sq

match modified names

Optimal number of sqrts and trapezoid calculations.

Remove forward_pass(). Call forward_pass_kernel() from recalculate() instead.
Fix potential for large speed changes if planner falls behind.

Save and use block->steps_per_mm to avoid many divisions.
This commit is contained in:
Mihail Dumitrescu
2024-04-16 13:06:43 +03:00
committed by InsanityAutomation
parent 6ae43349f7
commit 0f489c6584
5 changed files with 229 additions and 230 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Marlin/src/core/macros.h
+1 -1
View File
@@ -89,7 +89,7 @@
#define HYPOT2(x,y) (sq(x)+sq(y))
#define NORMSQ(x,y,z) (sq(x)+sq(y)+sq(z))
#define FLOAT_SQ(I) sq(float(I))
#define FLOAT_SQ(I) float(sq(I))
#define CIRCLE_AREA(R) (float(M_PI) * FLOAT_SQ(R))
#define CIRCLE_CIRC(R) (2 * float(M_PI) * float(R))
Marlin/src/module/planner.cpp
+118 -208
View File
@@ -128,7 +128,6 @@ Planner planner;
block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed
Planner::block_buffer_nonbusy, // Index of the first non-busy block
Planner::block_buffer_planned, // Index of the optimally planned block
Planner::block_buffer_tail; // Index of the busy block, if any
uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks
uint8_t Planner::delay_before_delivering; // Delay block delivery so initial blocks in an empty queue may merge
@@ -766,10 +765,6 @@ block_t* Planner::get_current_block() {
// As this block is busy, advance the nonbusy block pointer
block_buffer_nonbusy = next_block_index(block_buffer_tail);
// Push block_buffer_planned pointer, if encountered.
if (block_buffer_tail == block_buffer_planned)
block_buffer_planned = block_buffer_nonbusy;
// Return the block
return block;
}
@@ -782,21 +777,22 @@ block_t* Planner::get_current_block() {
/**
* Calculate trapezoid parameters, multiplying the entry- and exit-speeds
* by the provided factors.
* The factors come from the current and next entry speeds divided by the nominal speed,
* which is the top speed achievable during the move. Since entry and exit are presumed to
* be smaller, these factors should always be <= 1.0.
* by the provided factors. If entry_factor is 0 don't change the initial_rate.
* Assumes that the implied initial_rate and final_rate are no less than
* sqrt(block->acceleration_steps_per_s2 / 2). This is ensured through
* minimum_planner_speed_sqr / min_entry_speed_sqr though note there's one
* exception in recalculate_trapezoids().
**
* ############ VERY IMPORTANT ############
* NOTE that the PRECONDITION to call this function is that the block is
* NOT BUSY and it is marked as RECALCULATE. That WARRANTIES the Stepper ISR
* is not and will not use the block while we modify it, so it is safe to
* alter its values.
* is not and will not use the block while we modify it.
*/
void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t entry_factor, const_float_t exit_factor) {
void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t entry_speed, const_float_t exit_speed) {
uint32_t initial_rate = LROUND(block->nominal_rate * entry_factor),
final_rate = LROUND(block->nominal_rate * exit_factor); // (steps per second)
const float spmm = block->steps_per_mm;
uint32_t initial_rate = entry_speed ? _MAX(long(MINIMAL_STEP_RATE), LROUND(entry_speed * spmm)) : block->initial_rate,
final_rate = _MAX(long(MINIMAL_STEP_RATE), LROUND(exit_speed * spmm));
// 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
@@ -804,17 +800,11 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
// 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));
NOLESS(block->nominal_rate, 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);
NOLESS(block->nominal_rate, (uint32_t)MINIMAL_STEP_RATE);
// Limit minimal step rate (Otherwise the timer will overflow.)
NOLESS(initial_rate, MINIMAL_STEP_RATE);
NOLESS(final_rate, MINIMAL_STEP_RATE);
NOLESS(block->nominal_rate, MINIMAL_STEP_RATE);
//NOMORE(initial_rate, block->nominal_rate);
//NOMORE(final_rate, block->nominal_rate);
if (exit_factor < 1.0f) final_rate *= exit_factor;
#if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE)
// If we have some plateau time, the cruise rate will be the nominal rate
@@ -835,7 +825,6 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
// Steps required for acceleration, deceleration to/from nominal rate
decelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(final_rate)),
accelerate_steps_float = half_inverse_accel * (nominal_rate_sq - FLOAT_SQ(initial_rate));
// Aims to fully reach nominal and final rates
accelerate_steps = CEIL(accelerate_steps_float);
decelerate_steps = CEIL(decelerate_steps_float);
@@ -950,16 +939,16 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
*
* Recalculates the motion plan according to the following basic guidelines:
*
* 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds
* (i.e. current->entry_speed) such that:
* a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of
* 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 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.
* 1. Go over blocks sequentially in reverse order and maximize the entry junction speed:
* a. Entry speed should stay below/at the pre-computed maximum junction speed limit
* b. Aim for the maximum entry speed which is the one reverse-computed from its exit speed
* (next->entry_speed) if assuming maximum deceleration over the full block travel distance
* c. The last (newest appended) block uses safe_exit_speed exit speed (there's no 'next')
* 2. Go over blocks in chronological (forward) order and fix the exit junction speed:
* a. Exit speed (next->entry_speed) must be below/at the maximum exit speed forward-computed
* from its entry speed if assuming maximum acceleration over the full block travel distance
* b. Exit speed should stay above/at the pre-computed minimum junction speed limit
* 3. Convert entry / exit speeds (mm/s) into final/initial steps/s
*
* When these stages are complete, the planner will have maximized the velocity profiles throughout the all
* of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In
@@ -967,28 +956,22 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
* are possible. If a new block is added to the buffer, the plan is recomputed according to the said
* guidelines for a new optimal plan.
*
* To increase computational efficiency of these guidelines, a set of planner block pointers have been
* created to indicate stop-compute points for when the planner guidelines cannot logically make any further
* changes or improvements to the plan when in normal operation and new blocks are streamed and added to the
* planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are
* bracketed by junction velocities at their maximums (or by the first planner block as well), no new block
* added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute
* them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute
* point) are all accelerating, they are all optimal and can not be altered by a new block added to the
* planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum
* junction velocity is reached. However, if the operational conditions of the plan changes from infrequently
* used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is
* recomputed as stated in the general guidelines.
* To increase computational efficiency of these guidelines:
* 1. We keep track of which blocks need calculation (block->flag.recalculate)
* 2. We stop the reverse pass on the first block whose entry_speed == max_entry_speed. As soon
* as that happens, there can be no further increases (ensured by the previous recalculate)
* 3. On the forward pass we skip through to the first block with a modified exit speed
* (next->entry_speed)
* 4. On the forward pass if we encounter a full acceleration block that limits its exit speed
* (next->entry_speed) we also update the maximum for that junction (next->max_entry_speed)
* so it's never updated again
* 5. We use speed squared (ex: entry_speed_sqr in mm^2/s^2) in acceleration limit computations
* 6. We don't recompute sqrt(entry_speed_sqr) if the block's entry speed didn't change
*
* Planner buffer index mapping:
* - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
* - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether
* the buffer is full or empty. As described for standard ring buffers, this block is always empty.
* - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal
* streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the
* planner buffer that don't change with the addition of a new block, as describe above. In addition,
* this block can never be less than block_buffer_tail and will always be pushed forward and maintain
* this requirement when encountered by the Planner::release_current_block() routine during a cycle.
*
* NOTE: Since the planner only computes on what's in the planner buffer, some motions with many short
* segments (e.g., complex curves) may seem to move slowly. This is because there simply isn't
@@ -1011,7 +994,8 @@ 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) {
// Returns true if it could increase the current block's entry speed.
bool Planner::reverse_pass_kernel(block_t * const current, const block_t * const next, const_float_t safe_exit_speed_sqr) {
// 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.
@@ -1029,196 +1013,136 @@ void Planner::reverse_pass_kernel(block_t * const current, const block_t * const
// become BUSY just before being marked RECALCULATE, so check for that!
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.
// recalculating BUSY blocks).
current->flag.recalculate = false;
}
else {
// Block is not BUSY so this is ahead of the Stepper ISR:
// Just Set the new entry speed.
current->entry_speed_sqr = new_entry_speed_sqr;
return true;
}
}
}
}
return false;
}
/**
* recalculate() needs to go over the current plan twice.
* Once in reverse and once forward. This implements the reverse pass.
* Once in reverse and once forward. This implements the reverse pass that
* coarsely maximizes the entry speeds starting from last block.
* Requires there's at least one block with flag.recalculate in the buffer.
*/
void Planner::reverse_pass(const_float_t safe_exit_speed_sqr) {
// Initialize block index to the last block in the planner buffer.
// This last block will have flag.recalculate set.
uint8_t block_index = prev_block_index(block_buffer_head);
// Read the index of the last buffer planned block.
// The ISR may change it so get a stable local copy.
uint8_t planned_block_index = block_buffer_planned;
// The ISR may change block_buffer_nonbusy so get a stable local copy.
uint8_t nonbusy_block_index = block_buffer_nonbusy;
// If there was a race condition and block_buffer_planned was incremented
// or was pointing at the head (queue empty) break loop now and avoid
// planning already consumed blocks
if (planned_block_index == block_buffer_head) return;
// Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
// block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
// NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
const block_t *next = nullptr;
while (block_index != planned_block_index) {
// Perform the reverse pass
// Don't try to change the entry speed of the first non-busy block.
while (block_index != nonbusy_block_index) {
block_t *current = &block_buffer[block_index];
// Only process movement blocks
if (current->is_move()) {
reverse_pass_kernel(current, next, safe_exit_speed_sqr);
// If no entry speed increase was possible we end the reverse pass.
if (!reverse_pass_kernel(current, next, safe_exit_speed_sqr)) return;
next = current;
}
// Advance to the next
block_index = prev_block_index(block_index);
// The ISR could advance the block_buffer_planned while we were doing the reverse pass.
// The ISR could advance block_buffer_nonbusy while we were doing the reverse pass.
// We must try to avoid using an already consumed block as the last one - So follow
// changes to the pointer and make sure to limit the loop to the currently busy block
while (planned_block_index != block_buffer_planned) {
while (nonbusy_block_index != block_buffer_nonbusy) {
// If we reached the busy block or an already processed block, break the loop now
if (block_index == planned_block_index) return;
if (block_index == nonbusy_block_index) return;
// Advance the pointer, following the busy block
planned_block_index = next_block_index(planned_block_index);
nonbusy_block_index = next_block_index(nonbusy_block_index);
}
}
}
// 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) {
// 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);
// The kernel called during the forward pass. Assumes current->flag.recalculate.
void Planner::forward_pass_kernel(const block_t * const previous, block_t * const current) {
// Check if the previous block is accelerating.
if (previous->entry_speed_sqr < current->entry_speed_sqr) {
// Compute the maximum achievable speed if the previous block was fully accelerating.
float new_exit_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;
if (new_exit_speed_sqr < current->entry_speed_sqr) {
// Current entry speed limited by full acceleration from previous entry speed.
// Set optimal plan pointer.
block_buffer_planned = block_index;
}
else {
// Previous entry speed has been maximized.
block_buffer_planned = prev_block_index(block_index);
}
// Make sure entry speed not lower than minimum_planner_speed_sqr.
NOLESS(new_exit_speed_sqr, current->min_entry_speed_sqr);
current->entry_speed_sqr = new_exit_speed_sqr;
// Ensure we don't try updating entry_speed_sqr again.
current->max_entry_speed_sqr = new_exit_speed_sqr;
}
// Any block set at its maximum entry speed also creates an optimal plan up to this
// point in the buffer. When the plan is bracketed by either the beginning of the
// buffer and a maximum entry speed or two maximum entry speeds, every block in between
// cannot logically be further improved. Hence, we don't have to recompute them anymore.
if (current->entry_speed_sqr == current->max_entry_speed_sqr)
block_buffer_planned = block_index;
}
// The fully optimized entry speed is our new minimum speed.
current->min_entry_speed_sqr = current->entry_speed_sqr;
}
/**
* recalculate() needs to go over the current plan twice.
* Once in reverse and once forward. This implements the forward pass.
*/
void Planner::forward_pass() {
// Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
// Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
// Begin at buffer planned pointer. Note that block_buffer_planned can be modified
// by the stepper ISR, so read it ONCE. It it guaranteed that block_buffer_planned
// will never lead head, so the loop is safe to execute. Also note that the forward
// pass will never modify the values at the tail.
uint8_t block_index = block_buffer_planned;
block_t *block;
const block_t * previous = nullptr;
while (block_index != block_buffer_head) {
// Perform the forward pass
block = &block_buffer[block_index];
// Only process movement blocks
if (block->is_move()) {
// If there's no previous block or the previous block is not
// BUSY (thus, modifiable) run the forward_pass_kernel. Otherwise,
// 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))
forward_pass_kernel(previous, block, block_index);
previous = block;
}
// Advance to the previous
block_index = next_block_index(block_index);
}
}
/**
* Recalculate the trapezoid speed profiles for all blocks in the plan
* according to the entry_factor for each junction. Must be called by
* recalculate() after updating the blocks.
* Do the forward pass and recalculate the trapezoid speed profiles for all blocks in the plan
* according to entry/exit speeds.
*/
void Planner::recalculate_trapezoids(const_float_t safe_exit_speed_sqr) {
// The tail may be changed by the ISR so get a local copy.
// Start with the block that's about to execute or is executing.
uint8_t block_index = block_buffer_tail,
head_block_index = block_buffer_head;
// Since there could be a sync block in the head of the queue, and the
// next loop must not recalculate the head block (as it needs to be
// specially handled), scan backwards to the first non-SYNC block.
while (head_block_index != block_index) {
// Go back (head always point to the first free block)
const uint8_t prev_index = prev_block_index(head_block_index);
// Get the pointer to the block
block_t *prev = &block_buffer[prev_index];
// It the block is a move, we're done with this loop
if (prev->is_move()) break;
// Examine the previous block. This and all following are SYNC blocks
head_block_index = prev_index;
}
// Go from the tail (currently executed block) to the first block, without including it)
block_t *block = nullptr, *next = nullptr;
float current_entry_speed = 0.0f, next_entry_speed = 0.0f;
float next_entry_speed = 0.0f;
while (block_index != head_block_index) {
next = &block_buffer[block_index];
// Only process movement blocks
if (next->is_move()) {
next_entry_speed = SQRT(next->entry_speed_sqr);
// Check if the next block's entry speed changed
if (next->flag.recalculate) {
if (!block) {
// 'next' is the first move due to either being the first added move or due to the planner
// having completely fallen behind. Revert any reverse pass change.
next->entry_speed_sqr = next->min_entry_speed_sqr;
next_entry_speed = SQRT(next->min_entry_speed_sqr);
}
else {
// Try to fix exit speed which requires trapezoid recalculation
block->flag.recalculate = true;
if (block) {
// But there is an inherent race condition here, as the block may have
// become BUSY just before being marked RECALCULATE, so check for that!
if (stepper.is_block_busy(block)) {
// Block is BUSY so we can't change the exit speed. Revert any reverse pass change.
next->entry_speed_sqr = next->min_entry_speed_sqr;
if (!next->initial_rate) {
// 'next' was never calculated. Planner is falling behind so for maximum efficiency
// set next's stepping speed directly and forgo checking against min_entry_speed_sqr.
// calculate_trapezoid_for_block() can handle it, albeit sub-optimally.
next->initial_rate = block->final_rate;
}
// Note that at this point next_entry_speed is (still) 0.
}
else {
// Block is not BUSY: we won the race against the ISR or recalculate was already set
// If the next block is marked to RECALCULATE, also mark the previously-fetched one
if (next->flag.recalculate) block->flag.recalculate = true;
if (next->entry_speed_sqr != next->min_entry_speed_sqr)
forward_pass_kernel(block, next);
// Recalculate if current block entry or exit junction speed has changed.
if (block->flag.recalculate) {
const float current_entry_speed = next_entry_speed;
next_entry_speed = SQRT(next->entry_speed_sqr);
// 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(block)) {
// Block is not BUSY, we won the race against the Stepper ISR:
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
const float nomr = 1.0f / block->nominal_speed;
calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr);
calculate_trapezoid_for_block(block, current_entry_speed, next_entry_speed);
}
// Reset current only to ensure next trapezoid is computed - The
@@ -1228,30 +1152,17 @@ void Planner::recalculate_trapezoids(const_float_t safe_exit_speed_sqr) {
}
block = next;
current_entry_speed = next_entry_speed;
}
block_index = next_block_index(block_index);
}
// Last/newest block in buffer. Always recalculated.
if (block) {
// Last/newest block in buffer. The above guarantees it's a move block.
if (block && block->flag.recalculate) {
const float current_entry_speed = next_entry_speed;
next_entry_speed = SQRT(safe_exit_speed_sqr);
// Mark the next(last) block as RECALCULATE, to prevent the Stepper ISR running it.
// As the last block is always recalculated here, there is a chance the block isn't
// marked as RECALCULATE yet. That's the reason for the following line.
block->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(block)) {
// Block is not BUSY, we won the race against the Stepper ISR:
const float nomr = 1.0f / block->nominal_speed;
calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr);
}
calculate_trapezoid_for_block(block, current_entry_speed, next_entry_speed);
// Reset block to ensure its trapezoid is computed - The stepper is free to use
// the block from now on.
@@ -1259,14 +1170,10 @@ void Planner::recalculate_trapezoids(const_float_t safe_exit_speed_sqr) {
}
}
// Requires there's at least one block with flag.recalculate in the buffer
void Planner::recalculate(const_float_t safe_exit_speed_sqr) {
// Initialize block index to the last block in the planner buffer.
const uint8_t block_index = prev_block_index(block_buffer_head);
// If there is just one block, no planning can be done. Avoid it!
if (block_index != block_buffer_planned) {
reverse_pass(safe_exit_speed_sqr);
forward_pass();
}
reverse_pass(safe_exit_speed_sqr);
// The forward pass is done as part of recalculate_trapezoids()
recalculate_trapezoids(safe_exit_speed_sqr);
}
@@ -1675,7 +1582,7 @@ void Planner::quick_stop() {
const bool was_enabled = stepper.suspend();
// Drop all queue entries
block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail;
block_buffer_nonbusy = block_buffer_head = block_buffer_tail;
// Restart the block delay for the first movement - As the queue was
// forced to empty, there's no risk the ISR will touch this.
@@ -2444,6 +2351,7 @@ bool Planner::_populate_block(
// Compute and limit the acceleration rate for the trapezoid generator.
const float steps_per_mm = block->step_event_count * inverse_millimeters;
block->steps_per_mm = steps_per_mm;
uint32_t accel;
#if ENABLED(LIN_ADVANCE)
bool use_advance_lead = false;
@@ -2554,7 +2462,7 @@ bool Planner::_populate_block(
// 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.
// 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));
@@ -2828,7 +2736,7 @@ bool Planner::_populate_block(
#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)
// 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.
@@ -2837,6 +2745,8 @@ bool Planner::_populate_block(
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;
// Zero the initial_rate to indicate that calculate_trapezoid_for_block() hasn't been called yet.
block->initial_rate = 0;
block->flag.recalculate = true;
Marlin/src/module/planner.h
+6 -7
View File
@@ -227,6 +227,7 @@ typedef struct PlannerBlock {
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
steps_per_mm, // steps/mm
acceleration; // acceleration mm/sec^2
union {
@@ -246,7 +247,7 @@ typedef struct PlannerBlock {
#endif
// Settings for the trapezoid generator
uint32_t accelerate_before, // The index of the step event where cruising starts
uint32_t accelerate_before, // The index of the step event on which to start cruising
decelerate_start; // The index of the step event on which to start decelerating
#if ENABLED(S_CURVE_ACCELERATION)
@@ -450,7 +451,6 @@ class Planner {
static block_t block_buffer[BLOCK_BUFFER_SIZE];
static volatile uint8_t block_buffer_head, // Index of the next block to be pushed
block_buffer_nonbusy, // Index of the first non busy block
block_buffer_planned, // Index of the optimally planned block
block_buffer_tail; // Index of the busy block, if any
static uint16_t cleaning_buffer_counter; // A counter to disable queuing of blocks
static uint8_t delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
@@ -812,7 +812,7 @@ class Planner {
FORCE_INLINE static uint8_t nonbusy_movesplanned() { return block_dec_mod(block_buffer_head, block_buffer_nonbusy); }
// Remove all blocks from the buffer
FORCE_INLINE static void clear_block_buffer() { block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail = 0; }
FORCE_INLINE static void clear_block_buffer() { block_buffer_nonbusy = block_buffer_head = block_buffer_tail = 0; }
// Check if movement queue is full
FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
@@ -1089,13 +1089,12 @@ class Planner {
}
#endif
static void calculate_trapezoid_for_block(block_t * const block, const_float_t entry_factor, const_float_t exit_factor);
static void calculate_trapezoid_for_block(block_t * const block, const_float_t entry_speed, const_float_t exit_speed);
static void reverse_pass_kernel(block_t * const current, const block_t * const next, const_float_t safe_exit_speed_sqr);
static void forward_pass_kernel(const block_t * const previous, block_t * const current, uint8_t block_index);
static bool reverse_pass_kernel(block_t * const current, const block_t * const next, const_float_t safe_exit_speed_sqr);
static void forward_pass_kernel(const block_t * const previous, block_t * const current);
static void reverse_pass(const_float_t safe_exit_speed_sqr);
static void forward_pass();
static void recalculate_trapezoids(const_float_t safe_exit_speed_sqr);
Marlin/src/module/stepper.cpp
+102 -12
View File
@@ -58,16 +58,10 @@
*
* time ----->
*
* 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.
* The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
* while step_events_completed < block->accelerate_before, then starts cruising at constant speed while
* step_events_completed < block->decelerate_start, then 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.
*/
/**
@@ -2600,6 +2594,25 @@ hal_timer_t Stepper::block_phase_isr() {
// The timer interval is just the nominal value for the nominal speed
interval = ticks_nominal;
}
/**
* Adjust Laser Power - Cruise
* power - direct or floor adjusted active laser power.
*/
#if ENABLED(LASER_POWER_TRAP)
if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) {
if (step_events_completed + 1 == accelerate_before) {
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;
cutter.apply_power(current_block->laser.power);
}
}
// Not a powered move.
else cutter.apply_power(0);
}
}
#endif
}
#if ENABLED(LASER_FEATURE)
@@ -2691,8 +2704,85 @@ hal_timer_t Stepper::block_phase_isr() {
}
#endif
// Set flags for all moving axes, accounting for kinematics
set_axis_moved_for_current_block();
// Flag all moving axes for proper endstop handling
#if IS_CORE
// Define conditions for checking endstops
#define S_(N) current_block->steps[CORE_AXIS_##N]
#define D_(N) current_block->direction_bits[CORE_AXIS_##N]
#endif
#if CORE_IS_XY || CORE_IS_XZ
/**
* Head direction in -X axis for CoreXY and CoreXZ bots.
*
* If steps differ, both axes are moving.
* If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z, handled below)
* If DeltaA == DeltaB, the movement is only in the 1st axis (X)
*/
#if ANY(COREXY, COREXZ)
#define X_CMP(A,B) ((A)==(B))
#else
#define X_CMP(A,B) ((A)!=(B))
#endif
#define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && X_CMP(D_(1),D_(2))) )
#elif ENABLED(MARKFORGED_XY)
#define X_MOVE_TEST (current_block->steps.a != current_block->steps.b)
#else
#define X_MOVE_TEST !!current_block->steps.a
#endif
#if CORE_IS_XY || CORE_IS_YZ
/**
* Head direction in -Y axis for CoreXY / CoreYZ bots.
*
* If steps differ, both axes are moving
* If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y)
* If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z)
*/
#if ANY(COREYX, COREYZ)
#define Y_CMP(A,B) ((A)==(B))
#else
#define Y_CMP(A,B) ((A)!=(B))
#endif
#define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Y_CMP(D_(1),D_(2))) )
#elif ENABLED(MARKFORGED_YX)
#define Y_MOVE_TEST (current_block->steps.a != current_block->steps.b)
#else
#define Y_MOVE_TEST !!current_block->steps.b
#endif
#if CORE_IS_XZ || CORE_IS_YZ
/**
* Head direction in -Z axis for CoreXZ or CoreYZ bots.
*
* If steps differ, both axes are moving
* If DeltaA == DeltaB, the movement is only in the 1st axis (X or Y, already handled above)
* If DeltaA == -DeltaB, the movement is only in the 2nd axis (Z)
*/
#if ANY(COREZX, COREZY)
#define Z_CMP(A,B) ((A)==(B))
#else
#define Z_CMP(A,B) ((A)!=(B))
#endif
#define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Z_CMP(D_(1),D_(2))) )
#else
#define Z_MOVE_TEST !!current_block->steps.c
#endif
AxisBits didmove;
NUM_AXIS_CODE(
if (X_MOVE_TEST) didmove.a = true,
if (Y_MOVE_TEST) didmove.b = true,
if (Z_MOVE_TEST) didmove.c = true,
if (!!current_block->steps.i) didmove.i = true,
if (!!current_block->steps.j) didmove.j = true,
if (!!current_block->steps.k) didmove.k = true,
if (!!current_block->steps.u) didmove.u = true,
if (!!current_block->steps.v) didmove.v = true,
if (!!current_block->steps.w) didmove.w = true
);
axis_did_move = didmove;
#if ENABLED(ADAPTIVE_STEP_SMOOTHING)
// Nonlinear Extrusion needs at least 2x oversampling to permit increase of E step rate
Marlin/src/module/stepper.h
+2 -2
View File
@@ -386,8 +386,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_before, // The count at which to start cruising
decelerate_start, // The count at which to start decelerating
accelerate_before, // The point from where we need to stop acceleration
decelerate_start, // The point from where we need to start decelerating
step_event_count; // The total event count for the current block
#if ANY(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER)