Mirrors_Framework/grblHAL
1
0
Fork 0
mirror of https://github.com/grblHAL/core.git synced 2026-09-22 03:08:33 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #660 from Dietz0r/master

Browse Source 
Improvements on Jerk-Accel code
This commit is contained in:
Terje Io
2025-01-18 20:41:30 +01:00
committed by GitHub
parent 5f135ed433 8ed5d369a3
commit 31fc78c949
2 changed files with 41 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
planner.c
+13 -3
View File
@@ -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.
stepper.c
+28 -16
View File
@@ -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 jerk_rampdown; // calculated startpoint of jerk rampdown
#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)
// 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.
#if ENABLE_JERK_ACCELERATION
time_to_jerk = last_segment_accel / pl_block->jerk;
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 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 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;
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 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 {
// 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);
}
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.