From 6acd7df93022f222fe15b5456be355c4fa74dcee Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Tue, 12 May 2026 12:17:39 -1000 Subject: [PATCH 1/8] Add jerk-limited motion planning groundwork --- FluidNC/src/Machine/Axis.cpp | 1 + FluidNC/src/Machine/Axis.h | 1 + FluidNC/src/NutsBolts.cpp | 13 ++++++++++++ FluidNC/src/NutsBolts.h | 1 + FluidNC/src/Planner.cpp | 26 +++++++++++++++++++++--- FluidNC/src/Planner.h | 6 ++++-- FluidNC/src/Stepper.cpp | 39 ++++++++++++++++++++++++++++++++++-- 7 files changed, 80 insertions(+), 7 deletions(-) diff --git a/FluidNC/src/Machine/Axis.cpp b/FluidNC/src/Machine/Axis.cpp index b75c3f6a4f..3b62bba2c0 100644 --- a/FluidNC/src/Machine/Axis.cpp +++ b/FluidNC/src/Machine/Axis.cpp @@ -9,6 +9,7 @@ namespace Machine { handler.item("steps_per_mm", _stepsPerMm, 0.001, 100000.0); handler.item("max_rate_mm_per_min", _maxRate, 0.001, 250000.0); handler.item("acceleration_mm_per_sec2", _acceleration, 0.001, 100000.0); + handler.item("jerk_mm_per_sec3", _jerk, 0.001, 1000000.0); handler.item("max_travel_mm", _maxTravel, 0.1, 10000000.0); handler.item("soft_limits", _softLimits); handler.item("idle_disable", _idleDisable); diff --git a/FluidNC/src/Machine/Axis.h b/FluidNC/src/Machine/Axis.h index 16f474acc3..66b18f297e 100644 --- a/FluidNC/src/Machine/Axis.h +++ b/FluidNC/src/Machine/Axis.h @@ -33,6 +33,7 @@ namespace Machine { float _stepsPerMm = 80.0f; float _maxRate = 1000.0f; float _acceleration = 25.0f; + float _jerk = 250.0f; float _maxTravel = 1000.0f; bool _softLimits = false; bool _idleDisable = true; diff --git a/FluidNC/src/NutsBolts.cpp b/FluidNC/src/NutsBolts.cpp index be2c2fa04d..b44c81f962 100644 --- a/FluidNC/src/NutsBolts.cpp +++ b/FluidNC/src/NutsBolts.cpp @@ -78,6 +78,7 @@ float convert_delta_vector_to_unit_vector(float* v) { } const float secPerMinSq = 60.0 * 60.0; // Seconds Per Minute Squared, for acceleration conversion +const float secPerMinCu = 60.0 * 60.0 * 60.0; // Seconds Per Minute Cubed, for jerk conversion float limit_acceleration_by_axis_maximum(float* unit_vec) { float limit_value = SOME_LARGE_VALUE; @@ -95,6 +96,18 @@ float limit_acceleration_by_axis_maximum(float* unit_vec) { return limit_value * secPerMinSq; } +float limit_jerk_by_axis_maximum(float* unit_vec) { + float limit_value = SOME_LARGE_VALUE; + auto n_axis = Axes::_numberAxis; + for (axis_t axis = X_AXIS; axis < n_axis; axis++) { + auto axisSetting = Axes::_axis[axis]; + if (unit_vec[axis] != 0) { // Avoid divide by zero. + limit_value = MIN(limit_value, fabsf(axisSetting->_jerk / unit_vec[axis])); + } + } + return limit_value * secPerMinCu; +} + float limit_rate_by_axis_maximum(float* unit_vec) { float limit_value = SOME_LARGE_VALUE; auto n_axis = Axes::_numberAxis; diff --git a/FluidNC/src/NutsBolts.h b/FluidNC/src/NutsBolts.h index fb324ab25f..c6fb81bad5 100644 --- a/FluidNC/src/NutsBolts.h +++ b/FluidNC/src/NutsBolts.h @@ -75,6 +75,7 @@ void scale_vector(float* v, float scale, size_t n); float convert_delta_vector_to_unit_vector(float* vector); float limit_acceleration_by_axis_maximum(float* unit_vec); +float limit_jerk_by_axis_maximum(float* unit_vec); float limit_rate_by_axis_maximum(float* unit_vec); const char* to_hex(uint32_t n); diff --git a/FluidNC/src/Planner.cpp b/FluidNC/src/Planner.cpp index eb0b1b8195..7f79ad1d5e 100644 --- a/FluidNC/src/Planner.cpp +++ b/FluidNC/src/Planner.cpp @@ -348,9 +348,11 @@ bool plan_buffer_line(float* target, plan_line_data_t* pl_data) { // down such that no individual axes maximum values are exceeded with respect to the line direction. // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. - block->millimeters = convert_delta_vector_to_unit_vector(unit_vec); - block->acceleration = limit_acceleration_by_axis_maximum(unit_vec); - block->rapid_rate = limit_rate_by_axis_maximum(unit_vec); + block->millimeters = convert_delta_vector_to_unit_vector(unit_vec); + block->max_acceleration = limit_acceleration_by_axis_maximum(unit_vec); + block->acceleration = block->max_acceleration; + block->jerk = (block->is_jog || block->motion.systemMotion) ? 0.0f : limit_jerk_by_axis_maximum(unit_vec); + block->rapid_rate = limit_rate_by_axis_maximum(unit_vec); // Store programmed rate. if (block->motion.rapidMotion) { block->programmed_rate = block->rapid_rate; @@ -360,6 +362,24 @@ bool plan_buffer_line(float* target, plan_line_data_t* pl_data) { block->programmed_rate *= block->millimeters; } } + + if (block->jerk > 0.0f) { + // Use an effective acceleration for lookahead while keeping the stepper-side + // segment generator free to apply a jerk-limited ramp shape later. + float time_to_max_accel = block->max_acceleration / block->jerk; + float speed_after_jerk_ramp = 0.5f * block->jerk * time_to_max_accel * time_to_max_accel; + float half_programmed_rate = 0.5f * block->programmed_rate; + + if (half_programmed_rate > speed_after_jerk_ramp) { + block->acceleration = block->programmed_rate + / (2.0f + * (time_to_max_accel + + (half_programmed_rate - speed_after_jerk_ramp) / block->max_acceleration)); + } else { + block->acceleration = block->programmed_rate / (2.0f * sqrtf(block->programmed_rate / block->jerk)); + } + } + // TODO: Need to check this method handling zero junction speeds when starting from rest. if ((block_buffer_head == block_buffer_tail) || (block->motion.systemMotion)) { // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. diff --git a/FluidNC/src/Planner.h b/FluidNC/src/Planner.h index defead28d8..7f98c10b81 100644 --- a/FluidNC/src/Planner.h +++ b/FluidNC/src/Planner.h @@ -44,8 +44,10 @@ struct plan_block_t { float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 float max_entry_speed_sqr; // Maximum allowable entry speed based on the minimum of junction limit and // neighboring nominal speeds with overrides in (mm/min)^2 - float acceleration; // Axis-limit adjusted line acceleration in (mm/min^2). Does not change. - float millimeters; // The remaining distance for this block to be executed in (mm). + float acceleration; // Effective line acceleration used by the planner in (mm/min^2). + float max_acceleration; // Axis-limit adjusted line acceleration in (mm/min^2). Does not change. + float jerk; // Axis-limit adjusted line jerk in (mm/min^3). Does not change. + float millimeters; // The remaining distance for this block to be executed in (mm). // NOTE: This value may be altered by stepper algorithm during execution. // Stored rate limiting data used by planner when changes occur. diff --git a/FluidNC/src/Stepper.cpp b/FluidNC/src/Stepper.cpp index 6d62c6ab09..1c08be793e 100644 --- a/FluidNC/src/Stepper.cpp +++ b/FluidNC/src/Stepper.cpp @@ -93,6 +93,7 @@ typedef struct { uint8_t st_block_index; // Index of stepper common data block being prepped PrepFlag recalculate_flag; + float current_acceleration; float dt_remainder; float steps_remaining; float step_per_mm; @@ -421,6 +422,7 @@ void Stepper::prep_buffer() { prep.steps_remaining = (float)pl_block->step_event_count; prep.step_per_mm = prep.steps_remaining / pl_block->millimeters; prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.step_per_mm; + prep.current_acceleration = 0.0f; prep.dt_remainder = 0.0; // Reset for new segment block if ((sys.step_control.executeHold) || prep.recalculate_flag.decelOverride) { // New block loaded mid-hold. Override planner block entry speed to enforce deceleration. @@ -577,10 +579,26 @@ void Stepper::prep_buffer() { } else { // Mid-deceleration override ramp. prep.current_speed -= speed_var; } + prep.current_acceleration = 0.0f; break; case RAMP_ACCEL: // NOTE: Acceleration ramp only computes during first do-while loop. - speed_var = pl_block->acceleration * time_var; + if (pl_block->jerk > 0.0f) { + float accel_var = pl_block->jerk * time_var; + float time_to_jerk = prep.current_acceleration / pl_block->jerk; + float jerk_rampdown = time_to_jerk + * (prep.current_speed + (0.5f * prep.current_acceleration * time_to_jerk) + + (pl_block->jerk * time_to_jerk * time_to_jerk * (1.0f / 6.0f))); + + if ((mm_remaining - prep.accelerate_until) > jerk_rampdown) { + prep.current_acceleration = MIN(prep.current_acceleration + accel_var, pl_block->max_acceleration); + } else { + prep.current_acceleration = MAX(prep.current_acceleration - accel_var, accel_var); + } + speed_var = prep.current_acceleration * time_var; + } else { + speed_var = pl_block->acceleration * time_var; + } mm_remaining -= time_var * (prep.current_speed + 0.5f * speed_var); if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. @@ -592,6 +610,7 @@ void Stepper::prep_buffer() { prep.ramp_type = RAMP_CRUISE; } prep.current_speed = prep.maximum_speed; + prep.current_acceleration = 0.0f; } else { // Acceleration only. prep.current_speed += speed_var; } @@ -612,7 +631,23 @@ void Stepper::prep_buffer() { break; default: // case RAMP_DECEL: // NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed. - speed_var = pl_block->acceleration * time_var; // Used as delta speed (mm/min) + if (pl_block->jerk > 0.0f) { + float accel_var = pl_block->jerk * time_var; + float time_to_jerk = prep.current_acceleration > 0.0f ? (prep.current_acceleration / pl_block->jerk) : time_var; + float jerk_rampdown = prep.exit_speed + + time_to_jerk + * (prep.current_acceleration + - (0.5f * pl_block->jerk * time_to_jerk)); + + if (prep.current_speed > jerk_rampdown) { + prep.current_acceleration = MIN(prep.current_acceleration + accel_var, pl_block->max_acceleration); + } else { + prep.current_acceleration = MAX(prep.current_acceleration - accel_var, accel_var); + } + speed_var = prep.current_acceleration * time_var; + } else { + speed_var = pl_block->acceleration * time_var; // Used as delta speed (mm/min) + } if (prep.current_speed > speed_var) { // Check if at or below zero speed. // Compute distance from end of segment to end of block. mm_var = mm_remaining - time_var * (prep.current_speed - 0.5f * speed_var); // (mm) From ea3d843709ae2eafaf71bd0e85bbd3fb22d3e058 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Wed, 13 May 2026 10:04:44 -1000 Subject: [PATCH 2/8] Preserve jerk state across parking restore --- FluidNC/src/Stepper.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/FluidNC/src/Stepper.cpp b/FluidNC/src/Stepper.cpp index 1c08be793e..3bfd6c497b 100644 --- a/FluidNC/src/Stepper.cpp +++ b/FluidNC/src/Stepper.cpp @@ -103,6 +103,7 @@ typedef struct { float last_steps_remaining; float last_step_per_mm; float last_dt_remainder; + float last_current_acceleration; uint8_t ramp_type; // Current segment ramp state float mm_complete; // End of velocity profile from end of current planner block in (mm). @@ -328,6 +329,7 @@ void Stepper::parking_setup_buffer() { prep.last_steps_remaining = prep.steps_remaining; prep.last_dt_remainder = prep.dt_remainder; prep.last_step_per_mm = prep.step_per_mm; + prep.last_current_acceleration = prep.current_acceleration; } // Set flags to execute a parking motion prep.recalculate_flag.parking = 1; @@ -344,6 +346,7 @@ void Stepper::parking_restore_buffer() { prep.steps_remaining = prep.last_steps_remaining; prep.dt_remainder = prep.last_dt_remainder; prep.step_per_mm = prep.last_step_per_mm; + prep.current_acceleration = prep.last_current_acceleration; prep.recalculate_flag.holdPartialBlock = 1; prep.recalculate_flag.recalculate = 1; prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.step_per_mm; // Recompute this value. From c08f67cfb74930b87e4015b789f361f5d5de57d0 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Wed, 13 May 2026 10:07:49 -1000 Subject: [PATCH 3/8] Propagate jerk limit across delta towers --- FluidNC/src/Kinematics/ParallelDelta.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/FluidNC/src/Kinematics/ParallelDelta.cpp b/FluidNC/src/Kinematics/ParallelDelta.cpp index 2ac2a893ac..097df0191d 100644 --- a/FluidNC/src/Kinematics/ParallelDelta.cpp +++ b/FluidNC/src/Kinematics/ParallelDelta.cpp @@ -73,6 +73,7 @@ namespace Kinematics { auto steps0 = axis0->_stepsPerMm; auto accel0 = axis0->_acceleration; auto rate0 = axis0->_maxRate; + auto jerk0 = axis0->_jerk; for (axis_t axis = X_AXIS; axis < A_AXIS; axis++) { auto axisp = axes->_axis[axis]; @@ -81,6 +82,7 @@ namespace Kinematics { axisp->_stepsPerMm = steps0; axisp->_maxRate = rate0; axisp->_acceleration = accel0; + axisp->_jerk = jerk0; } init_position(); From a6c043b91d94117fea76df9be2b4cf19cdf0521e Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Wed, 13 May 2026 10:11:14 -1000 Subject: [PATCH 4/8] Recompute jerk acceleration after overrides --- FluidNC/src/Planner.cpp | 38 +++++++++++++++++++++----------------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/FluidNC/src/Planner.cpp b/FluidNC/src/Planner.cpp index 7f79ad1d5e..cf7655c444 100644 --- a/FluidNC/src/Planner.cpp +++ b/FluidNC/src/Planner.cpp @@ -244,6 +244,23 @@ uint8_t plan_check_full_buffer() { return block_buffer_tail == next_buffer_head; } +static float plan_compute_effective_acceleration(plan_block_t* block, float target_speed) { + if (block->jerk <= 0.0f) { + return block->max_acceleration; + } + + float time_to_max_accel = block->max_acceleration / block->jerk; + float speed_after_jerk_ramp = 0.5f * block->jerk * time_to_max_accel * time_to_max_accel; + float half_target_speed = 0.5f * target_speed; + + if (half_target_speed > speed_after_jerk_ramp) { + return target_speed + / (2.0f * (time_to_max_accel + (half_target_speed - speed_after_jerk_ramp) / block->max_acceleration)); + } + + return target_speed / (2.0f * sqrtf(target_speed / block->jerk)); +} + // Computes and returns block nominal speed based on running condition and override values. // NOTE: All system motion commands, such as homing/parking, are not subject to overrides. float plan_compute_profile_nominal_speed(plan_block_t* block) { @@ -288,6 +305,7 @@ void plan_update_velocity_profile_parameters() { while (block_index != block_buffer_head) { block = &block_buffer[block_index]; nominal_speed = plan_compute_profile_nominal_speed(block); + block->acceleration = plan_compute_effective_acceleration(block, nominal_speed); plan_compute_profile_parameters(block, nominal_speed, prev_nominal_speed); prev_nominal_speed = nominal_speed; block_index = plan_next_block_index(block_index); @@ -350,7 +368,6 @@ bool plan_buffer_line(float* target, plan_line_data_t* pl_data) { // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. block->millimeters = convert_delta_vector_to_unit_vector(unit_vec); block->max_acceleration = limit_acceleration_by_axis_maximum(unit_vec); - block->acceleration = block->max_acceleration; block->jerk = (block->is_jog || block->motion.systemMotion) ? 0.0f : limit_jerk_by_axis_maximum(unit_vec); block->rapid_rate = limit_rate_by_axis_maximum(unit_vec); // Store programmed rate. @@ -363,22 +380,9 @@ bool plan_buffer_line(float* target, plan_line_data_t* pl_data) { } } - if (block->jerk > 0.0f) { - // Use an effective acceleration for lookahead while keeping the stepper-side - // segment generator free to apply a jerk-limited ramp shape later. - float time_to_max_accel = block->max_acceleration / block->jerk; - float speed_after_jerk_ramp = 0.5f * block->jerk * time_to_max_accel * time_to_max_accel; - float half_programmed_rate = 0.5f * block->programmed_rate; - - if (half_programmed_rate > speed_after_jerk_ramp) { - block->acceleration = block->programmed_rate - / (2.0f - * (time_to_max_accel - + (half_programmed_rate - speed_after_jerk_ramp) / block->max_acceleration)); - } else { - block->acceleration = block->programmed_rate / (2.0f * sqrtf(block->programmed_rate / block->jerk)); - } - } + // Use an effective acceleration for lookahead while keeping the stepper-side + // segment generator free to apply a jerk-limited ramp shape later. + block->acceleration = plan_compute_effective_acceleration(block, plan_compute_profile_nominal_speed(block)); // TODO: Need to check this method handling zero junction speeds when starting from rest. if ((block_buffer_head == block_buffer_tail) || (block->motion.systemMotion)) { From 4578a1aac4e8d8f4ef9495f26322bd223fd90b97 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Wed, 13 May 2026 10:14:36 -1000 Subject: [PATCH 5/8] Use average acceleration in jerk ramp integration --- FluidNC/src/Stepper.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/FluidNC/src/Stepper.cpp b/FluidNC/src/Stepper.cpp index 3bfd6c497b..a751b9b134 100644 --- a/FluidNC/src/Stepper.cpp +++ b/FluidNC/src/Stepper.cpp @@ -587,6 +587,7 @@ void Stepper::prep_buffer() { case RAMP_ACCEL: // NOTE: Acceleration ramp only computes during first do-while loop. if (pl_block->jerk > 0.0f) { + float previous_acceleration = prep.current_acceleration; float accel_var = pl_block->jerk * time_var; float time_to_jerk = prep.current_acceleration / pl_block->jerk; float jerk_rampdown = time_to_jerk @@ -598,7 +599,7 @@ void Stepper::prep_buffer() { } else { prep.current_acceleration = MAX(prep.current_acceleration - accel_var, accel_var); } - speed_var = prep.current_acceleration * time_var; + speed_var = 0.5f * (previous_acceleration + prep.current_acceleration) * time_var; } else { speed_var = pl_block->acceleration * time_var; } @@ -635,6 +636,7 @@ void Stepper::prep_buffer() { default: // case RAMP_DECEL: // NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed. if (pl_block->jerk > 0.0f) { + float previous_acceleration = prep.current_acceleration; float accel_var = pl_block->jerk * time_var; float time_to_jerk = prep.current_acceleration > 0.0f ? (prep.current_acceleration / pl_block->jerk) : time_var; float jerk_rampdown = prep.exit_speed @@ -647,7 +649,7 @@ void Stepper::prep_buffer() { } else { prep.current_acceleration = MAX(prep.current_acceleration - accel_var, accel_var); } - speed_var = prep.current_acceleration * time_var; + speed_var = 0.5f * (previous_acceleration + prep.current_acceleration) * time_var; } else { speed_var = pl_block->acceleration * time_var; // Used as delta speed (mm/min) } From ff03040405994de2386714f7bee92421ef8802a8 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Wed, 13 May 2026 10:46:03 -1000 Subject: [PATCH 6/8] Default jerk config to trapezoidal compatibility mode --- FluidNC/src/Machine/Axis.cpp | 2 +- FluidNC/src/Machine/Axis.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/FluidNC/src/Machine/Axis.cpp b/FluidNC/src/Machine/Axis.cpp index 3b62bba2c0..5aeefc8fec 100644 --- a/FluidNC/src/Machine/Axis.cpp +++ b/FluidNC/src/Machine/Axis.cpp @@ -9,7 +9,7 @@ namespace Machine { handler.item("steps_per_mm", _stepsPerMm, 0.001, 100000.0); handler.item("max_rate_mm_per_min", _maxRate, 0.001, 250000.0); handler.item("acceleration_mm_per_sec2", _acceleration, 0.001, 100000.0); - handler.item("jerk_mm_per_sec3", _jerk, 0.001, 1000000.0); + handler.item("jerk_mm_per_sec3", _jerk, 0.0, 1000000.0); handler.item("max_travel_mm", _maxTravel, 0.1, 10000000.0); handler.item("soft_limits", _softLimits); handler.item("idle_disable", _idleDisable); diff --git a/FluidNC/src/Machine/Axis.h b/FluidNC/src/Machine/Axis.h index 66b18f297e..2a2477220b 100644 --- a/FluidNC/src/Machine/Axis.h +++ b/FluidNC/src/Machine/Axis.h @@ -33,7 +33,7 @@ namespace Machine { float _stepsPerMm = 80.0f; float _maxRate = 1000.0f; float _acceleration = 25.0f; - float _jerk = 250.0f; + float _jerk = 0.0f; float _maxTravel = 1000.0f; bool _softLimits = false; bool _idleDisable = true; From 336937fb37c22041735857172ef114bf5d529cac Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 14 May 2026 12:57:15 -1000 Subject: [PATCH 7/8] Fixed a last-segment jerk problem that caused artificial pauses --- FluidNC/src/Stepper.cpp | 36 ++++++++++++++++++++++++++++++------ 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/FluidNC/src/Stepper.cpp b/FluidNC/src/Stepper.cpp index a751b9b134..213e1a25de 100644 --- a/FluidNC/src/Stepper.cpp +++ b/FluidNC/src/Stepper.cpp @@ -422,10 +422,14 @@ void Stepper::prep_buffer() { st_prep_block->step_event_count = pl_block->step_event_count << maxAmassLevel; // Initialize segment buffer data for generating the segments. + bool carry_forced_decel_accel = (pl_block->jerk > 0.0f) && (sys.step_control.executeHold || prep.recalculate_flag.decelOverride) + && ((prep.ramp_type == RAMP_DECEL) || (prep.ramp_type == RAMP_DECEL_OVERRIDE)); prep.steps_remaining = (float)pl_block->step_event_count; prep.step_per_mm = prep.steps_remaining / pl_block->millimeters; prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.step_per_mm; - prep.current_acceleration = 0.0f; + if (!carry_forced_decel_accel) { + prep.current_acceleration = 0.0f; + } prep.dt_remainder = 0.0; // Reset for new segment block if ((sys.step_control.executeHold) || prep.recalculate_flag.decelOverride) { // New block loaded mid-hold. Override planner block entry speed to enforce deceleration. @@ -570,8 +574,25 @@ void Stepper::prep_buffer() { do { switch (prep.ramp_type) { case RAMP_DECEL_OVERRIDE: - speed_var = pl_block->acceleration * time_var; - mm_var = time_var * (prep.current_speed - 0.5f * speed_var); + if (pl_block->jerk > 0.0f) { + float previous_acceleration = prep.current_acceleration; + float accel_var = pl_block->jerk * time_var; + float time_to_jerk = prep.current_acceleration > 0.0f ? (prep.current_acceleration / pl_block->jerk) : time_var; + float jerk_rampdown = prep.maximum_speed + + time_to_jerk + * (prep.current_acceleration + - (0.5f * pl_block->jerk * time_to_jerk)); + + if (prep.current_speed > jerk_rampdown) { + prep.current_acceleration = MIN(prep.current_acceleration + accel_var, pl_block->max_acceleration); + } else { + prep.current_acceleration = MAX(prep.current_acceleration - accel_var, accel_var); + } + speed_var = 0.5f * (previous_acceleration + prep.current_acceleration) * time_var; + } else { + speed_var = pl_block->acceleration * time_var; + } + mm_var = time_var * (prep.current_speed - 0.5f * speed_var); mm_remaining -= mm_var; if ((mm_remaining < prep.accelerate_until) || (mm_var <= 0)) { // Cruise or cruise-deceleration types only for deceleration override. @@ -579,10 +600,10 @@ void Stepper::prep_buffer() { time_var = 2.0f * (pl_block->millimeters - mm_remaining) / (prep.current_speed + prep.maximum_speed); prep.ramp_type = RAMP_CRUISE; prep.current_speed = prep.maximum_speed; + prep.current_acceleration = 0.0f; } else { // Mid-deceleration override ramp. prep.current_speed -= speed_var; } - prep.current_acceleration = 0.0f; break; case RAMP_ACCEL: // NOTE: Acceleration ramp only computes during first do-while loop. @@ -662,8 +683,11 @@ void Stepper::prep_buffer() { break; // Segment complete. Exit switch-case statement. Continue do-while loop. } } - // Otherwise, at end of block or end of forced-deceleration. - time_var = 2.0f * (mm_remaining - prep.mm_complete) / (prep.current_speed + prep.exit_speed); + // Otherwise, at end of block or end of forced-deceleration. The completion + // time must fit inside the current segment budget; if jerk shaping leaves + // a numerically inconsistent near-zero-speed remainder, avoid stretching + // the segment into an artificial pause. + time_var = MIN(2.0f * (mm_remaining - prep.mm_complete) / (prep.current_speed + prep.exit_speed), time_var); mm_remaining = prep.mm_complete; prep.current_speed = prep.exit_speed; } From 6393fb4a9aaed12efc6616a9fd17dc3948b2e8d2 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 14 May 2026 13:28:07 -1000 Subject: [PATCH 8/8] Changed jerk_mm_per_sec3 to dimensionless jerk_factor --- FluidNC/src/Kinematics/ParallelDelta.cpp | 4 +- FluidNC/src/Machine/Axis.cpp | 2 +- FluidNC/src/Machine/Axis.h | 2 +- FluidNC/src/NutsBolts.cpp | 43 -------------- FluidNC/src/NutsBolts.h | 3 - FluidNC/src/Planner.cpp | 71 ++++++++++++++++++++++-- FluidNC/src/Protocol.cpp | 1 + 7 files changed, 72 insertions(+), 54 deletions(-) diff --git a/FluidNC/src/Kinematics/ParallelDelta.cpp b/FluidNC/src/Kinematics/ParallelDelta.cpp index 097df0191d..e2b6d6b8fc 100644 --- a/FluidNC/src/Kinematics/ParallelDelta.cpp +++ b/FluidNC/src/Kinematics/ParallelDelta.cpp @@ -73,7 +73,7 @@ namespace Kinematics { auto steps0 = axis0->_stepsPerMm; auto accel0 = axis0->_acceleration; auto rate0 = axis0->_maxRate; - auto jerk0 = axis0->_jerk; + auto jerkFactor0 = axis0->_jerkFactor; for (axis_t axis = X_AXIS; axis < A_AXIS; axis++) { auto axisp = axes->_axis[axis]; @@ -82,7 +82,7 @@ namespace Kinematics { axisp->_stepsPerMm = steps0; axisp->_maxRate = rate0; axisp->_acceleration = accel0; - axisp->_jerk = jerk0; + axisp->_jerkFactor = jerkFactor0; } init_position(); diff --git a/FluidNC/src/Machine/Axis.cpp b/FluidNC/src/Machine/Axis.cpp index 5aeefc8fec..775bde47ea 100644 --- a/FluidNC/src/Machine/Axis.cpp +++ b/FluidNC/src/Machine/Axis.cpp @@ -9,7 +9,7 @@ namespace Machine { handler.item("steps_per_mm", _stepsPerMm, 0.001, 100000.0); handler.item("max_rate_mm_per_min", _maxRate, 0.001, 250000.0); handler.item("acceleration_mm_per_sec2", _acceleration, 0.001, 100000.0); - handler.item("jerk_mm_per_sec3", _jerk, 0.0, 1000000.0); + handler.item("jerk_factor", _jerkFactor, 0.0, 1000.0); handler.item("max_travel_mm", _maxTravel, 0.1, 10000000.0); handler.item("soft_limits", _softLimits); handler.item("idle_disable", _idleDisable); diff --git a/FluidNC/src/Machine/Axis.h b/FluidNC/src/Machine/Axis.h index 2a2477220b..9c46a9da54 100644 --- a/FluidNC/src/Machine/Axis.h +++ b/FluidNC/src/Machine/Axis.h @@ -33,7 +33,7 @@ namespace Machine { float _stepsPerMm = 80.0f; float _maxRate = 1000.0f; float _acceleration = 25.0f; - float _jerk = 0.0f; + float _jerkFactor = 0.0f; float _maxTravel = 1000.0f; bool _softLimits = false; bool _idleDisable = true; diff --git a/FluidNC/src/NutsBolts.cpp b/FluidNC/src/NutsBolts.cpp index b44c81f962..b40cee8006 100644 --- a/FluidNC/src/NutsBolts.cpp +++ b/FluidNC/src/NutsBolts.cpp @@ -77,49 +77,6 @@ float convert_delta_vector_to_unit_vector(float* v) { return magnitude; } -const float secPerMinSq = 60.0 * 60.0; // Seconds Per Minute Squared, for acceleration conversion -const float secPerMinCu = 60.0 * 60.0 * 60.0; // Seconds Per Minute Cubed, for jerk conversion - -float limit_acceleration_by_axis_maximum(float* unit_vec) { - float limit_value = SOME_LARGE_VALUE; - auto n_axis = Axes::_numberAxis; - for (axis_t axis = X_AXIS; axis < n_axis; axis++) { - auto axisSetting = Axes::_axis[axis]; - if (unit_vec[axis] != 0) { // Avoid divide by zero. - limit_value = MIN(limit_value, fabsf(axisSetting->_acceleration / unit_vec[axis])); - } - } - // The acceleration setting is stored and displayed in units of mm/sec^2, - // but used in units of mm/min^2. It suffices to perform the conversion once on - // exit, since the limit computation above is independent of units - it simply - // finds the smallest value. - return limit_value * secPerMinSq; -} - -float limit_jerk_by_axis_maximum(float* unit_vec) { - float limit_value = SOME_LARGE_VALUE; - auto n_axis = Axes::_numberAxis; - for (axis_t axis = X_AXIS; axis < n_axis; axis++) { - auto axisSetting = Axes::_axis[axis]; - if (unit_vec[axis] != 0) { // Avoid divide by zero. - limit_value = MIN(limit_value, fabsf(axisSetting->_jerk / unit_vec[axis])); - } - } - return limit_value * secPerMinCu; -} - -float limit_rate_by_axis_maximum(float* unit_vec) { - float limit_value = SOME_LARGE_VALUE; - auto n_axis = Axes::_numberAxis; - for (axis_t axis = X_AXIS; axis < n_axis; axis++) { - auto axisSetting = Axes::_axis[axis]; - if (unit_vec[axis] != 0) { // Avoid divide by zero. - limit_value = MIN(limit_value, fabsf(axisSetting->_maxRate / unit_vec[axis])); - } - } - return limit_value; -} - bool char_is_numeric(char value) { return value >= '0' && value <= '9'; } diff --git a/FluidNC/src/NutsBolts.h b/FluidNC/src/NutsBolts.h index c6fb81bad5..71c08be26f 100644 --- a/FluidNC/src/NutsBolts.h +++ b/FluidNC/src/NutsBolts.h @@ -74,9 +74,6 @@ float vector_length(float* v, size_t n); void scale_vector(float* v, float scale, size_t n); float convert_delta_vector_to_unit_vector(float* vector); -float limit_acceleration_by_axis_maximum(float* unit_vec); -float limit_jerk_by_axis_maximum(float* unit_vec); -float limit_rate_by_axis_maximum(float* unit_vec); const char* to_hex(uint32_t n); diff --git a/FluidNC/src/Planner.cpp b/FluidNC/src/Planner.cpp index cf7655c444..e513d71649 100644 --- a/FluidNC/src/Planner.cpp +++ b/FluidNC/src/Planner.cpp @@ -20,6 +20,52 @@ static uint8_t block_buffer_head; // Index of the next block to be p static uint8_t next_buffer_head; // Index of the next buffer head static uint8_t block_buffer_planned; // Index of the optimally planned block +namespace { +const float secPerMinSq = 60.0 * 60.0; // Seconds Per Minute Squared, for acceleration conversion +const float secPerMinCu = 60.0 * 60.0 * 60.0; // Seconds Per Minute Cubed, for jerk conversion + +float limit_acceleration_by_axis_maximum(float* unit_vec) { + float limit_value = SOME_LARGE_VALUE; + auto n_axis = Axes::_numberAxis; + for (axis_t axis = X_AXIS; axis < n_axis; axis++) { + auto axisSetting = Axes::_axis[axis]; + if (unit_vec[axis] != 0) { // Avoid divide by zero. + limit_value = MIN(limit_value, fabsf(axisSetting->_acceleration / unit_vec[axis])); + } + } + return limit_value * secPerMinSq; +} + +float limit_jerk_by_axis_maximum(float* unit_vec) { + float limit_value = SOME_LARGE_VALUE; + auto n_axis = Axes::_numberAxis; + for (axis_t axis = X_AXIS; axis < n_axis; axis++) { + auto axisSetting = Axes::_axis[axis]; + if (unit_vec[axis] != 0) { // Avoid divide by zero. + float axisJerk = 0.0f; + if (axisSetting->_jerkFactor > 0.0f) { + axisJerk = 60.0f * axisSetting->_jerkFactor * axisSetting->_acceleration * axisSetting->_acceleration + / axisSetting->_maxRate; + } + limit_value = MIN(limit_value, fabsf(axisJerk / unit_vec[axis])); + } + } + return limit_value * secPerMinCu; +} + +float limit_rate_by_axis_maximum(float* unit_vec) { + float limit_value = SOME_LARGE_VALUE; + auto n_axis = Axes::_numberAxis; + for (axis_t axis = X_AXIS; axis < n_axis; axis++) { + auto axisSetting = Axes::_axis[axis]; + if (unit_vec[axis] != 0) { // Avoid divide by zero. + limit_value = MIN(limit_value, fabsf(axisSetting->_maxRate / unit_vec[axis])); + } + } + return limit_value; +} +} // namespace + void plan_init() { if (block_buffer) { delete[] block_buffer; @@ -261,6 +307,24 @@ static float plan_compute_effective_acceleration(plan_block_t* block, float targ return target_speed / (2.0f * sqrtf(target_speed / block->jerk)); } +static void plan_refresh_block_directional_limits(plan_block_t* block) { + float unit_vec[MAX_N_AXIS]; + auto n_axis = Axes::_numberAxis; + + for (axis_t axis = X_AXIS; axis < n_axis; axis++) { + steps_t signed_steps = block->steps[axis]; + if (block->direction_bits & bitnum_to_mask(axis)) { + signed_steps = -signed_steps; + } + unit_vec[axis] = steps_to_motor_pos(signed_steps, axis); + } + + convert_delta_vector_to_unit_vector(unit_vec); + block->max_acceleration = limit_acceleration_by_axis_maximum(unit_vec); + block->jerk = (block->is_jog || block->motion.systemMotion) ? 0.0f : limit_jerk_by_axis_maximum(unit_vec); + block->rapid_rate = limit_rate_by_axis_maximum(unit_vec); +} + // Computes and returns block nominal speed based on running condition and override values. // NOTE: All system motion commands, such as homing/parking, are not subject to overrides. float plan_compute_profile_nominal_speed(plan_block_t* block) { @@ -304,6 +368,7 @@ void plan_update_velocity_profile_parameters() { float prev_nominal_speed = SOME_LARGE_VALUE; // Set high for first block nominal speed calculation. while (block_index != block_buffer_head) { block = &block_buffer[block_index]; + plan_refresh_block_directional_limits(block); nominal_speed = plan_compute_profile_nominal_speed(block); block->acceleration = plan_compute_effective_acceleration(block, nominal_speed); plan_compute_profile_parameters(block, nominal_speed, prev_nominal_speed); @@ -366,10 +431,8 @@ bool plan_buffer_line(float* target, plan_line_data_t* pl_data) { // down such that no individual axes maximum values are exceeded with respect to the line direction. // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. - block->millimeters = convert_delta_vector_to_unit_vector(unit_vec); - block->max_acceleration = limit_acceleration_by_axis_maximum(unit_vec); - block->jerk = (block->is_jog || block->motion.systemMotion) ? 0.0f : limit_jerk_by_axis_maximum(unit_vec); - block->rapid_rate = limit_rate_by_axis_maximum(unit_vec); + block->millimeters = convert_delta_vector_to_unit_vector(unit_vec); + plan_refresh_block_directional_limits(block); // Store programmed rate. if (block->motion.rapidMotion) { block->programmed_rate = block->rapid_rate; diff --git a/FluidNC/src/Protocol.cpp b/FluidNC/src/Protocol.cpp index ec7e655ec8..0a194e0356 100644 --- a/FluidNC/src/Protocol.cpp +++ b/FluidNC/src/Protocol.cpp @@ -698,6 +698,7 @@ static void protocol_do_initiate_cycle() { // log_debug("protocol_do_initiate_cycle " << state_name()); // Start cycle only if queued motions exist in planner buffer and the motion is not canceled. sys.step_control = {}; // Restore step control to normal operation + plan_update_velocity_profile_parameters(); plan_block_t* pb; if ((pb = plan_get_current_block()) && !sys.suspend().bit.motionCancel) { auto suspend = sys.suspend();