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. diff --git a/stepper.c b/stepper.c index 8acc1f8..ffe04f4 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 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.