diff --git a/FluidNC/src/Kinematics/ParallelDelta.cpp b/FluidNC/src/Kinematics/ParallelDelta.cpp index 2ac2a893a..e2b6d6b8f 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 jerkFactor0 = axis0->_jerkFactor; 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->_jerkFactor = jerkFactor0; } init_position(); diff --git a/FluidNC/src/Machine/Axis.cpp b/FluidNC/src/Machine/Axis.cpp index b75c3f6a4..775bde47e 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_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 16f474acc..9c46a9da5 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 _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 be2c2fa04..b40cee800 100644 --- a/FluidNC/src/NutsBolts.cpp +++ b/FluidNC/src/NutsBolts.cpp @@ -77,36 +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 - -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_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 fb324ab25..71c08be26 100644 --- a/FluidNC/src/NutsBolts.h +++ b/FluidNC/src/NutsBolts.h @@ -74,8 +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_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 eb0b1b819..e513d7164 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; @@ -244,6 +290,41 @@ 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)); +} + +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) { @@ -287,7 +368,9 @@ 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); prev_nominal_speed = nominal_speed; block_index = plan_next_block_index(block_index); @@ -348,9 +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->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); + plan_refresh_block_directional_limits(block); // Store programmed rate. if (block->motion.rapidMotion) { block->programmed_rate = block->rapid_rate; @@ -360,6 +442,11 @@ bool plan_buffer_line(float* target, plan_line_data_t* pl_data) { block->programmed_rate *= block->millimeters; } } + + // 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)) { // 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 defead28d..7f98c10b8 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/Protocol.cpp b/FluidNC/src/Protocol.cpp index ec7e655ec..0a194e035 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(); diff --git a/FluidNC/src/Stepper.cpp b/FluidNC/src/Stepper.cpp index 6d62c6ab0..213e1a25d 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; @@ -102,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). @@ -327,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; @@ -343,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. @@ -418,9 +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; + 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. @@ -565,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. @@ -574,13 +600,30 @@ 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; } 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 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 + * (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 = 0.5f * (previous_acceleration + 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 +635,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 +656,24 @@ 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 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 + + 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; // 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) @@ -622,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; }