Skip to content

Commit

Permalink
🐛 Fix planner jerk limits (MarlinFirmware#26529)
Browse files Browse the repository at this point in the history 
Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
  • Loading branch information
@mh-dm @thinkyhead
mh-dm and thinkyhead authored Dec 14, 2023
1 parent 75da355 commit b901338
Show file tree
Hide file tree
Showing 4 changed files with 79 additions and 129 deletions.
5 changes: 1 addition & 4 deletions Marlin/src/gcode/config/M200-M205.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -297,14 +297,11 @@ void GcodeSuite::M205() {
if (parser.seenval('S')) planner.settings.min_feedrate_mm_s = parser.value_linear_units();
if (parser.seenval('T')) planner.settings.min_travel_feedrate_mm_s = parser.value_linear_units();
#if HAS_JUNCTION_DEVIATION
#if ENABLED(CLASSIC_JERK) && AXIS_COLLISION('J')
#error "Can't set_max_jerk for 'J' axis because 'J' is used for Junction Deviation."
#endif
if (parser.seenval('J')) {
const float junc_dev = parser.value_linear_units();
if (WITHIN(junc_dev, 0.01f, 0.3f)) {
planner.junction_deviation_mm = junc_dev;
TERN_(LIN_ADVANCE, planner.recalculate_max_e_jerk());
TERN_(HAS_LINEAR_E_JERK, planner.recalculate_max_e_jerk());
}
else
SERIAL_ERROR_MSG("?J out of range (0.01 to 0.3)");
Expand Down
7 changes: 4 additions & 3 deletions Marlin/src/gcode/motion/G4.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,8 @@ void GcodeSuite::G4() {
SERIAL_ECHOLNPGM(STR_Z_MOVE_COMP);
#endif

if (!ui.has_status()) LCD_MESSAGE(MSG_DWELL);

dwell(dwell_ms);
if (dwell_ms) {
if (!ui.has_status()) LCD_MESSAGE(MSG_DWELL);
dwell(dwell_ms);
}
}
192 changes: 73 additions & 119 deletions Marlin/src/module/planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,9 +153,7 @@ float Planner::mm_per_step[DISTINCT_AXES]; // (mm) Millimeters per step
#if HAS_LINEAR_E_JERK
float Planner::max_e_jerk[DISTINCT_E]; // Calculated from junction_deviation_mm
#endif
#endif

#if ENABLED(CLASSIC_JERK)
#else // CLASSIC_JERK
TERN(HAS_LINEAR_E_JERK, xyz_pos_t, xyze_pos_t) Planner::max_jerk;
#endif

Expand Down Expand Up @@ -2374,42 +2372,42 @@ bool Planner::_populate_block(

// Limit speed on extruders, if any
#if HAS_EXTRUDERS
{
current_speed.e = dist_mm.e * inverse_secs;
#if HAS_MIXER_SYNC_CHANNEL
// Move all mixing extruders at the specified rate
if (mixer.get_current_vtool() == MIXER_AUTORETRACT_TOOL)
current_speed.e *= MIXING_STEPPERS;
#endif

const feedRate_t cs = ABS(current_speed.e),
max_fr = settings.max_feedrate_mm_s[E_AXIS_N(extruder)]
* TERN(HAS_MIXER_SYNC_CHANNEL, MIXING_STEPPERS, 1);

if (cs > max_fr) NOMORE(speed_factor, max_fr / cs); //respect max feedrate on any movement (doesn't matter if E axes only or not)

#if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
const feedRate_t max_vfr = volumetric_extruder_feedrate_limit[extruder]
* TERN(HAS_MIXER_SYNC_CHANNEL, MIXING_STEPPERS, 1);
{
current_speed.e = dist_mm.e * inverse_secs;
#if HAS_MIXER_SYNC_CHANNEL
// Move all mixing extruders at the specified rate
if (mixer.get_current_vtool() == MIXER_AUTORETRACT_TOOL)
current_speed.e *= MIXING_STEPPERS;
#endif

// TODO: Doesn't work properly for joined segments. Set MIN_STEPS_PER_SEGMENT 1 as workaround.
const feedRate_t cs = ABS(current_speed.e),
max_fr = settings.max_feedrate_mm_s[E_AXIS_N(extruder)]
* TERN(HAS_MIXER_SYNC_CHANNEL, MIXING_STEPPERS, 1);

if (block->steps.a || block->steps.b || block->steps.c) {
if (cs > max_fr) NOMORE(speed_factor, max_fr / cs); //respect max feedrate on any movement (doesn't matter if E axes only or not)

if (max_vfr > 0 && cs > max_vfr) {
NOMORE(speed_factor, max_vfr / cs); // respect volumetric extruder limit (if any)
/* <-- add a slash to enable
SERIAL_ECHOPGM("volumetric extruder limit enforced: ", (cs * CIRCLE_AREA(filament_size[extruder] * 0.5f)));
SERIAL_ECHOPGM(" mm^3/s (", cs);
SERIAL_ECHOPGM(" mm/s) limited to ", (max_vfr * CIRCLE_AREA(filament_size[extruder] * 0.5f)));
SERIAL_ECHOPGM(" mm^3/s (", max_vfr);
SERIAL_ECHOLNPGM(" mm/s)");
//*/
}
#if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
const feedRate_t max_vfr = volumetric_extruder_feedrate_limit[extruder]
* TERN(HAS_MIXER_SYNC_CHANNEL, MIXING_STEPPERS, 1);

// TODO: Doesn't work properly for joined segments. Set MIN_STEPS_PER_SEGMENT 1 as workaround.

if (block->steps.a || block->steps.b || block->steps.c) {

if (max_vfr > 0 && cs > max_vfr) {
NOMORE(speed_factor, max_vfr / cs); // respect volumetric extruder limit (if any)
/* <-- add a slash to enable
SERIAL_ECHOPGM("volumetric extruder limit enforced: ", (cs * CIRCLE_AREA(filament_size[extruder] * 0.5f)));
SERIAL_ECHOPGM(" mm^3/s (", cs);
SERIAL_ECHOPGM(" mm/s) limited to ", (max_vfr * CIRCLE_AREA(filament_size[extruder] * 0.5f)));
SERIAL_ECHOPGM(" mm^3/s (", max_vfr);
SERIAL_ECHOLNPGM(" mm/s)");
//*/
}
#endif
}
#endif
}
#endif
}
#endif // HAS_EXTRUDERS

#ifdef XY_FREQUENCY_LIMIT

Expand Down Expand Up @@ -2492,7 +2490,7 @@ bool Planner::_populate_block(
*
* extruder_advance_K[extruder] : There is an advance factor set for this extruder.
*
* dist.e > 0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
* dist.e > 0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
*/
use_advance_lead = esteps && extruder_advance_K[E_INDEX_N(extruder)] && dist.e > 0;

Expand All @@ -2511,9 +2509,10 @@ bool Planner::_populate_block(
else {
// Scale E acceleration so that it will be possible to jump to the advance speed.
const uint32_t max_accel_steps_per_s2 = MAX_E_JERK(extruder) / (extruder_advance_K[E_INDEX_N(extruder)] * e_D_ratio) * steps_per_mm;
if (TERN0(LA_DEBUG, accel > max_accel_steps_per_s2))
SERIAL_ECHOLNPGM("Acceleration limited.");
NOMORE(accel, max_accel_steps_per_s2);
if (accel > max_accel_steps_per_s2) {
accel = max_accel_steps_per_s2;
if (ENABLED(LA_DEBUG)) SERIAL_ECHOLNPGM("Acceleration limited.");
}
}
}
#endif
Expand Down Expand Up @@ -2764,104 +2763,59 @@ bool Planner::_populate_block(

prev_unit_vec = unit_vec;

#endif

#if ENABLED(CLASSIC_JERK)
#else // CLASSIC_JERK

/**
* Adapted from Průša MKS firmware
* Heavily modified. Originally adapted from Průša firmware.
* https://github.com/prusa3d/Prusa-Firmware
*/
// 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 = block->nominal_speed;

#ifndef TRAVEL_EXTRA_XYJERK
#define TRAVEL_EXTRA_XYJERK 0
#define TRAVEL_EXTRA_XYJERK 0.0f
#endif
const float extra_xyjerk = TERN0(HAS_EXTRUDERS, dist.e <= 0) ? TRAVEL_EXTRA_XYJERK : 0;

uint8_t limited = 0;
TERN(HAS_LINEAR_E_JERK, LOOP_NUM_AXES, LOOP_LOGICAL_AXES)(i) {
const float jerk = ABS(current_speed[i]), // cs : Starting from zero, change in speed 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 (limited) { // limited already?
const float mjerk = block->nominal_speed * maxj; // ns*mj
if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; // ns*mj/cs
}
else {
safe_speed *= maxj / jerk; // Initial limit: ns*mj/cs
++limited; // Initially limited
}
}
}
const float extra_xyjerk = TERN0(HAS_EXTRUDERS, dist.e <= 0) ? TRAVEL_EXTRA_XYJERK : 0.0f;

float vmax_junction;
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.
if (!moves_queued || UNEAR_ZERO(previous_nominal_speed)) {
// Compute "safe" speed, limited by a jerk to/from full halt.

// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
float v_factor = 1;
limited = 0;
float v_factor = 1.0f;
LOOP_LOGICAL_AXES(i) {
const float jerk = ABS(current_speed[i]), // Starting from zero, change in speed for this axis
maxj = max_jerk[i] + (i == X_AXIS || i == Y_AXIS ? extra_xyjerk : 0.0f); // The max jerk setting for this axis
if (jerk * v_factor > maxj) v_factor = maxj / jerk;
}
vmax_junction_sqr = sq(block->nominal_speed * v_factor);
NOLESS(minimum_planner_speed_sqr, vmax_junction_sqr);
}
else {
// Compute the maximum velocity allowed at a joint of two successive segments.

// 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.
float smaller_speed_factor = 1.0f;
float vmax_junction, previous_speed_factor, current_speed_factor;
if (block->nominal_speed < previous_nominal_speed) {
vmax_junction = block->nominal_speed;
smaller_speed_factor = vmax_junction / previous_nominal_speed;
previous_speed_factor = vmax_junction / previous_nominal_speed;
current_speed_factor = 1.0f;
}
else
else {
vmax_junction = previous_nominal_speed;
previous_speed_factor = 1.0f;
current_speed_factor = vmax_junction / block->nominal_speed;
}

// Now limit the jerk in all axes.
TERN(HAS_LINEAR_E_JERK, LOOP_NUM_AXES, LOOP_LOGICAL_AXES)(axis) {
// Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
float v_exit = previous_speed[axis] * smaller_speed_factor,
v_entry = current_speed[axis];
if (limited) {
v_exit *= v_factor;
v_entry *= v_factor;
}

// Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
const float jerk = (v_exit > v_entry)
? // coasting axis reversal
( (v_entry > 0 || v_exit < 0) ? (v_exit - v_entry) : _MAX(v_exit, -v_entry) )
: // v_exit <= v_entry coasting axis reversal
( (v_entry < 0 || v_exit > 0) ? (v_entry - v_exit) : _MAX(-v_exit, v_entry) );

const float maxj = (max_jerk[axis] + (axis == X_AXIS || axis == Y_AXIS ? extra_xyjerk : 0.0f));

if (jerk > maxj) {
v_factor *= maxj / jerk;
++limited;
}
float v_factor = 1.0f;
LOOP_LOGICAL_AXES(i) {
// Scale per-axis velocities for the same vmax_junction.
const float v_exit = previous_speed[i] * previous_speed_factor,
v_entry = current_speed[i] * current_speed_factor;

// Jerk is the per-axis velocity difference.
const float jerk = ABS(v_exit - v_entry),
maxj = max_jerk[i] + (i == X_AXIS || i == Y_AXIS ? extra_xyjerk : 0.0f);
if (jerk * v_factor > maxj) v_factor = maxj / jerk;
}
if (limited) vmax_junction *= v_factor;
// Now the transition velocity is known, which maximizes the shared exit / entry velocity while
// respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
const float vmax_junction_threshold = vmax_junction * 0.99f;
if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold)
vmax_junction = safe_speed;
vmax_junction_sqr = sq(vmax_junction * v_factor);
}
else
vmax_junction = safe_speed;

previous_safe_speed = safe_speed;

NOLESS(minimum_planner_speed_sqr, sq(safe_speed));

#if HAS_JUNCTION_DEVIATION
NOMORE(vmax_junction_sqr, sq(vmax_junction)); // Throttle down to max speed
#else
vmax_junction_sqr = sq(vmax_junction); // Go up or down to the new speed
#endif

#endif // CLASSIC_JERK

Expand Down
4 changes: 1 addition & 3 deletions Marlin/src/module/planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -475,9 +475,7 @@ class Planner {
#if HAS_LINEAR_E_JERK
static float max_e_jerk[DISTINCT_E]; // Calculated from junction_deviation_mm
#endif
#endif

#if ENABLED(CLASSIC_JERK)
#else // CLASSIC_JERK
// (mm/s^2) M205 XYZ(E) - The largest speed change requiring no acceleration.
static TERN(HAS_LINEAR_E_JERK, xyz_pos_t, xyze_pos_t) max_jerk;
#endif
Expand Down

0 comments on commit b901338

Please sign in to comment.