From f9451f00e9188a45666a5b1466a3b4f7b957741b Mon Sep 17 00:00:00 2001 From: Mihail Dumitrescu Date: Mon, 22 Apr 2024 12:10:26 +0300 Subject: [PATCH] Add and enforce min_entry_speed_sqr. Remove nominal_length, add min_entry_speed_sqr, initial clean up of reverse_pass_kernel and forward_pass_kernel. Added min_entry_speed_sqr to avoid a specific potential source of judder due to working with discrete steps rather than 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 | 127 ++++++++++++---------------------- Marlin/src/module/planner.h | 11 +-- 2 files changed, 48 insertions(+), 90 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index c21230f662b3..23f861836b9a 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -784,7 +784,9 @@ block_t* Planner::get_current_block() { /** * Calculate trapezoid parameters, multiplying the entry- and exit-speeds - * by the provided factors. + * by the provided factors. Requires that initial_rate and final_rate are + * no less than sqrt(block->acceleration_steps_per_s2 / 2), which is ensured + * through minimum_planner_speed_sqr in _populate_block(). ** * ############ VERY IMPORTANT ############ * NOTE that the PRECONDITION to call this function is that the block is @@ -940,7 +942,7 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t * neighboring blocks. * b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) * with a maximum allowable deceleration over the block travel distance. - * c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). + * c. The last (or newest appended) block is planned from safe_exit_speed_sqr. * 2. Go over every block in chronological (forward) order and dial down junction speed values if * a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable * acceleration over the block travel distance. @@ -996,29 +998,13 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t // The kernel called by recalculate() when scanning the plan from last to first entry. void Planner::reverse_pass_kernel(block_t * const current, const block_t * const next, const_float_t safe_exit_speed_sqr) { - if (current) { - // If entry speed is already at the maximum entry speed, and there was no change of speed - // in the next block, there is no need to recheck. Block is cruising and there is no need to - // compute anything for this block, - // If not, block entry speed needs to be recalculated to ensure maximum possible planned speed. - const float max_entry_speed_sqr = current->max_entry_speed_sqr; - - // Compute maximum entry speed decelerating over the current block from its exit speed. - // If not at the maximum entry speed, or the previous block entry speed changed - if (current->entry_speed_sqr != max_entry_speed_sqr || (next && next->flag.recalculate)) { - - // If nominal length true, max junction speed is guaranteed to be reached. - // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then - // the current block and next block junction speeds are guaranteed to always be at their maximum - // junction speeds in deceleration and acceleration, respectively. This is due to how the current - // block nominal speed limits both the current and next maximum junction speeds. Hence, in both - // the reverse and forward planners, the corresponding block junction speed will always be at the - // the maximum junction speed and may always be ignored for any speed reduction checks. - - const float next_entry_speed_sqr = next ? next->entry_speed_sqr : safe_exit_speed_sqr, - new_entry_speed_sqr = current->flag.nominal_length - ? max_entry_speed_sqr - : _MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next_entry_speed_sqr, current->millimeters)); + // We need to recalculate only for the last block added or if next->entry_speed_sqr changed. + if (!next || next->flag.recalculate) { + // And only if we're not already at max entry speed. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + const float next_entry_speed_sqr = next ? next->entry_speed_sqr : safe_exit_speed_sqr; + float new_entry_speed_sqr = max_allowable_speed_sqr(-current->acceleration, next_entry_speed_sqr, current->millimeters); + NOMORE(new_entry_speed_sqr, current->max_entry_speed_sqr); if (current->entry_speed_sqr != new_entry_speed_sqr) { // Need to recalculate the block speed - Mark it now, so the stepper @@ -1094,41 +1080,26 @@ void Planner::reverse_pass(const_float_t safe_exit_speed_sqr) { // The kernel called by recalculate() when scanning the plan from first to last entry. void Planner::forward_pass_kernel(const block_t * const previous, block_t * const current, const uint8_t block_index) { - if (previous) { - // If the previous block is an acceleration block, too short to complete the full speed - // change, adjust the entry speed accordingly. Entry speeds have already been reset, - // maximized, and reverse-planned. If nominal length is set, max junction speed is - // guaranteed to be reached. No need to recheck. - if (!previous->flag.nominal_length && previous->entry_speed_sqr < current->entry_speed_sqr) { - - // Compute the maximum allowable speed - const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters); - - // If true, current block is full-acceleration and we can move the planned pointer forward. + // Check against previous speed only on current->entry_speed_sqr changes (or if first time). + if (current->flag.recalculate) { + // If the previous block is accelerating check if it's too short to complete the full speed + // change then adjust the entry speed accordingly. Entry speeds have already been maximized. + if (previous->entry_speed_sqr < current->entry_speed_sqr) { + float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters); + + // If true, previous block is full-acceleration and we can move the planned pointer forward. if (new_entry_speed_sqr < current->entry_speed_sqr) { + // Current entry speed limited by full acceleration from previous entry speed. + // Make sure entry speed not lower than minimum_planner_speed_sqr. + NOLESS(new_entry_speed_sqr, current->min_entry_speed_sqr); + current->entry_speed_sqr = new_entry_speed_sqr; - // Mark we need to recompute the trapezoidal shape, and do it now, - // so the stepper ISR does not consume the block before being recalculated - current->flag.recalculate = true; - - // But there is an inherent race condition here, as the block maybe - // became BUSY, just before it was marked as RECALCULATE, so check - // if that is the case! - if (stepper.is_block_busy(current)) { - // Block became busy. Clear the RECALCULATE flag (no point in - // recalculating BUSY blocks and don't set its speed, as it can't - // be updated at this time. - current->flag.recalculate = false; - } - else { - // Block is not BUSY, we won the race against the Stepper ISR: - - // Always <= max_entry_speed_sqr. Backward pass sets this. - current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. - - // Set optimal plan pointer. - block_buffer_planned = block_index; - } + // Set optimal plan pointer. + block_buffer_planned = block_index; + } + else { + // Previous entry speed has been maximized. + block_buffer_planned = prev_block_index(block_index); } } @@ -1170,7 +1141,7 @@ void Planner::forward_pass() { // the previous block became BUSY, so assume the current block's // entry speed can't be altered (since that would also require // updating the exit speed of the previous block). - if (!previous || !stepper.is_block_busy(previous)) + if (previous && !stepper.is_block_busy(previous)) forward_pass_kernel(previous, block, block_index); previous = block; } @@ -2565,9 +2536,13 @@ bool Planner::_populate_block( } #endif - // The minimum possible speed is the average speed for - // the first / last step at current acceleration limit + // Formula for the average speed over a 1 step worth of distance if starting from zero and + // accelerating at the current limit. Since we can only change the speed every step this is a + // good lower limit for the entry and exit speeds. Note that for calculate_trapezoid_for_block() + // to work correctly, this must be accurately set and propagated. minimum_planner_speed_sqr = 0.5f * block->acceleration / steps_per_mm; + // Go straight to/from nominal speed if block->acceleration is too high for it. + NOMORE(minimum_planner_speed_sqr, sq(block->nominal_speed)); float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2 @@ -2763,8 +2738,7 @@ bool Planner::_populate_block( // Get the lowest speed vmax_junction_sqr = _MIN(vmax_junction_sqr, sq(block->nominal_speed), sq(previous_nominal_speed)); } - else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later. - vmax_junction_sqr = 0; + else vmax_junction_sqr = minimum_planner_speed_sqr; prev_unit_vec = unit_vec; @@ -2806,8 +2780,7 @@ bool Planner::_populate_block( xyze_float_t speed_diff = current_speed; float vmax_junction; - const bool start_from_zero = !moves_queued || UNEAR_ZERO(previous_nominal_speed); - if (start_from_zero) { + if (!moves_queued || UNEAR_ZERO(previous_nominal_speed)) { // Limited by a jerk to/from full halt. vmax_junction = block->nominal_speed; } @@ -2837,28 +2810,20 @@ bool Planner::_populate_block( } vmax_junction_sqr = sq(vmax_junction * v_factor); - if (start_from_zero) minimum_planner_speed_sqr = vmax_junction_sqr; - #endif // CLASSIC_JERK + // High acceleration limits override low jerk/junction deviation limits (as fixing trapezoids + // or reducing acceleration introduces too much complexity and/or too much compute) + NOLESS(vmax_junction_sqr, minimum_planner_speed_sqr); + // Max entry speed of this block equals the max exit speed of the previous block. block->max_entry_speed_sqr = vmax_junction_sqr; - - // Initialize block entry speed. Compute based on deceleration to sqrt(minimum_planner_speed_sqr). - const float v_allowable_sqr = max_allowable_speed_sqr(-block->acceleration, minimum_planner_speed_sqr, block->millimeters); - - // Start with the minimum allowed speed + // Set entry speed. The reverse and forward passes will optimize it later. block->entry_speed_sqr = minimum_planner_speed_sqr; + // Set min entry speed. Rarely it could be higher than the previous nominal speed but that's ok. + block->min_entry_speed_sqr = minimum_planner_speed_sqr; - // Initialize planner efficiency flags - // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. - // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then - // the current block and next block junction speeds are guaranteed to always be at their maximum - // junction speeds in deceleration and acceleration, respectively. This is due to how the current - // block nominal speed limits both the current and next maximum junction speeds. Hence, in both - // the reverse and forward planners, the corresponding block junction speed will always be at the - // the maximum junction speed and may always be ignored for any speed reduction checks. - block->flag.set_nominal(sq(block->nominal_speed) <= v_allowable_sqr); + block->flag.recalculate = true; // Update previous path unit_vector and nominal speed previous_speed = current_speed; diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index b7b1abbb6153..fd99c7b3c148 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