ManuelW/grbl-LPC-CoreXY
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

Planner optimizations.

Browse Source 
- Improved planner execution speed by 5% or more. Re-factored most of
the calculations in terms of the square of velocity. This removed a lot
of sqrt() calculations for every planner_recalculate.
This commit is contained in:
Sonny Jeon 2012-12-11 17:42:29 -07:00
parent 4f273db805
commit 2dc920a8e5
3 changed files with 84 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
planner.c
View File

@ -42,7 +42,7 @@ typedef struct {
// from g-code position for movements requiring multiple line motions, // from g-code position for movements requiring multiple line motions,
// i.e. arcs, canned cycles, and backlash compensation. // i.e. arcs, canned cycles, and backlash compensation.
float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment
float previous_nominal_speed; // Nominal speed of previous path line segment float previous_nominal_speed_sqr; // Nominal speed of previous path line segment
} planner_t; } planner_t;
static planner_t pl; static planner_t pl;
@ -65,14 +65,6 @@ static uint8_t prev_block_index(uint8_t block_index)
} }
// Calculates the final velocity from an initial velocity over a distance with constant acceleration.
// NOTE: Guaranteed to not exceed BLOCK_BUFFER_SIZE calls per planner cycle.
static float calculate_final_velocity(float acceleration, float initial_velocity, float distance)
{
return( sqrt(initial_velocity*initial_velocity+2*acceleration*distance) );
}
/* STEPPER VELOCITY PROFILE DEFINITION /* STEPPER VELOCITY PROFILE DEFINITION
less than nominal rate-> + less than nominal rate-> +
+--------+ <- nominal_rate /|\ +--------+ <- nominal_rate /|\
@ -96,10 +88,10 @@ static float calculate_final_velocity(float acceleration, float initial_velocity
the new initial rate and n_steps until deceleration are computed, since the stepper algorithm the new initial rate and n_steps until deceleration are computed, since the stepper algorithm
already handles acceleration and cruising and just needs to know when to start decelerating. already handles acceleration and cruising and just needs to know when to start decelerating.
*/ */
static void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed) static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr)
{ {
// Compute new initial rate for stepper algorithm // Compute new initial rate for stepper algorithm
block->initial_rate = ceil(entry_speed*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) block->initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// Compute efficiency variable for following calculations. Removes a float divide and multiply. // Compute efficiency variable for following calculations. Removes a float divide and multiply.
// TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds. // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds.
@ -107,7 +99,7 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed, flo
// First determine intersection distance (in steps) from the exit point for a triangular profile. // First determine intersection distance (in steps) from the exit point for a triangular profile.
// Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) )
int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed*entry_speed-exit_speed*exit_speed)) ); int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) );
// Check if this is a pure acceleration block by a intersection distance less than zero. Also // Check if this is a pure acceleration block by a intersection distance less than zero. Also
// prevents signed and unsigned integer conversion errors. // prevents signed and unsigned integer conversion errors.
@ -117,8 +109,7 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed, flo
// Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile.
// Value is never negative. Nominal speed is always greater than or equal to the exit speed. // Value is never negative. Nominal speed is always greater than or equal to the exit speed.
// Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) )
block->decelerate_after = ceil(steps_per_mm_div_2_acc * block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr));
(block->nominal_speed*block->nominal_speed-exit_speed*exit_speed));
// The lesser of the two triangle and trapezoid distances always defines the velocity profile. // The lesser of the two triangle and trapezoid distances always defines the velocity profile.
if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; } if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; }
@ -171,59 +162,61 @@ static void planner_recalculate()
// for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to // for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to
// find out where in the planner buffer is the a safe place to begin re-planning from. // find out where in the planner buffer is the a safe place to begin re-planning from.
// if (block_buffer_head != block_buffer_tail) {
float entry_speed_sqr;
// Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the
// tail(first) block to prevent over-writing of the initial entry speed. // tail(first) block to prevent over-writing of the initial entry speed.
uint8_t block_index = block_buffer_head; uint8_t block_index = prev_block_index( block_buffer_head ); // Assume buffer is not empty.
block_t *next = NULL; block_t *current = &block_buffer[block_index]; // Head block-1 = Newly appended block
block_t *current = NULL; block_t *next;
block_t *previous = NULL; if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); }
while(block_index != block_buffer_tail) { while (block_index != block_buffer_tail) {
block_index = prev_block_index( block_index );
next = current; next = current;
current = previous; current = &block_buffer[block_index];
previous = &block_buffer[block_index];
if (current) {
if (next) {
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
// If not, block in state of acceleration or deceleration. Reset entry speed to maximum and // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
// check for maximum allowable speed reductions to ensure maximum possible planned speed. // check for maximum allowable speed reductions to ensure maximum possible planned speed.
if (current->entry_speed != current->max_entry_speed) { if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
// If nominal length true, max junction speed is guaranteed to be reached. Only compute // If nominal length true, max junction speed is guaranteed to be reached. Only compute
// for max allowable speed if block is decelerating and nominal length is false. // for max allowable speed if block is decelerating and nominal length is false.
if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
current->entry_speed = min( current->max_entry_speed, current->entry_speed_sqr = current->max_entry_speed_sqr;
calculate_final_velocity(settings.acceleration,next->entry_speed,current->millimeters)); current->recalculate_flag = true; // Almost always changes. So force recalculate.
} else {
current->entry_speed = current->max_entry_speed; if ((!current->nominal_length_flag) && (current->max_entry_speed_sqr > next->entry_speed_sqr)) {
} // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance
current->recalculate_flag = true; entry_speed_sqr = next->entry_speed_sqr + 2*settings.acceleration*current->millimeters;
if (entry_speed_sqr < current->max_entry_speed_sqr) {
current->entry_speed_sqr = entry_speed_sqr;
} }
} }
} }
block_index = prev_block_index( block_index );
} }
// Perform forward planner pass. Begins junction speed adjustments after tail(first) block. // Perform forward planner pass. Begins junction speed adjustments after tail(first) block.
// Also recalculate trapezoids, block by block, as the forward pass completes the plan. // Also recalculate trapezoids, block by block, as the forward pass completes the plan.
block_index = block_buffer_tail; block_index = next_block_index(block_buffer_tail);
next = NULL; next = &block_buffer[block_buffer_tail]; // Places tail(first) block into current
while (block_index != block_buffer_head) { while (block_index != block_buffer_head) {
current = next; current = next;
next= &block_buffer[block_index]; next = &block_buffer[block_index];
if (current) {
// If the current block is an acceleration block, but it is not long enough to complete the // If the current block is an acceleration block, but it is not long enough to complete the
// full speed change within the block, we need to adjust the entry speed accordingly. Entry // full speed change within the block, we need to adjust the exit speed accordingly. Entry
// speeds have already been reset, maximized, and reverse planned by reverse planner. // speeds have already been reset, maximized, and reverse planned by reverse planner.
// If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. // If nominal length is true, max junction speed is guaranteed to be reached. So skip block.
if (!current->nominal_length_flag) { if (!current->nominal_length_flag) {
if (current->entry_speed < next->entry_speed) { if (current->entry_speed_sqr < next->entry_speed_sqr) {
float entry_speed = min( next->entry_speed, // Compute block exit speed based on the current block speed and distance
calculate_final_velocity(settings.acceleration,current->entry_speed,current->millimeters) ); // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance
entry_speed_sqr = current->entry_speed_sqr + 2*settings.acceleration*current->millimeters;
// Check for junction speed change // If it's less than the stored value, update the exit speed and set recalculate flag.
if (next->entry_speed != entry_speed) { if (entry_speed_sqr < next->entry_speed_sqr) {
next->entry_speed = entry_speed; next->entry_speed_sqr = entry_speed_sqr;
next->recalculate_flag = true; next->recalculate_flag = true;
} }
} }
@ -232,15 +225,18 @@ static void planner_recalculate()
// Recalculate if current block entry or exit junction speed has changed. // Recalculate if current block entry or exit junction speed has changed.
if (current->recalculate_flag || next->recalculate_flag) { if (current->recalculate_flag || next->recalculate_flag) {
// NOTE: Entry and exit factors always > 0 by all previous logic operations. // NOTE: Entry and exit factors always > 0 by all previous logic operations.
calculate_trapezoid_for_block(current, current->entry_speed, next->entry_speed); calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);
current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
} }
}
block_index = next_block_index( block_index ); block_index = next_block_index( block_index );
} }
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
calculate_trapezoid_for_block(next, next->entry_speed, MINIMUM_PLANNER_SPEED); calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
next->recalculate_flag = false; next->recalculate_flag = false;
// }
} }
void plan_reset_buffer() void plan_reset_buffer()
@ -342,18 +338,18 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
unit_vec[Z_AXIS] = -unit_vec[Z_AXIS]; unit_vec[Z_AXIS] = -unit_vec[Z_AXIS];
} }
// Calculate speed in mm/minute for each axis. No divide by zero due to previous checks. // Calculate speed in mm/minute and stepper rate for each axis. No divide by zero due to previous checks.
// NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }
block->nominal_speed = feed_rate; // (mm/min) Always > 0 block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0
block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// TODO: When acceleration independence is installed, it can be kept in terms of 2*acceleration for // TODO: When acceleration independence is installed, it can be kept in terms of 2*acceleration for
// each block. This could save some 2*acceleration multiplications elsewhere. Need to check though. // each block. This could save some 2*acceleration multiplications elsewhere. Need to check though.
// Compute the acceleration, nominal rate, and distance traveled per step event for the stepper algorithm. // Compute the acceleration and distance traveled per step event for the stepper algorithm.
block->rate_delta = ceil(settings.acceleration* block->rate_delta = ceil(settings.acceleration*
((RANADE_MULTIPLIER/(60*60))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) ((RANADE_MULTIPLIER/(60*60))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
block->nominal_rate = ceil(block->nominal_speed*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step) block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
@ -365,16 +361,16 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
// path width or max_jerk in the previous grbl version. This approach does not actually deviate // path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the // from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity. // nonlinearities of both the junction angle and junction velocity.
// NOTE: This is basically an exact path mode (G61), but it doesn't come to a complete stop unless // NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
// the junction deviation value is high. In the future, if continuous mode (G64) is desired, the // mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
// math here is exactly the same. Instead of motioning all the way to junction point, the machine // stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
// will just need to follow the arc circle defined above and check if the arc radii are no longer // is exactly the same. Instead of motioning all the way to junction point, the machine will
// than half of either line segment to ensure no overlapping. Right now, the Arduino likely doesn't // just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
// have the horsepower to do these calculations at high feed rates. // a continuous mode path, but ARM-based microcontrollers most certainly do.
float vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed float vmax_junction_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed > 0.0)) { if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed_sqr > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
@ -383,21 +379,21 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
// Skip and use default max junction speed for 0 degree acute junction. // Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) { if (cos_theta < 0.95) {
vmax_junction = min(pl.previous_nominal_speed,block->nominal_speed); vmax_junction_sqr = min(pl.previous_nominal_speed_sqr,block->nominal_speed_sqr);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) { if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation // Compute maximum junction velocity based on maximum acceleration and junction deviation
float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction, vmax_junction_sqr = min(vmax_junction_sqr,
sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2) );
} }
} }
} }
block->max_entry_speed = vmax_junction; block->max_entry_speed_sqr = vmax_junction_sqr;
// Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED. // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED.
float v_allowable = calculate_final_velocity(settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); float v_allowable_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*settings.acceleration*block->millimeters;
block->entry_speed = min(vmax_junction, v_allowable); block->entry_speed_sqr = min(vmax_junction_sqr, v_allowable_sqr);
// Initialize planner efficiency flags // Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
@ -407,13 +403,13 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the // the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks. // the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; } if (block->nominal_speed_sqr <= v_allowable_sqr) { block->nominal_length_flag = true; }
else { block->nominal_length_flag = false; } else { block->nominal_length_flag = false; }
block->recalculate_flag = true; // Always calculate trapezoid for new block block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed // Update previous path unit_vector and nominal speed (squared)
memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
pl.previous_nominal_speed = block->nominal_speed; pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
// Update buffer head and next buffer head indices // Update buffer head and next buffer head indices
block_buffer_head = next_buffer_head; block_buffer_head = next_buffer_head;
@ -446,8 +442,8 @@ void plan_cycle_reinitialize(int32_t step_events_remaining)
block->step_event_count = step_events_remaining; block->step_event_count = step_events_remaining;
// Re-plan from a complete stop. Reset planner entry speeds and flags. // Re-plan from a complete stop. Reset planner entry speeds and flags.
block->entry_speed = 0.0; block->entry_speed_sqr = 0.0;
block->max_entry_speed = 0.0; block->max_entry_speed_sqr = 0.0;
block->nominal_length_flag = false; block->nominal_length_flag = false;
block->recalculate_flag = true; block->recalculate_flag = true;
planner_recalculate(); planner_recalculate();

6
planner.h
View File

@ -37,9 +37,9 @@ typedef struct {
int32_t step_event_count; // The number of step events required to complete this block int32_t step_event_count; // The number of step events required to complete this block
// Fields used by the motion planner to manage acceleration // Fields used by the motion planner to manage acceleration
float nominal_speed; // The nominal speed for this block in mm/min float nominal_speed_sqr; // The nominal speed for this block in mm/min
float entry_speed; // Entry speed at previous-current block junction in mm/min float entry_speed_sqr; // Entry speed at previous-current block junction in mm/min
float max_entry_speed; // Maximum allowable junction entry speed in mm/min float max_entry_speed_sqr; // Maximum allowable junction entry speed in mm/min
float millimeters; // The total travel of this block in mm float millimeters; // The total travel of this block in mm
uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction
uint8_t nominal_length_flag; // Planner flag for nominal speed always reached uint8_t nominal_length_flag; // Planner flag for nominal speed always reached

3
stepper.c
View File

@ -30,7 +30,6 @@
// Some useful constants // Some useful constants
#define TICKS_PER_MICROSECOND (F_CPU/1000000) #define TICKS_PER_MICROSECOND (F_CPU/1000000)
// #define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND)
#define INTERRUPTS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) #define INTERRUPTS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)
#define CRUISE_RAMP 0 #define CRUISE_RAMP 0
#define ACCEL_RAMP 1 #define ACCEL_RAMP 1
@ -189,7 +188,7 @@ ISR(TIMER2_COMPA_vect)
// Initialize ramp type. // Initialize ramp type.
if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; }
else if (current_block->entry_speed == current_block->nominal_speed) { st.ramp_type = CRUISE_RAMP; } else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; }
else { st.ramp_type = ACCEL_RAMP; } else { st.ramp_type = ACCEL_RAMP; }
} }