From 752f3d440dd72d869140268dc79f3efb49043c1e Mon Sep 17 00:00:00 2001 From: tombrazier <68918209+tombrazier@users.noreply.github.com> Date: Sat, 16 Jul 2022 00:15:51 +0100 Subject: [PATCH] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Optimize=20Planner=20calcu?= =?UTF-8?q?lations=20(#24484,=20#24509)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/module/planner.cpp | 226 +++++++++++++++++----------------- Marlin/src/module/planner.h | 26 +--- 2 files changed, 115 insertions(+), 137 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index a4e4686ba5..0ab8d85907 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -28,12 +28,14 @@ * Derived from Grbl * Copyright (c) 2009-2011 Simen Svale Skogsrud * - * The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. + * Ring buffer gleaned from wiring_serial library by David A. Mellis. * + * Fast inverse function needed for Bézier interpolation for AVR + * was designed, written and tested by Eduardo José Tagle, April 2018. * - * Reasoning behind the mathematics in this module (in the key of 'Mathematica'): + * Planner mathematics (Mathematica-style): * - * s == speed, a == acceleration, t == time, d == distance + * Where: s == speed, a == acceleration, t == time, d == distance * * Basic definitions: * Speed[s_, a_, t_] := s + (a*t) @@ -41,7 +43,7 @@ * * Distance to reach a specific speed with a constant acceleration: * Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t] - * d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance() + * d -> (m^2 - s^2) / (2 a) * * Speed after a given distance of travel with constant acceleration: * Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t] @@ -49,17 +51,18 @@ * * DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2] * - * When to start braking (di) to reach a specified destination speed (s2) after accelerating - * from initial speed s1 without ever stopping at a plateau: + * When to start braking (di) to reach a specified destination speed (s2) after + * acceleration from initial speed s1 without ever reaching a plateau: * Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di] - * di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance() + * di -> (2 a d - s1^2 + s2^2)/(4 a) * - * IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) + * We note, as an optimization, that if we have already calculated an + * acceleration distance d1 from s1 to m and a deceration distance d2 + * from m to s2 then * - * -- - * - * The fast inverse function needed for Bézier interpolation for AVR - * was designed, written and tested by Eduardo José Tagle on April/2018 + * d1 -> (m^2 - s1^2) / (2 a) + * d2 -> (m^2 - s2^2) / (2 a) + * di -> (d + d1 - d2) / 2 */ #include "planner.h" @@ -211,7 +214,7 @@ xyze_long_t Planner::position{0}; uint32_t Planner::acceleration_long_cutoff; xyze_float_t Planner::previous_speed; -float Planner::previous_nominal_speed_sqr; +float Planner::previous_nominal_speed; #if ENABLED(DISABLE_INACTIVE_EXTRUDER) last_move_t Planner::g_uc_extruder_last_move[E_STEPPERS] = { 0 }; @@ -220,7 +223,7 @@ float Planner::previous_nominal_speed_sqr; #ifdef XY_FREQUENCY_LIMIT int8_t Planner::xy_freq_limit_hz = XY_FREQUENCY_LIMIT; float Planner::xy_freq_min_speed_factor = (XY_FREQUENCY_MIN_PERCENT) * 0.01f; - int32_t Planner::xy_freq_min_interval_us = LROUND(1000000.0 / (XY_FREQUENCY_LIMIT)); + int32_t Planner::xy_freq_min_interval_us = LROUND(1000000.0f / (XY_FREQUENCY_LIMIT)); #endif #if ENABLED(LIN_ADVANCE) @@ -250,7 +253,7 @@ void Planner::init() { TERN_(HAS_POSITION_FLOAT, position_float.reset()); TERN_(IS_KINEMATIC, position_cart.reset()); previous_speed.reset(); - previous_nominal_speed_sqr = 0; + previous_nominal_speed = 0; TERN_(ABL_PLANAR, bed_level_matrix.set_to_identity()); clear_block_buffer(); delay_before_delivering = 0; @@ -786,41 +789,48 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE)); #if ENABLED(S_CURVE_ACCELERATION) - uint32_t cruise_rate = initial_rate; + // If we have some plateau time, the cruise rate will be the nominal rate + uint32_t cruise_rate = block->nominal_rate; #endif const int32_t accel = block->acceleration_steps_per_s2; - // Steps required for acceleration, deceleration to/from nominal rate - uint32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)), - decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)); - // Steps between acceleration and deceleration, if any - int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps; + // Steps for acceleration, plateau and deceleration + int32_t plateau_steps = block->step_event_count; + uint32_t accelerate_steps = 0, + decelerate_steps = 0; - // Does accelerate_steps + decelerate_steps exceed step_event_count? - // Then we can't possibly reach the nominal rate, there will be no cruising. - // Use intersection_distance() to calculate accel / braking time in order to - // reach the final_rate exactly at the end of this block. - if (plateau_steps < 0) { - const float accelerate_steps_float = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count)); - accelerate_steps = _MIN(uint32_t(_MAX(accelerate_steps_float, 0)), block->step_event_count); - decelerate_steps = block->step_event_count - accelerate_steps; - plateau_steps = 0; + if (accel != 0) { + // Steps required for acceleration, deceleration to/from nominal rate + const float nominal_rate_sq = sq(float(block->nominal_rate)); + float accelerate_steps_float = (nominal_rate_sq - sq(float(initial_rate))) * (0.5f / accel); + accelerate_steps = CEIL(accelerate_steps_float); + const float decelerate_steps_float = (nominal_rate_sq - sq(float(final_rate))) * (0.5f / accel); + decelerate_steps = FLOOR(decelerate_steps_float); - #if ENABLED(S_CURVE_ACCELERATION) - // We won't reach the cruising rate. Let's calculate the speed we will reach - cruise_rate = final_speed(initial_rate, accel, accelerate_steps); - #endif + // Steps between acceleration and deceleration, if any + plateau_steps -= accelerate_steps + decelerate_steps; + + // Does accelerate_steps + decelerate_steps exceed step_event_count? + // Then we can't possibly reach the nominal rate, there will be no cruising. + // Calculate accel / braking time in order to reach the final_rate exactly + // at the end of this block. + if (plateau_steps < 0) { + accelerate_steps_float = CEIL((block->step_event_count + accelerate_steps_float - decelerate_steps_float) * 0.5f); + accelerate_steps = _MIN(uint32_t(_MAX(accelerate_steps_float, 0)), block->step_event_count); + decelerate_steps = block->step_event_count - accelerate_steps; + + #if ENABLED(S_CURVE_ACCELERATION) + // We won't reach the cruising rate. Let's calculate the speed we will reach + cruise_rate = final_speed(initial_rate, accel, accelerate_steps); + #endif + } } - #if ENABLED(S_CURVE_ACCELERATION) - else // We have some plateau time, so the cruise rate will be the nominal rate - cruise_rate = block->nominal_rate; - #endif #if ENABLED(S_CURVE_ACCELERATION) // Jerk controlled speed requires to express speed versus time, NOT steps - uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE), - deceleration_time = ((float)(cruise_rate - final_rate) / accel) * (STEPPER_TIMER_RATE), + uint32_t acceleration_time = (float(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE), + deceleration_time = (float(cruise_rate - final_rate) / accel) * (STEPPER_TIMER_RATE), // And to offload calculations from the ISR, we also calculate the inverse of those times here acceleration_time_inverse = get_period_inverse(acceleration_time), deceleration_time_inverse = get_period_inverse(deceleration_time); @@ -1175,7 +1185,7 @@ void Planner::recalculate_trapezoids(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t // 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.0, next_entry_speed = 0.0; + float current_entry_speed = 0.0f, next_entry_speed = 0.0f; while (block_index != head_block_index) { next = &block_buffer[block_index]; @@ -1199,13 +1209,12 @@ void Planner::recalculate_trapezoids(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t // 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 current_nominal_speed = SQRT(block->nominal_speed_sqr), - nomr = 1.0f / current_nominal_speed; + const float nomr = 1.0f / block->nominal_speed; calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr); #if ENABLED(LIN_ADVANCE) if (block->use_advance_lead) { const float comp = block->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS]; - block->max_adv_steps = current_nominal_speed * comp; + block->max_adv_steps = block->nominal_speed * comp; block->final_adv_steps = next_entry_speed * comp; } #endif @@ -1240,13 +1249,12 @@ void Planner::recalculate_trapezoids(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t if (!stepper.is_block_busy(block)) { // Block is not BUSY, we won the race against the Stepper ISR: - const float current_nominal_speed = SQRT(block->nominal_speed_sqr), - nomr = 1.0f / current_nominal_speed; + const float nomr = 1.0f / block->nominal_speed; calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr); #if ENABLED(LIN_ADVANCE) if (block->use_advance_lead) { const float comp = block->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS]; - block->max_adv_steps = current_nominal_speed * comp; + block->max_adv_steps = block->nominal_speed * comp; block->final_adv_steps = next_entry_speed * comp; } #endif @@ -1290,14 +1298,10 @@ void Planner::recalculate(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t safe_exit_s #define FAN_SET(F) do{ kickstart_fan(fan_speed, ms, F); _FAN_SET(F); }while(0) const millis_t ms = millis(); - TERN_(HAS_FAN0, FAN_SET(0)); - TERN_(HAS_FAN1, FAN_SET(1)); - TERN_(HAS_FAN2, FAN_SET(2)); - TERN_(HAS_FAN3, FAN_SET(3)); - TERN_(HAS_FAN4, FAN_SET(4)); - TERN_(HAS_FAN5, FAN_SET(5)); - TERN_(HAS_FAN6, FAN_SET(6)); - TERN_(HAS_FAN7, FAN_SET(7)); + TERN_(HAS_FAN0, FAN_SET(0)); TERN_(HAS_FAN1, FAN_SET(1)); + TERN_(HAS_FAN2, FAN_SET(2)); TERN_(HAS_FAN3, FAN_SET(3)); + TERN_(HAS_FAN4, FAN_SET(4)); TERN_(HAS_FAN5, FAN_SET(5)); + TERN_(HAS_FAN6, FAN_SET(6)); TERN_(HAS_FAN7, FAN_SET(7)); } #if FAN_KICKSTART_TIME @@ -1479,7 +1483,7 @@ void Planner::check_axes_activity() { for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) { const block_t * const block = &block_buffer[b]; if (LINEAR_AXIS_GANG(block->steps.x, || block->steps.y, || block->steps.z, || block->steps.i, || block->steps.j, || block->steps.k)) { - const float se = (float)block->steps.e / block->step_event_count * SQRT(block->nominal_speed_sqr); // mm/sec + const float se = float(block->steps.e) / block->step_event_count * block->nominal_speed; // mm/sec NOLESS(high, se); } } @@ -1919,7 +1923,7 @@ bool Planner::_populate_block( #if ENABLED(MIXING_EXTRUDER) bool ignore_e = false; float collector[MIXING_STEPPERS]; - mixer.refresh_collector(1.0, mixer.get_current_vtool(), collector); + mixer.refresh_collector(1.0f, mixer.get_current_vtool(), collector); MIXER_STEPPER_LOOP(e) if (e_steps * collector[e] > max_e_steps) { ignore_e = true; break; } #else @@ -2081,9 +2085,6 @@ bool Planner::_populate_block( steps_dist_mm.b = (db + dc) * mm_per_step[B_AXIS]; steps_dist_mm.c = CORESIGN(db - dc) * mm_per_step[C_AXIS]; #endif - TERN_(HAS_I_AXIS, steps_dist_mm.i = di * mm_per_step[I_AXIS]); - TERN_(HAS_J_AXIS, steps_dist_mm.j = dj * mm_per_step[J_AXIS]); - TERN_(HAS_K_AXIS, steps_dist_mm.k = dk * mm_per_step[K_AXIS]); #elif ENABLED(MARKFORGED_XY) steps_dist_mm.a = (da - db) * mm_per_step[A_AXIS]; steps_dist_mm.b = db * mm_per_step[B_AXIS]; @@ -2123,40 +2124,50 @@ bool Planner::_populate_block( if (hints.millimeters) block->millimeters = hints.millimeters; else { - block->millimeters = SQRT( + /** + * Distance for interpretation of feedrate in accordance with LinuxCNC (the successor of NIST + * RS274NGC interpreter - version 3) and its default CANON_XYZ feed reference mode. + * Assume that X, Y, Z are the primary linear axes and U, V, W are secondary linear axes and A, B, C are + * rotational axes. Then dX, dY, dZ are the displacements of the primary linear axes and dU, dV, dW are the displacements of linear axes and + * dA, dB, dC are the displacements of rotational axes. + * The time it takes to execute move command with feedrate F is t = D/F, where D is the total distance, calculated as follows: + * D^2 = dX^2 + dY^2 + dZ^2 + * if D^2 == 0 (none of XYZ move but any secondary linear axes move, whether other axes are moved or not): + * D^2 = dU^2 + dV^2 + dW^2 + * if D^2 == 0 (only rotational axes are moved): + * D^2 = dA^2 + dB^2 + dC^2 + */ + float distance_sqr = ( #if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX) - LINEAR_AXIS_GANG( - sq(steps_dist_mm.head.x), + sq(steps_dist_mm.head.y), + sq(steps_dist_mm.z), - + sq(steps_dist_mm.i), + sq(steps_dist_mm.j), + sq(steps_dist_mm.k) - ) + XYZ_GANG(sq(steps_dist_mm.head.x), + sq(steps_dist_mm.head.y), + sq(steps_dist_mm.z)) #elif CORE_IS_XZ - LINEAR_AXIS_GANG( - sq(steps_dist_mm.head.x), + sq(steps_dist_mm.y), + sq(steps_dist_mm.head.z), - + sq(steps_dist_mm.i), + sq(steps_dist_mm.j), + sq(steps_dist_mm.k) - ) + XYZ_GANG(sq(steps_dist_mm.head.x), + sq(steps_dist_mm.y), + sq(steps_dist_mm.head.z)) #elif CORE_IS_YZ - LINEAR_AXIS_GANG( - sq(steps_dist_mm.x) + sq(steps_dist_mm.head.y) + sq(steps_dist_mm.head.z) - + sq(steps_dist_mm.i), + sq(steps_dist_mm.j), + sq(steps_dist_mm.k) - ) - #elif ENABLED(FOAMCUTTER_XYUV) - // Return the largest distance move from either X/Y or I/J plane - #if HAS_J_AXIS - _MAX(sq(steps_dist_mm.x) + sq(steps_dist_mm.y), sq(steps_dist_mm.i) + sq(steps_dist_mm.j)) - #else - sq(steps_dist_mm.x) + sq(steps_dist_mm.y) - #endif + XYZ_GANG(sq(steps_dist_mm.x), + sq(steps_dist_mm.head.y), + sq(steps_dist_mm.head.z)) #else - XYZ_GANG(sq(steps_dist_mm.x), + sq(steps_dist_mm.y), + sq(steps_dist_mm.z)) + XYZ_GANG(sq(steps_dist_mm.x), + sq(steps_dist_mm.y), + sq(steps_dist_mm.z)) #endif ); + + #if SECONDARY_AXES >= 1 + if (NEAR_ZERO(distance_sqr)) { + // Move does not involve any primary linear axes (xyz) but might involve secondary linear axes + distance_sqr = (0.0f + SECONDARY_AXIS_GANG( + + sq(steps_dist_mm.i), + sq(steps_dist_mm.j), + sq(steps_dist_mm.k) + ) + ); + } + #endif + + block->millimeters = SQRT(distance_sqr); } /** * At this point at least one of the axes has more steps than - * MIN_STEPS_PER_SEGMENT, ensuring the segment won't get dropped - * as zero-length. It's important to not apply corrections to blocks - * that would get dropped! + * MIN_STEPS_PER_SEGMENT, ensuring the segment won't get dropped as + * zero-length. It's important to not apply corrections + * to blocks that would get dropped! * * A correction function is permitted to add steps to an axis, it * should *never* remove steps! @@ -2277,7 +2288,8 @@ bool Planner::_populate_block( const float inverse_millimeters = 1.0f / block->millimeters; // Inverse millimeters to remove multiple divides // Calculate inverse time for this move. No divide by zero due to previous checks. - // Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0. + // Example: At 120mm/s a 60mm move involving XYZ axes takes 0.5s. So this will give 2.0. + // Example 2: At 120°/s a 60° move involving only rotational axes takes 0.5s. So this will give 2.0. float inverse_secs = fr_mm_s * inverse_millimeters; // Get the number of non busy movements in queue (non busy means that they can be altered) @@ -2316,7 +2328,7 @@ bool Planner::_populate_block( if (was_enabled) stepper.wake_up(); #endif - block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0 + block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0 block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0 #if ENABLED(FILAMENT_WIDTH_SENSOR) @@ -2412,7 +2424,7 @@ bool Planner::_populate_block( if (speed_factor < 1.0f) { current_speed *= speed_factor; block->nominal_rate *= speed_factor; - block->nominal_speed_sqr = block->nominal_speed_sqr * sq(speed_factor); + block->nominal_speed *= speed_factor; } // Compute and limit the acceleration rate for the trapezoid generator. @@ -2509,7 +2521,7 @@ bool Planner::_populate_block( if (block->use_advance_lead) { block->advance_speed = (STEPPER_TIMER_RATE) / (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * settings.axis_steps_per_mm[E_AXIS_N(extruder)]); #if ENABLED(LA_DEBUG) - if (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * 2 < SQRT(block->nominal_speed_sqr) * block->e_D_ratio) + if (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * 2 < block->nominal_speed * block->e_D_ratio) SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed."); if (block->advance_speed < 200) SERIAL_ECHOLNPGM("eISR running at > 10kHz."); @@ -2577,7 +2589,7 @@ bool Planner::_populate_block( unit_vec *= inverse_millimeters; // Use pre-calculated (1 / SQRT(x^2 + y^2 + z^2)) // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. - if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) { + if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. float junction_cos_theta = LOGICAL_AXIS_GANG( @@ -2703,7 +2715,7 @@ bool Planner::_populate_block( } // Get the lowest speed - vmax_junction_sqr = _MIN(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr); + 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; @@ -2712,27 +2724,17 @@ bool Planner::_populate_block( #endif - #ifdef USE_CACHED_SQRT - #define CACHED_SQRT(N, V) \ - static float saved_V, N; \ - if (V != saved_V) { N = SQRT(V); saved_V = V; } - #else - #define CACHED_SQRT(N, V) const float N = SQRT(V) - #endif - #if HAS_CLASSIC_JERK /** * Adapted from Průša MKS firmware * https://github.com/prusa3d/Prusa-Firmware */ - CACHED_SQRT(nominal_speed, block->nominal_speed_sqr); - // Exit speed limited by a jerk to full halt of a previous last segment static float previous_safe_speed; // Start with a safe speed (from which the machine may halt to stop immediately). - float safe_speed = nominal_speed; + float safe_speed = block->nominal_speed; #ifndef TRAVEL_EXTRA_XYJERK #define TRAVEL_EXTRA_XYJERK 0 @@ -2745,7 +2747,7 @@ bool Planner::_populate_block( maxj = (max_jerk[i] + (i == X_AXIS || i == Y_AXIS ? extra_xyjerk : 0.0f)); // mj : The max jerk setting for this axis if (jerk > maxj) { // cs > mj : New current speed too fast? if (limited) { // limited already? - const float mjerk = nominal_speed * maxj; // ns*mj + const float mjerk = block->nominal_speed * maxj; // ns*mj if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; // ns*mj/cs } else { @@ -2756,7 +2758,7 @@ bool Planner::_populate_block( } float vmax_junction; - if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) { + if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) { // Estimate a maximum velocity allowed at a joint of two successive segments. // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities, // then the machine is not coasting anymore and the safe entry / exit velocities shall be used. @@ -2767,11 +2769,9 @@ bool Planner::_populate_block( // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum. // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting. - CACHED_SQRT(previous_nominal_speed, previous_nominal_speed_sqr); - float smaller_speed_factor = 1.0f; - if (nominal_speed < previous_nominal_speed) { - vmax_junction = nominal_speed; + if (block->nominal_speed < previous_nominal_speed) { + vmax_junction = block->nominal_speed; smaller_speed_factor = vmax_junction / previous_nominal_speed; } else @@ -2838,11 +2838,11 @@ bool Planner::_populate_block( // 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(block->nominal_speed_sqr <= v_allowable_sqr); + block->flag.set_nominal(sq(block->nominal_speed) <= v_allowable_sqr); // Update previous path unit_vector and nominal speed previous_speed = current_speed; - previous_nominal_speed_sqr = block->nominal_speed_sqr; + previous_nominal_speed = block->nominal_speed; position = target; // Update the position @@ -3039,12 +3039,12 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s const xyze_pos_t cart_dist_mm = LOGICAL_AXIS_ARRAY( cart.e - position_cart.e, cart.x - position_cart.x, cart.y - position_cart.y, cart.z - position_cart.z, - cart.i - position_cart.i, cart.j - position_cart.j, cart.j - position_cart.k + cart.i - position_cart.i, cart.j - position_cart.j, cart.k - position_cart.k ); #else const xyz_pos_t cart_dist_mm = LINEAR_AXIS_ARRAY( cart.x - position_cart.x, cart.y - position_cart.y, cart.z - position_cart.z, - cart.i - position_cart.i, cart.j - position_cart.j, cart.j - position_cart.k + cart.i - position_cart.i, cart.j - position_cart.j, cart.k - position_cart.k ); #endif @@ -3156,7 +3156,7 @@ void Planner::set_machine_position_mm(const abce_pos_t &abce) { ); if (has_blocks_queued()) { - //previous_nominal_speed_sqr = 0.0; // Reset planner junction speeds. Assume start from rest. + //previous_nominal_speed = 0.0f; // Reset planner junction speeds. Assume start from rest. //previous_speed.reset(); buffer_sync_block(BLOCK_BIT_SYNC_POSITION); } @@ -3232,7 +3232,7 @@ void Planner::refresh_positioning() { inline void limit_and_warn(float &val, const AxisEnum axis, PGM_P const setting_name, const xyze_float_t &max_limit) { const uint8_t lim_axis = TERN_(HAS_EXTRUDERS, axis > E_AXIS ? E_AXIS :) axis; const float before = val; - LIMIT(val, 0.1, max_limit[lim_axis]); + LIMIT(val, 0.1f, max_limit[lim_axis]); if (before != val) { SERIAL_CHAR(AXIS_CHAR(lim_axis)); SERIAL_ECHOPGM(" Max "); diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index bab5307f8b..642e32050a 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -198,7 +198,7 @@ typedef struct PlannerBlock { volatile bool is_move() { return !(is_sync() || is_page()); } // Fields used by the motion planner to manage acceleration - float nominal_speed_sqr, // The nominal speed for this block in (mm/sec)^2 + 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 max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2 millimeters, // The total travel of this block in mm @@ -509,7 +509,7 @@ class Planner { /** * Nominal speed of previous path line segment (mm/s)^2 */ - static float previous_nominal_speed_sqr; + static float previous_nominal_speed; /** * Limit where 64bit math is necessary for acceleration calculation @@ -1007,28 +1007,6 @@ class Planner { static constexpr uint8_t next_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index + 1); } static constexpr uint8_t prev_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index - 1); } - /** - * Calculate the distance (not time) it takes to accelerate - * from initial_rate to target_rate using the given acceleration: - */ - static float estimate_acceleration_distance(const_float_t initial_rate, const_float_t target_rate, const_float_t accel) { - if (accel == 0) return 0; // accel was 0, set acceleration distance to 0 - return (sq(target_rate) - sq(initial_rate)) / (accel * 2); - } - - /** - * Return the point at which you must start braking (at the rate of -'accel') if - * you start at 'initial_rate', accelerate (until reaching the point), and want to end at - * 'final_rate' after traveling 'distance'. - * - * This is used to compute the intersection point between acceleration and deceleration - * in cases where the "trapezoid" has no plateau (i.e., never reaches maximum speed) - */ - static float intersection_distance(const_float_t initial_rate, const_float_t final_rate, const_float_t accel, const_float_t distance) { - if (accel == 0) return 0; // accel was 0, set intersection distance to 0 - return (accel * 2 * distance - sq(initial_rate) + sq(final_rate)) / (accel * 4); - } - /** * Calculate the maximum allowable speed squared at this point, in order * to reach 'target_velocity_sqr' using 'acceleration' within a given