From 5ffa6bcb81d0248841f82c2f5e750e31930c807f Mon Sep 17 00:00:00 2001 From: Dietz0r <102995203+Dietz0r@users.noreply.github.com> Date: Fri, 3 Jan 2025 16:40:30 +0100 Subject: [PATCH 1/4] improved low speed regime handling and better overall effective acceleration calculation --- planner.c | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/planner.c b/planner.c index 7bf7059..e072782 100644 --- a/planner.c +++ b/planner.c @@ -549,9 +549,19 @@ bool plan_buffer_line (float *target, plan_line_data_t *pl_data) #if ENABLE_JERK_ACCELERATION // Calculate effective acceleration over block. Since jerk acceleration takes longer to execute due to ramp up and // ramp down of the acceleration at the start and end of a ramp we need to adjust the acceleration value the planner - // uses so it still calculates reasonable entry and exit speeds. We do this by adding 2x the time it takes to reach - // full acceleration to the trapezoidal acceleration time and dividing the programmed rate by the value obtained. - block->acceleration = block->programmed_rate / ((block->programmed_rate / block->max_acceleration) + 2.0f * (block->max_acceleration / block->jerk)); + // uses so it still calculates reasonable entry speeds, exit speeds and times to decelerate/accelerate. + // 2 general cases emerge: + // -slow speed regime: uncomplete jerk ramp (max_acceleration is not reached) + // -high speed regime: complete jerk ramp + time at max_axcel to reach desired programmed_rates + // Profiles are calculated as symmetrical (calculate to 1/2 programmed rate, then double) + float time_to_max_accel = block->max_acceleration / block->jerk; // unit: min - time it takes to reach max acceleration + float speed_after_jerkramp = 0.5f * block->jerk * time_to_max_accel * time_to_max_accel; // unit: mm / min - velocity after one completed jerk ramp up - Vt = V0 + A0T + 1/2 jerk*T + if (0.5*block->programmed_rate > speed_after_jerkramp) + // Profile time = 2x (1 complete jerk ramp + additional time at max_accel to reach desired speed) + block->acceleration = block->programmed_rate / (2.0f *(time_to_max_accel + (0.5f * block->programmed_rate - speed_after_jerkramp) / block->max_acceleration)); + else + // Max Accel is not reached. time_to_halfvelocity = sqrt( 0.5 programmed_rate * 2 / jerk) -> derived from Vt = V0 + A0T + 1/2 jerk*T (v0 and a0t == 0 in this case) + block->acceleration = block->programmed_rate / (2.0f * sqrt(block->programmed_rate / block->jerk)); #endif // TODO: Need to check this method handling zero junction speeds when starting from rest. From ca82c9a1e1c7b5d240f53d704bdbcc91e99f084c Mon Sep 17 00:00:00 2001 From: Dietz0r <102995203+Dietz0r@users.noreply.github.com> Date: Sun, 5 Jan 2025 23:57:55 +0100 Subject: [PATCH 2/4] moved last_segment_accel to static so values dont get lost between stepper loops, adjusted jerk ramps to grblhal calculations in mm instead of time based calculations. now works with low and very low jerk values. known issue: decel ramp does not hit 0 speed at end. --- stepper.c | 40 ++++++++++++++++++++++++++-------------- 1 file changed, 26 insertions(+), 14 deletions(-) diff --git a/stepper.c b/stepper.c index 8acc1f8..214c8fc 100644 --- a/stepper.c +++ b/stepper.c @@ -95,6 +95,11 @@ static float cycles_per_min; static volatile segment_t *segment_buffer_tail; static segment_t *segment_buffer_head, *segment_next_head; +#if ENABLE_JERK_ACCELERATION +// Static storage for acceleration value of last computed segment. +static float last_segment_accel = 0.0f; +#endif + // Pointers for the step segment being prepped from the planner buffer. Accessed only by the // main program. Pointers may be planning segments or planner blocks ahead of what being executed. static plan_block_t *pl_block; // Pointer to the planner block being prepped @@ -866,13 +871,14 @@ void st_prep_buffer (void) float dt_max = DT_SEGMENT; // Maximum segment time float dt = 0.0f; // Initialize segment time float time_var = dt_max; // Time worker variable -#if ENABLE_JERK_ACCELERATION - float last_segment_accel = 0.0f; // Acceleration value of last computed segment. Initialize as 0.0 -#endif float mm_var; // mm - Distance worker variable float speed_var; // Speed worker variable float mm_remaining = pl_block->millimeters; // New segment distance from end of block. float minimum_mm = mm_remaining - prep.req_mm_increment; // Guarantee at least one step. +#if ENABLE_ACCELERATION_PROFILES + float time_to_jerk; // time needed for jerk ramp + float mm_to_rampend; // mm needed to complete one jerk ramp mm == V(0)*T + 1/2 A0*T^2 + 1/6 J*T^3 +#endif if (minimum_mm < 0.0f) minimum_mm = 0.0f; @@ -897,15 +903,17 @@ void st_prep_buffer (void) case Ramp_Accel: // NOTE: Acceleration ramp only computes during first do-while loop. -#if ENABLE_JERK_ACCELERATION - if (((mm_remaining - prep.accelerate_until) / (prep.current_speed + 0.001f)) <= (last_segment_accel / pl_block->jerk)) { - //+0.001f to avoid divide by 0 speed, minor effect on jerk ramp (+1.0f was too large for low jerk values) +#if ENABLE_JERK_ACCELERATION + time_to_jerk = last_segment_accel / pl_block->jerk; + mm_to_rampend = (prep.current_speed * time_to_jerk) + (0.5f * last_segment_accel * time_to_jerk * time_to_jerk) + (pl_block->jerk * time_to_jerk * time_to_jerk * time_to_jerk) / 6.0f; + if ((mm_remaining - prep.accelerate_until) > mm_to_rampend) { + //+1.0f to avoid divide by 0 speed, minor effect on jerk ramp // Check if we are on ramp up or ramp down. Ramp down if time to end of acceleration is less than time needed to reach 0 acceleration. // Then limit acceleration change by jerk up to max acceleration and update for next segment. - // Minimum acceleration jerk per time_var to ensure acceleartion completes. Acceleration change at end of ramp is in acceptable jerk range. - last_segment_accel = max(last_segment_accel - pl_block->jerk * time_var, pl_block->jerk * time_var); - } else { + // Minimum acceleration jerk per time_var to ensure acceleration completes. Acceleration change at end of ramp is in acceptable jerk range. last_segment_accel = min(last_segment_accel + pl_block->jerk * time_var, pl_block->max_acceleration); + } else { + last_segment_accel = max(last_segment_accel - pl_block->jerk * time_var, pl_block->jerk * time_var); } speed_var = last_segment_accel * time_var; #else @@ -942,14 +950,15 @@ void st_prep_buffer (void) default: // case Ramp_Decel: // NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed. #if ENABLE_JERK_ACCELERATION - if ((mm_remaining / (prep.current_speed + 0.001f)) <= (last_segment_accel / pl_block->jerk)) { - //+0.001f to avoid divide by 0 speed, minor effect on jerk ramp (+1.0f was too large for low jerk values) - // Check if we are on ramp up or ramp down. Ramp down if time to end of deceleration is less than time needed to reach 0 acceleration. + time_to_jerk = last_segment_accel / pl_block->jerk; + mm_to_rampend = (prep.current_speed * time_to_jerk) + (0.5f * last_segment_accel * time_to_jerk * time_to_jerk) + (pl_block->jerk * time_to_jerk * time_to_jerk * time_to_jerk) / 6.0f; + if (mm_remaining > mm_to_rampend) { + // Check if we are on ramp up or ramp down. Ramp down if travel distance to end of deceleration is less than distance needed to reach 0 acceleration. // Then limit acceleration change by jerk up to max acceleration and update for next segment. // Minimum acceleration of jerk per time_var to ensure acceleration completes. Acceleration change at end of ramp is in acceptable jerk range. - last_segment_accel = max(last_segment_accel - pl_block->jerk * time_var, pl_block->jerk * time_var); - } else { last_segment_accel = min(last_segment_accel + pl_block->jerk * time_var, pl_block->max_acceleration); + } else { + last_segment_accel = max(last_segment_accel - pl_block->jerk * time_var, pl_block->jerk * time_var); } speed_var = last_segment_accel * time_var; // Used as delta speed (mm/min) #else @@ -968,6 +977,9 @@ void st_prep_buffer (void) time_var = 2.0f * (mm_remaining - prep.mm_complete) / (prep.current_speed + prep.exit_speed); mm_remaining = prep.mm_complete; prep.current_speed = prep.exit_speed; +#if ENABLE_JERK_ACCELERATION + last_segment_accel = 0.0f; // reset acceleration variable to 0 for next accel ramp +#endif } dt += time_var; // Add computed ramp time to total segment time. From 5312946d7cce38f0c0f4112d1e9c20d17b4a3ac5 Mon Sep 17 00:00:00 2001 From: Dietz0r <102995203+Dietz0r@users.noreply.github.com> Date: Tue, 7 Jan 2025 18:38:30 +0100 Subject: [PATCH 3/4] small update to mm_to_rampend calc --- stepper.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/stepper.c b/stepper.c index 214c8fc..bddb345 100644 --- a/stepper.c +++ b/stepper.c @@ -905,7 +905,7 @@ void st_prep_buffer (void) // NOTE: Acceleration ramp only computes during first do-while loop. #if ENABLE_JERK_ACCELERATION time_to_jerk = last_segment_accel / pl_block->jerk; - mm_to_rampend = (prep.current_speed * time_to_jerk) + (0.5f * last_segment_accel * time_to_jerk * time_to_jerk) + (pl_block->jerk * time_to_jerk * time_to_jerk * time_to_jerk) / 6.0f; + mm_to_rampend = (prep.current_speed * time_to_jerk) + (0.5f * last_segment_accel * time_to_jerk * time_to_jerk) + (pl_block->jerk * time_to_jerk * time_to_jerk * time_to_jerk) / 6.0f; //Distance to 0 acceleration at speed if ((mm_remaining - prep.accelerate_until) > mm_to_rampend) { //+1.0f to avoid divide by 0 speed, minor effect on jerk ramp // Check if we are on ramp up or ramp down. Ramp down if time to end of acceleration is less than time needed to reach 0 acceleration. @@ -951,11 +951,11 @@ void st_prep_buffer (void) // NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed. #if ENABLE_JERK_ACCELERATION time_to_jerk = last_segment_accel / pl_block->jerk; - mm_to_rampend = (prep.current_speed * time_to_jerk) + (0.5f * last_segment_accel * time_to_jerk * time_to_jerk) + (pl_block->jerk * time_to_jerk * time_to_jerk * time_to_jerk) / 6.0f; + mm_to_rampend = (0.5f * last_segment_accel * time_to_jerk * time_to_jerk) + (pl_block->jerk * time_to_jerk * time_to_jerk * time_to_jerk) / 6.0f; // Distance to 0 acceleration if (mm_remaining > mm_to_rampend) { // Check if we are on ramp up or ramp down. Ramp down if travel distance to end of deceleration is less than distance needed to reach 0 acceleration. // Then limit acceleration change by jerk up to max acceleration and update for next segment. - // Minimum acceleration of jerk per time_var to ensure acceleration completes. Acceleration change at end of ramp is in acceptable jerk range. + // Minimum acceleration of jerk per time_var to ensure deceleration completes. Acceleration change at end of ramp is in acceptable jerk range. last_segment_accel = min(last_segment_accel + pl_block->jerk * time_var, pl_block->max_acceleration); } else { last_segment_accel = max(last_segment_accel - pl_block->jerk * time_var, pl_block->jerk * time_var); From ae70c6cbe3111df9a685e052320a3eb99cf97932 Mon Sep 17 00:00:00 2001 From: Dietz0r <102995203+Dietz0r@users.noreply.github.com> Date: Wed, 8 Jan 2025 09:06:00 +0100 Subject: [PATCH 4/4] improved deceleration ramp behaviour - cleaned up code a bit Thanks to @Engigneer, @fpq473 for helping figuring this out! --- stepper.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/stepper.c b/stepper.c index bddb345..ffe04f4 100644 --- a/stepper.c +++ b/stepper.c @@ -877,7 +877,7 @@ void st_prep_buffer (void) float minimum_mm = mm_remaining - prep.req_mm_increment; // Guarantee at least one step. #if ENABLE_ACCELERATION_PROFILES float time_to_jerk; // time needed for jerk ramp - float mm_to_rampend; // mm needed to complete one jerk ramp mm == V(0)*T + 1/2 A0*T^2 + 1/6 J*T^3 + float jerk_rampdown; // calculated startpoint of jerk rampdown #endif if (minimum_mm < 0.0f) @@ -905,10 +905,10 @@ void st_prep_buffer (void) // NOTE: Acceleration ramp only computes during first do-while loop. #if ENABLE_JERK_ACCELERATION time_to_jerk = last_segment_accel / pl_block->jerk; - mm_to_rampend = (prep.current_speed * time_to_jerk) + (0.5f * last_segment_accel * time_to_jerk * time_to_jerk) + (pl_block->jerk * time_to_jerk * time_to_jerk * time_to_jerk) / 6.0f; //Distance to 0 acceleration at speed - if ((mm_remaining - prep.accelerate_until) > mm_to_rampend) { + jerk_rampdown =time_to_jerk * (prep.current_speed + (0.5f * last_segment_accel * time_to_jerk) + (pl_block->jerk * time_to_jerk * time_to_jerk) / 6.0f); //Distance to 0 acceleration at speed (mm == V(0)*T + 1/2 A0*T^2 + 1/6 J*T^3) + if ((mm_remaining - prep.accelerate_until) > jerk_rampdown) { //+1.0f to avoid divide by 0 speed, minor effect on jerk ramp - // Check if we are on ramp up or ramp down. Ramp down if time to end of acceleration is less than time needed to reach 0 acceleration. + // Check if we are on ramp up or ramp down. Ramp down if distance to end of acceleration is less than distance needed to reach 0 acceleration. // Then limit acceleration change by jerk up to max acceleration and update for next segment. // Minimum acceleration jerk per time_var to ensure acceleration completes. Acceleration change at end of ramp is in acceptable jerk range. last_segment_accel = min(last_segment_accel + pl_block->jerk * time_var, pl_block->max_acceleration); @@ -951,9 +951,9 @@ void st_prep_buffer (void) // NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed. #if ENABLE_JERK_ACCELERATION time_to_jerk = last_segment_accel / pl_block->jerk; - mm_to_rampend = (0.5f * last_segment_accel * time_to_jerk * time_to_jerk) + (pl_block->jerk * time_to_jerk * time_to_jerk * time_to_jerk) / 6.0f; // Distance to 0 acceleration - if (mm_remaining > mm_to_rampend) { - // Check if we are on ramp up or ramp down. Ramp down if travel distance to end of deceleration is less than distance needed to reach 0 acceleration. + jerk_rampdown = prep.exit_speed + time_to_jerk * (last_segment_accel - (0.5f * pl_block->jerk * time_to_jerk)); // Speedpoint to start ramping down deceleration. (V = a * t - 1/2 j * t^2) + if (prep.current_speed > jerk_rampdown) { + // Check if we are on ramp up or ramp down. Ramp down if speed is less than speed needed for reaching 0 acceleration. // Then limit acceleration change by jerk up to max acceleration and update for next segment. // Minimum acceleration of jerk per time_var to ensure deceleration completes. Acceleration change at end of ramp is in acceptable jerk range. last_segment_accel = min(last_segment_accel + pl_block->jerk * time_var, pl_block->max_acceleration);