andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

⚡️ Optimize Planner calculations (#24484)

FB4S_WIFI
tombrazier 2 years ago
committed by Scott Lahteine
parent
0a164a88fe
commit
cc4fc28fe0
2 changed files with 80 additions and 110 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 138
      Marlin/src/module/planner.cpp
  2. 26
      Marlin/src/module/planner.h

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

@ -28,12 +28,14 @@
* Derived from Grbl * Derived from Grbl
* Copyright (c) 2009-2011 Simen Svale Skogsrud * 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: * Basic definitions:
* Speed[s_, a_, t_] := s + (a*t) * Speed[s_, a_, t_] := s + (a*t)
@ -41,7 +43,7 @@
* *
* Distance to reach a specific speed with a constant acceleration: * Distance to reach a specific speed with a constant acceleration:
* Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t] * 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: * Speed after a given distance of travel with constant acceleration:
* Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t] * 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] * DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
* *
* When to start braking (di) to reach a specified destination speed (s2) after accelerating * When to start braking (di) to reach a specified destination speed (s2) after
* from initial speed s1 without ever stopping at a plateau: * acceleration from initial speed s1 without ever reaching a plateau:
* Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di] * 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
* *
* -- * d1 -> (m^2 - s1^2) / (2 a)
* * d2 -> (m^2 - s2^2) / (2 a)
* The fast inverse function needed for Bézier interpolation for AVR * di -> (d + d1 - d2) / 2
* was designed, written and tested by Eduardo José Tagle on April/2018
*/ */
#include "planner.h" #include "planner.h"
@ -211,7 +214,7 @@ xyze_long_t Planner::position{0};
uint32_t Planner::acceleration_long_cutoff; uint32_t Planner::acceleration_long_cutoff;
xyze_float_t Planner::previous_speed; xyze_float_t Planner::previous_speed;
float Planner::previous_nominal_speed_sqr; float Planner::previous_nominal_speed;
#if ENABLED(DISABLE_INACTIVE_EXTRUDER) #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
last_move_t Planner::g_uc_extruder_last_move[E_STEPPERS] = { 0 }; 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 #ifdef XY_FREQUENCY_LIMIT
int8_t Planner::xy_freq_limit_hz = 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; 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 #endif
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
@ -250,7 +253,7 @@ void Planner::init() {
TERN_(HAS_POSITION_FLOAT, position_float.reset()); TERN_(HAS_POSITION_FLOAT, position_float.reset());
TERN_(IS_KINEMATIC, position_cart.reset()); TERN_(IS_KINEMATIC, position_cart.reset());
previous_speed.reset(); previous_speed.reset();
previous_nominal_speed_sqr = 0; previous_nominal_speed = 0;
TERN_(ABL_PLANAR, bed_level_matrix.set_to_identity()); TERN_(ABL_PLANAR, bed_level_matrix.set_to_identity());
clear_block_buffer(); clear_block_buffer();
delay_before_delivering = 0; 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)); NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE));
#if ENABLED(S_CURVE_ACCELERATION) #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 #endif
const int32_t accel = block->acceleration_steps_per_s2; const int32_t accel = block->acceleration_steps_per_s2;
// Steps for acceleration, plateau and deceleration
int32_t plateau_steps = block->step_event_count;
uint32_t accelerate_steps = 0,
decelerate_steps = 0;
if (accel != 0) {
// Steps required for acceleration, deceleration to/from nominal rate // Steps required for acceleration, deceleration to/from nominal rate
uint32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)), const float nominal_rate_sq = sq(float(block->nominal_rate));
decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)); 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 = decelerate_steps_float;
// Steps between acceleration and deceleration, if any // Steps between acceleration and deceleration, if any
int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps; plateau_steps -= accelerate_steps + decelerate_steps;
// Does accelerate_steps + decelerate_steps exceed step_event_count? // Does accelerate_steps + decelerate_steps exceed step_event_count?
// Then we can't possibly reach the nominal rate, there will be no cruising. // 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 // Calculate accel / braking time in order to reach the final_rate exactly
// reach the final_rate exactly at the end of this block. // at the end of this block.
if (plateau_steps < 0) { if (plateau_steps < 0) {
const float accelerate_steps_float = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count)); 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); accelerate_steps = _MIN(uint32_t(_MAX(accelerate_steps_float, 0)), block->step_event_count);
decelerate_steps = block->step_event_count - accelerate_steps; decelerate_steps = block->step_event_count - accelerate_steps;
plateau_steps = 0;
#if ENABLED(S_CURVE_ACCELERATION) #if ENABLED(S_CURVE_ACCELERATION)
// We won't reach the cruising rate. Let's calculate the speed we will reach // We won't reach the cruising rate. Let's calculate the speed we will reach
cruise_rate = final_speed(initial_rate, accel, accelerate_steps); cruise_rate = final_speed(initial_rate, accel, accelerate_steps);
#endif #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) #if ENABLED(S_CURVE_ACCELERATION)
// Jerk controlled speed requires to express speed versus time, NOT steps // Jerk controlled speed requires to express speed versus time, NOT steps
uint32_t acceleration_time = ((float)(cruise_rate - initial_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), 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 // And to offload calculations from the ISR, we also calculate the inverse of those times here
acceleration_time_inverse = get_period_inverse(acceleration_time), acceleration_time_inverse = get_period_inverse(acceleration_time),
deceleration_time_inverse = get_period_inverse(deceleration_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) // Go from the tail (currently executed block) to the first block, without including it)
block_t *block = nullptr, *next = nullptr; 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) { while (block_index != head_block_index) {
next = &block_buffer[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: // Block is not BUSY, we won the race against the Stepper ISR:
// NOTE: Entry and exit factors always > 0 by all previous logic operations. // NOTE: Entry and exit factors always > 0 by all previous logic operations.
const float current_nominal_speed = SQRT(block->nominal_speed_sqr), const float nomr = 1.0f / block->nominal_speed;
nomr = 1.0f / current_nominal_speed;
calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr); calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr);
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
if (block->use_advance_lead) { if (block->use_advance_lead) {
const float comp = block->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS]; 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; block->final_adv_steps = next_entry_speed * comp;
} }
#endif #endif
@ -1240,13 +1249,12 @@ void Planner::recalculate_trapezoids(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t
if (!stepper.is_block_busy(block)) { if (!stepper.is_block_busy(block)) {
// Block is not BUSY, we won the race against the Stepper ISR: // Block is not BUSY, we won the race against the Stepper ISR:
const float current_nominal_speed = SQRT(block->nominal_speed_sqr), const float nomr = 1.0f / block->nominal_speed;
nomr = 1.0f / current_nominal_speed;
calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr); calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr);
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
if (block->use_advance_lead) { if (block->use_advance_lead) {
const float comp = block->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS]; 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; block->final_adv_steps = next_entry_speed * comp;
} }
#endif #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) #define FAN_SET(F) do{ kickstart_fan(fan_speed, ms, F); _FAN_SET(F); }while(0)
const millis_t ms = millis(); const millis_t ms = millis();
TERN_(HAS_FAN0, FAN_SET(0)); TERN_(HAS_FAN0, FAN_SET(0)); TERN_(HAS_FAN1, FAN_SET(1));
TERN_(HAS_FAN1, FAN_SET(1)); TERN_(HAS_FAN2, FAN_SET(2)); TERN_(HAS_FAN3, FAN_SET(3));
TERN_(HAS_FAN2, FAN_SET(2)); TERN_(HAS_FAN4, FAN_SET(4)); TERN_(HAS_FAN5, FAN_SET(5));
TERN_(HAS_FAN3, FAN_SET(3)); TERN_(HAS_FAN6, FAN_SET(6)); TERN_(HAS_FAN7, FAN_SET(7));
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 #if FAN_KICKSTART_TIME
@ -1485,7 +1489,7 @@ void Planner::check_axes_activity() {
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) { for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
const block_t * const block = &block_buffer[b]; const block_t * const block = &block_buffer[b];
if (NUM_AXIS_GANG(block->steps.x, || block->steps.y, || block->steps.z, || block->steps.i, || block->steps.j, || block->steps.k, || block->steps.u, || block->steps.v, || block->steps.w)) { if (NUM_AXIS_GANG(block->steps.x, || block->steps.y, || block->steps.z, || block->steps.i, || block->steps.j, || block->steps.k, || block->steps.u, || block->steps.v, || block->steps.w)) {
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); NOLESS(high, se);
} }
} }
@ -1936,7 +1940,7 @@ bool Planner::_populate_block(
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
bool ignore_e = false; bool ignore_e = false;
float collector[MIXING_STEPPERS]; 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) MIXER_STEPPER_LOOP(e)
if (e_steps * collector[e] > max_e_steps) { ignore_e = true; break; } if (e_steps * collector[e] > max_e_steps) { ignore_e = true; break; }
#else #else
@ -2193,7 +2197,7 @@ bool Planner::_populate_block(
#if SECONDARY_LINEAR_AXES >= 1 && NONE(FOAMCUTTER_XYUV, ARTICULATED_ROBOT_ARM) #if SECONDARY_LINEAR_AXES >= 1 && NONE(FOAMCUTTER_XYUV, ARTICULATED_ROBOT_ARM)
if (NEAR_ZERO(distance_sqr)) { if (NEAR_ZERO(distance_sqr)) {
// Move does not involve any primary linear axes (xyz) but might involve secondary linear axes // Move does not involve any primary linear axes (xyz) but might involve secondary linear axes
distance_sqr = (0.0 distance_sqr = (0.0f
SECONDARY_AXIS_GANG( SECONDARY_AXIS_GANG(
IF_DISABLED(AXIS4_ROTATES, + sq(steps_dist_mm.i)), IF_DISABLED(AXIS4_ROTATES, + sq(steps_dist_mm.i)),
IF_DISABLED(AXIS5_ROTATES, + sq(steps_dist_mm.j)), IF_DISABLED(AXIS5_ROTATES, + sq(steps_dist_mm.j)),
@ -2396,7 +2400,7 @@ bool Planner::_populate_block(
if (was_enabled) stepper.wake_up(); if (was_enabled) stepper.wake_up();
#endif #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 block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
#if ENABLED(FILAMENT_WIDTH_SENSOR) #if ENABLED(FILAMENT_WIDTH_SENSOR)
@ -2492,7 +2496,7 @@ bool Planner::_populate_block(
if (speed_factor < 1.0f) { if (speed_factor < 1.0f) {
current_speed *= speed_factor; current_speed *= speed_factor;
block->nominal_rate *= 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. // Compute and limit the acceleration rate for the trapezoid generator.
@ -2592,7 +2596,7 @@ bool Planner::_populate_block(
if (block->use_advance_lead) { 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)]); 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 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."); SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed.");
if (block->advance_speed < 200) if (block->advance_speed < 200)
SERIAL_ECHOLNPGM("eISR running at > 10kHz."); SERIAL_ECHOLNPGM("eISR running at > 10kHz.");
@ -2663,7 +2667,7 @@ bool Planner::_populate_block(
unit_vec *= inverse_millimeters; // Use pre-calculated (1 / SQRT(x^2 + y^2 + z^2)) 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. // 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) // 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. // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float junction_cos_theta = LOGICAL_AXIS_GANG( float junction_cos_theta = LOGICAL_AXIS_GANG(
@ -2792,7 +2796,7 @@ bool Planner::_populate_block(
} }
// Get the lowest speed // 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. else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
vmax_junction_sqr = 0; vmax_junction_sqr = 0;
@ -2801,27 +2805,17 @@ bool Planner::_populate_block(
#endif #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 #if HAS_CLASSIC_JERK
/** /**
* Adapted from Průša MKS firmware * Adapted from Průša MKS firmware
* https://github.com/prusa3d/Prusa-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 // Exit speed limited by a jerk to full halt of a previous last segment
static float previous_safe_speed; static float previous_safe_speed;
// Start with a safe speed (from which the machine may halt to stop immediately). // 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 #ifndef TRAVEL_EXTRA_XYJERK
#define TRAVEL_EXTRA_XYJERK 0 #define TRAVEL_EXTRA_XYJERK 0
@ -2834,7 +2828,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 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 (jerk > maxj) { // cs > mj : New current speed too fast?
if (limited) { // limited already? 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 if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; // ns*mj/cs
} }
else { else {
@ -2845,7 +2839,7 @@ bool Planner::_populate_block(
} }
float vmax_junction; 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. // 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, // 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. // then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
@ -2856,11 +2850,9 @@ bool Planner::_populate_block(
// The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum. // 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. // 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; float smaller_speed_factor = 1.0f;
if (nominal_speed < previous_nominal_speed) { if (block->nominal_speed < previous_nominal_speed) {
vmax_junction = nominal_speed; vmax_junction = block->nominal_speed;
smaller_speed_factor = vmax_junction / previous_nominal_speed; smaller_speed_factor = vmax_junction / previous_nominal_speed;
} }
else else
@ -2927,11 +2919,11 @@ bool Planner::_populate_block(
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both // 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 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. // 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 // Update previous path unit_vector and nominal speed
previous_speed = current_speed; previous_speed = current_speed;
previous_nominal_speed_sqr = block->nominal_speed_sqr; previous_nominal_speed = block->nominal_speed;
position = target; // Update the position position = target; // Update the position
@ -3268,7 +3260,7 @@ void Planner::set_machine_position_mm(const abce_pos_t &abce) {
); );
if (has_blocks_queued()) { 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(); //previous_speed.reset();
buffer_sync_block(BLOCK_BIT_SYNC_POSITION); buffer_sync_block(BLOCK_BIT_SYNC_POSITION);
} }
@ -3344,7 +3336,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) { 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 uint8_t lim_axis = TERN_(HAS_EXTRUDERS, axis > E_AXIS ? E_AXIS :) axis;
const float before = val; const float before = val;
LIMIT(val, 0.1, max_limit[lim_axis]); LIMIT(val, 0.1f, max_limit[lim_axis]);
if (before != val) { if (before != val) {
SERIAL_CHAR(AXIS_CHAR(lim_axis)); SERIAL_CHAR(AXIS_CHAR(lim_axis));
SERIAL_ECHOPGM(" Max "); SERIAL_ECHOPGM(" Max ");

26
Marlin/src/module/planner.h
View File

@ -199,7 +199,7 @@ typedef struct PlannerBlock {
volatile bool is_move() { return !(is_sync() || is_page()); } volatile bool is_move() { return !(is_sync() || is_page()); }
// Fields used by the motion planner to manage acceleration // 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 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 max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2
millimeters, // The total travel of this block in mm millimeters, // The total travel of this block in mm
@ -510,7 +510,7 @@ class Planner {
/** /**
* Nominal speed of previous path line segment (mm/s)^2 * 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 * Limit where 64bit math is necessary for acceleration calculation
@ -1009,28 +1009,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 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); } 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 * Calculate the maximum allowable speed squared at this point, in order
* to reach 'target_velocity_sqr' using 'acceleration' within a given * to reach 'target_velocity_sqr' using 'acceleration' within a given

Write Preview
Loading…
Cancel
Save