From 5717a39422be6dedd0839e7e93cc3fed409bc51c Mon Sep 17 00:00:00 2001 From: Mihail Dumitrescu Date: Mon, 22 Apr 2024 12:10:26 +0300 Subject: [PATCH 1/6] 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. --- Marlin/src/module/planner.cpp | 133 ++++++++++++---------------------- Marlin/src/module/planner.h | 11 +-- 2 files changed, 47 insertions(+), 97 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index a752ba0178..efafa8a1f9 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -730,8 +730,6 @@ void Planner::init() { #endif #endif -#define MINIMAL_STEP_RATE 120 - /** * Get the current block for processing * and mark the block as busy. @@ -784,7 +782,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 @@ -797,12 +797,6 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t 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 results in less smooth motion for switching direction - // 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)); - NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE)); NOMORE(initial_rate, block->nominal_rate); NOMORE(final_rate, block->nominal_rate); @@ -946,7 +940,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. @@ -1002,29 +996,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 @@ -1100,41 +1078,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); } } @@ -1176,7 +1139,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; } @@ -2571,9 +2534,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 @@ -2769,8 +2736,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; @@ -2812,8 +2778,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; } @@ -2843,28 +2808,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; diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index b7b1abbb61..fd99c7b3c1 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -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 From 50a38bb3655c0d5a2d4c1be826f65c9ddefa974d Mon Sep 17 00:00:00 2001 From: Mihail Dumitrescu Date: Mon, 22 Apr 2024 16:32:25 +0300 Subject: [PATCH 2/6] 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. --- Marlin/src/module/planner.cpp | 307 +++++++++++++--------------------- Marlin/src/module/planner.h | 8 +- 2 files changed, 118 insertions(+), 197 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index efafa8a1f9..b4adff065f 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -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,26 @@ block_t* Planner::get_current_block() { /** * Calculate trapezoid parameters, multiplying the entry- and exit-speeds - * 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(). + * 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) { uint32_t initial_rate = LROUND(block->nominal_rate * entry_factor), final_rate = LROUND(block->nominal_rate * exit_factor); // (steps per second) + if (initial_rate == 0) { + initial_rate = block->initial_rate; + } + NOMORE(initial_rate, block->nominal_rate); NOMORE(final_rate, block->nominal_rate); @@ -934,16 +934,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 @@ -951,28 +951,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 @@ -995,7 +989,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. @@ -1013,194 +1008,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); } @@ -1212,30 +1149,18 @@ 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); - } + const float nomr = 1.0f / block->nominal_speed; + calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr); // Reset block to ensure its trapezoid is computed - The stepper is free to use // the block from now on. @@ -1243,14 +1168,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); } @@ -1659,7 +1580,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. @@ -2537,7 +2458,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)); @@ -2811,7 +2732,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. @@ -2820,6 +2741,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; diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index fd99c7b3c1..5462d5f606 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -441,7 +441,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 @@ -803,7 +802,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); } @@ -1082,11 +1081,10 @@ class Planner { static void calculate_trapezoid_for_block(block_t * const block, const_float_t entry_factor, const_float_t exit_factor); - 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); From 6ce4f039a25f4c24953a3fe60338bdfd504f4269 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 6 May 2024 17:24:07 -0500 Subject: [PATCH 3/6] group for clarity and review --- Marlin/src/module/planner.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index b4adff065f..c35e888f39 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -790,14 +790,11 @@ 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 = LROUND(block->nominal_rate * entry_factor), - final_rate = LROUND(block->nominal_rate * exit_factor); // (steps per second) - - if (initial_rate == 0) { - initial_rate = block->initial_rate; - } - + uint32_t initial_rate = LROUND(block->nominal_rate * entry_factor); // (steps per second) + if (initial_rate == 0) initial_rate = block->initial_rate; NOMORE(initial_rate, block->nominal_rate); + + uint32_t final_rate = LROUND(block->nominal_rate * exit_factor); // (steps per second) NOMORE(final_rate, block->nominal_rate); #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) From c0d217de4779d4593add2dbc273c42b545e49d7c Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 9 May 2024 16:48:09 -0500 Subject: [PATCH 4/6] as described --- Marlin/src/module/planner.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index c35e888f39..7d0787180c 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -794,8 +794,8 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t if (initial_rate == 0) initial_rate = block->initial_rate; NOMORE(initial_rate, block->nominal_rate); - uint32_t final_rate = LROUND(block->nominal_rate * exit_factor); // (steps per second) - NOMORE(final_rate, block->nominal_rate); + uint32_t final_rate = block->nominal_rate; // (steps per second) + 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 @@ -1128,9 +1128,8 @@ void Planner::recalculate_trapezoids(const_float_t safe_exit_speed_sqr) { else { // Block is not BUSY: we won the race against the ISR or recalculate was already set - if (next->entry_speed_sqr != next->min_entry_speed_sqr) { + if (next->entry_speed_sqr != next->min_entry_speed_sqr) forward_pass_kernel(block, next); - } const float current_entry_speed = next_entry_speed; next_entry_speed = SQRT(next->entry_speed_sqr); From f2be86f735a5a7505ddb7cf60267f2749d857941 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 9 May 2024 17:17:57 -0500 Subject: [PATCH 5/6] combined float sq --- Marlin/src/core/macros.h | 3 ++- Marlin/src/lcd/extui/ui_api.cpp | 2 +- Marlin/src/lcd/menu/menu_motion.cpp | 2 +- Marlin/src/lcd/tft/ui_common.cpp | 2 +- Marlin/src/module/motion.cpp | 4 ++-- Marlin/src/module/planner.cpp | 7 +++---- Marlin/src/module/scara.h | 8 ++++---- 7 files changed, 14 insertions(+), 14 deletions(-) diff --git a/Marlin/src/core/macros.h b/Marlin/src/core/macros.h index 784002c769..90974f1c98 100644 --- a/Marlin/src/core/macros.h +++ b/Marlin/src/core/macros.h @@ -89,7 +89,8 @@ #define HYPOT2(x,y) (sq(x)+sq(y)) #define NORMSQ(x,y,z) (sq(x)+sq(y)+sq(z)) -#define CIRCLE_AREA(R) (float(M_PI) * sq(float(R))) +#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)) #define SIGN(a) ({__typeof__(a) _a = (a); (_a>0)-(_a<0);}) diff --git a/Marlin/src/lcd/extui/ui_api.cpp b/Marlin/src/lcd/extui/ui_api.cpp index ec9b05318c..74463bdf5c 100644 --- a/Marlin/src/lcd/extui/ui_api.cpp +++ b/Marlin/src/lcd/extui/ui_api.cpp @@ -343,7 +343,7 @@ namespace ExtUI { // This assumes the center is 0,0 #if ENABLED(DELTA) if (axis != Z) { - max = SQRT(sq(float(PRINTABLE_RADIUS)) - sq(current_position[Y - axis])); // (Y - axis) == the other axis + max = SQRT(FLOAT_SQ(PRINTABLE_RADIUS) - sq(current_position[Y - axis])); // (Y - axis) == the other axis min = -max; } #endif diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp index ebc809fef8..f65eb96719 100644 --- a/Marlin/src/lcd/menu/menu_motion.cpp +++ b/Marlin/src/lcd/menu/menu_motion.cpp @@ -63,7 +63,7 @@ void lcd_move_axis(const AxisEnum axis) { // This assumes the center is 0,0 #if ENABLED(DELTA) if (axis != Z_AXIS) { - max = SQRT(sq(float(PRINTABLE_RADIUS)) - sq(current_position[Y_AXIS - axis])); // (Y_AXIS - axis) == the other axis + max = SQRT(FLOAT_SQ(PRINTABLE_RADIUS) - sq(current_position[Y_AXIS - axis])); // (Y_AXIS - axis) == the other axis min = -max; } #endif diff --git a/Marlin/src/lcd/tft/ui_common.cpp b/Marlin/src/lcd/tft/ui_common.cpp index 944be77ab5..17bc7a4e48 100644 --- a/Marlin/src/lcd/tft/ui_common.cpp +++ b/Marlin/src/lcd/tft/ui_common.cpp @@ -126,7 +126,7 @@ void moveAxis(const AxisEnum axis, const int8_t direction) { // This assumes the center is 0,0 #if ENABLED(DELTA) if (axis != Z_AXIS && TERN1(HAS_EXTRUDERS, axis != E_AXIS)) { - max = SQRT(sq(float(PRINTABLE_RADIUS)) - sq(current_position[Y_AXIS - axis])); // (Y_AXIS - axis) == the other axis + max = SQRT(FLOAT_SQ(PRINTABLE_RADIUS) - sq(current_position[Y_AXIS - axis])); // (Y_AXIS - axis) == the other axis min = -max; } #endif diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index e4e419dd76..1f7b90ddeb 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -328,7 +328,7 @@ void report_current_position_projected() { can_reach = ( R2 <= sq(L1 + L2) - inset #if MIDDLE_DEAD_ZONE_R > 0 - && R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) + && R2 >= FLOAT_SQ(MIDDLE_DEAD_ZONE_R) #endif ); @@ -338,7 +338,7 @@ void report_current_position_projected() { can_reach = ( R2 <= sq(L1 + L2) - inset #if MIDDLE_DEAD_ZONE_R > 0 - && R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) + && R2 >= FLOAT_SQ(MIDDLE_DEAD_ZONE_R) #endif ); diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 7d0787180c..e56c3e4df7 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -812,11 +812,10 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t if (accel != 0) { inverse_accel = 1.0f / accel; const float half_inverse_accel = 0.5f * inverse_accel, - nominal_rate_sq = sq(float(block->nominal_rate)), + nominal_rate_sq = FLOAT_SQ(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))), - accelerate_steps_float = half_inverse_accel * (nominal_rate_sq - sq(float(initial_rate))); - // Aims to fully reach nominal and final rates + 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)); accelerate_steps = CEIL(accelerate_steps_float); decelerate_steps = CEIL(decelerate_steps_float); diff --git a/Marlin/src/module/scara.h b/Marlin/src/module/scara.h index 8ce50e55e1..c574b2f818 100644 --- a/Marlin/src/module/scara.h +++ b/Marlin/src/module/scara.h @@ -32,8 +32,8 @@ extern float segments_per_second; #if ENABLED(AXEL_TPARA) float constexpr L1 = TPARA_LINKAGE_1, L2 = TPARA_LINKAGE_2, // Float constants for Robot arm calculations - L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2, - L2_2 = sq(float(L2)); + L1_2 = FLOAT_SQ(L1), L1_2_2 = 2.0 * L1_2, + L2_2 = FLOAT_SQ(L2); void forward_kinematics(const_float_t a, const_float_t b, const_float_t c); void home_TPARA(); @@ -41,8 +41,8 @@ extern float segments_per_second; #else float constexpr L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2, // Float constants for SCARA calculations - L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2, - L2_2 = sq(float(L2)); + L1_2 = FLOAT_SQ(L1), L1_2_2 = 2.0 * L1_2, + L2_2 = FLOAT_SQ(L2); void forward_kinematics(const_float_t a, const_float_t b); From fb05d3d1e60f1dbac2db074213925c1603205176 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Wed, 15 May 2024 15:30:22 -0500 Subject: [PATCH 6/6] match modified names --- Marlin/src/module/planner.cpp | 8 ++++---- Marlin/src/module/planner.h | 4 ++-- Marlin/src/module/stepper.cpp | 18 +++++++++--------- Marlin/src/module/stepper.h | 4 ++-- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index bdade9adda..20b459fdb4 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -849,8 +849,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; @@ -3074,8 +3074,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(); diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 5df0dfad32..c226625a30 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -238,8 +238,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 on which to start cruising + 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 diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 83bc742f32..5c98ea02b2 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -59,8 +59,8 @@ * 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. + * 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. */ @@ -225,8 +225,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) @@ -2404,7 +2404,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!) @@ -2461,7 +2461,7 @@ 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) @@ -2568,7 +2568,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) { 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; @@ -2727,8 +2727,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)); diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 43d1012f10..cf0348ba59 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -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 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)