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

New stepper subsystem bug fixes.

Browse Source 
- New stepper algorithm with the new optimized planner seems to be
working nearly twice as fast as the previous algorithm.

- For one, the planner computation overhead is probably a fraction of
what it used to be with the worst case being about half still.

- Secondly, anytime the planner plans back to the first executing
block, it no longer overwrites the block conditions and allows it to
complete without lost steps. So no matter if the streams slows, the
protected planner should keep the steppers moving without risk of lost
steps (although this still needs to be tested thoroughly and may
audibly sound weird when this happens.)

- It now seems that the bottleneck is the serial baudrate (which is
good!)
This commit is contained in:
Sonny Jeon 2013-10-12 10:35:26 -06:00
parent 805f0f219c
commit 8a10654b1c
1 changed files with 104 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

165
stepper.c
View File

@ -34,13 +34,12 @@
#define RAMP_DECEL 2 #define RAMP_DECEL 2
#define LOAD_NOOP 0 #define LOAD_NOOP 0
#define LOAD_LINE 1 #define LOAD_SEGMENT 1
#define LOAD_BLOCK 2 #define LOAD_BLOCK 2
#define ST_NOOP 0 #define ST_END_OF_BLOCK bit(0)
#define ST_END_OF_BLOCK 1 #define ST_ACCEL bit(1)
#define ST_DECEL 2 #define ST_DECEL bit(2)
#define ST_DECEL_EOB 3
#define SEGMENT_BUFFER_SIZE 6 #define SEGMENT_BUFFER_SIZE 6
@ -87,7 +86,7 @@ static st_data_t segment_data[SEGMENT_BUFFER_SIZE];
typedef struct { typedef struct {
uint8_t n_step; // Number of step events to be executed for this segment uint8_t n_step; // Number of step events to be executed for this segment
uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment.
uint8_t flag; // Stepper algorithm execution flag to notify special conditions. uint8_t flag; // Stepper algorithm bit-flag for special execution conditions.
} st_segment_t; } st_segment_t;
static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
@ -208,8 +207,17 @@ void st_go_idle()
stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper
outputs simultaneously with these two interrupts. outputs simultaneously with these two interrupts.
*/ */
// NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or /* TODO:
// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this. - Measure time in ISR. Typical and worst-case.
- Write how the acceleration counters work and why they are set at half via mid-point rule.
- Determine if placing the position counters elsewhere (or change them to 8-bit variables that
are added to the system position counters at the end of a segment) frees up cycles.
- Write a blurb about how the acceleration should be handled within the ISR. All of the
time/step/ramp counters accurately keep track of the remainders and phasing of the variables
with time. This means we do not have to compute them via expensive floating point beforehand.
- Need to do an analysis to determine if these counters are really that much cheaper. At least
find out when it isn't anymore. Particularly when the ISR is at a very high frequency.
*/
ISR(TIMER2_COMPA_vect) ISR(TIMER2_COMPA_vect)
{ {
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR // SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
@ -266,10 +274,10 @@ ISR(TIMER2_COMPA_vect)
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
else { st.d_per_tick = st.delta_d; } else { st.d_per_tick = st.delta_d; }
// During feed hold, do not update rate or ramp type. Keep decelerating. // During feed hold, do not update rate, ramp type, or ramp counters. Keep decelerating.
// if (sys.state == STATE_CYCLE) { // if (sys.state == STATE_CYCLE) {
st.delta_d = st_current_data->initial_rate; st.delta_d = st_current_data->initial_rate;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule
if (st.delta_d == st_current_data->nominal_rate) { st.ramp_type = RAMP_NOOP_CRUISE; } if (st.delta_d == st_current_data->nominal_rate) { st.ramp_type = RAMP_NOOP_CRUISE; }
else { st.ramp_type = RAMP_ACCEL; } else { st.ramp_type = RAMP_ACCEL; }
// } // }
@ -277,16 +285,20 @@ ISR(TIMER2_COMPA_vect)
} }
// Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin.
if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) { if ( st_current_segment->flag & (ST_DECEL | ST_ACCEL) ) {
if (st.ramp_type == RAMP_NOOP_CRUISE) { /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration,
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to
} else { the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known
} rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2
st.ramp_type = RAMP_DECEL; as mandated by the mid-point rule. For these conditions, the ramp count has been pre-initialized
such that the following computation is still correct. */
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count;
if ( st_current_segment->flag & ST_DECEL ) { st.ramp_type = RAMP_DECEL; }
else { st.ramp_type = RAMP_ACCEL; }
} }
st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete. st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution.
} else { } else {
// Can't discard planner block here if a feed hold stops in middle of block. // Can't discard planner block here if a feed hold stops in middle of block.
@ -299,18 +311,20 @@ ISR(TIMER2_COMPA_vect)
// Adjust inverse time counter for ac/de-celerations // Adjust inverse time counter for ac/de-celerations
// NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally
// efficient on the Arduino AVR. This could change eventually, but it definitely will with ARM development. // efficient on the Arduino AVR. This could may not be true with higher ISR frequencies or faster CPUs.
if (st.ramp_type) { if (st.ramp_type) { // Ignored when ramp type is NOOP_CRUISE
st.ramp_count--; // Tick acceleration ramp counter st.ramp_count--; // Tick acceleration ramp counter
if (st.ramp_count == 0) { // Adjust step rate when its time if (st.ramp_count == 0) { // Adjust step rate when its time
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
st.delta_d += st_current_data->rate_delta; st.delta_d += st_current_data->rate_delta;
if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate.
st.delta_d = st_current_data->nominal_rate; // Set cruising velocity st.delta_d = st_current_data->nominal_rate; // Set cruising velocity
st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp.
} }
} else { // Adjust velocity for deceleration } else { // Adjust velocity for deceleration.
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
if (st.delta_d > st_current_data->rate_delta) { if (st.delta_d > st_current_data->rate_delta) {
st.delta_d -= st_current_data->rate_delta; st.delta_d -= st_current_data->rate_delta;
} else { } else {
@ -318,7 +332,11 @@ ISR(TIMER2_COMPA_vect)
// Moving near zero feed rate. Gracefully slow down. // Moving near zero feed rate. Gracefully slow down.
st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
// Check for and handle feed hold exit? At this point, machine is stopped. // TODO: Check for and handle feed hold exit? At this point, machine is stopped.
// - Set system flag to recompute plan and reset segment buffer.
// - Segment steps in buffer needs to be returned to planner correctly.
// busy = false;
// return;
} }
} }
@ -343,6 +361,7 @@ ISR(TIMER2_COMPA_vect)
if (st.counter_x < 0) { if (st.counter_x < 0) {
st.out_bits |= (1<<X_STEP_BIT); st.out_bits |= (1<<X_STEP_BIT);
st.counter_x += pl_current_block->step_event_count; st.counter_x += pl_current_block->step_event_count;
// st.steps_x++;
if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; } if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; } else { sys.position[X_AXIS]++; }
} }
@ -350,6 +369,7 @@ ISR(TIMER2_COMPA_vect)
if (st.counter_y < 0) { if (st.counter_y < 0) {
st.out_bits |= (1<<Y_STEP_BIT); st.out_bits |= (1<<Y_STEP_BIT);
st.counter_y += pl_current_block->step_event_count; st.counter_y += pl_current_block->step_event_count;
// st.steps_y++;
if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; } if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
else { sys.position[Y_AXIS]++; } else { sys.position[Y_AXIS]++; }
} }
@ -357,6 +377,7 @@ ISR(TIMER2_COMPA_vect)
if (st.counter_z < 0) { if (st.counter_z < 0) {
st.out_bits |= (1<<Z_STEP_BIT); st.out_bits |= (1<<Z_STEP_BIT);
st.counter_z += pl_current_block->step_event_count; st.counter_z += pl_current_block->step_event_count;
// st.steps_z++;
if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; } if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
else { sys.position[Z_AXIS]++; } else { sys.position[Z_AXIS]++; }
} }
@ -365,22 +386,45 @@ ISR(TIMER2_COMPA_vect)
st.segment_steps_remaining--; // Decrement step events count st.segment_steps_remaining--; // Decrement step events count
if (st.segment_steps_remaining == 0) { if (st.segment_steps_remaining == 0) {
// NOTE: sys.position updates could be done here. The bresenham counters can have
// their own fast 8-bit addition-only counters. Here we would check the direction and
// apply it to sys.position accordingly. However, this could take too much time
// combined with loading a new segment during next cycle too.
// TODO: Measure the time it would take in the worst case. It could still be faster
// overall during segment execution if uint8 step counters tracked this and was added
// to the system position variables here. Compared to worst case now, it wouldn't be
// that much different.
/*
// TODO: Upon loading, step counters would need to be zeroed.
// TODO: For feedrate overrides, we will have to execute add these values.. although
// for probing, this breaks. Current values won't be correct, unless we query it.
// It makes things more complicated, but still manageable.
if (st.steps_x > 0) {
if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS] += st.steps_x; }
else { sys.position[X_AXIS] -= st.steps_x; }
}
if (st.steps_y > 0) {
if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS] += st.steps_y; }
else { sys.position[Y_AXIS] -= st.steps_y; }
}
if (st.steps_z > 0) {
if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS] += st.steps_z; }
else { sys.position[Z_AXIS] -= st.steps_z; }
}
*/
// Line move is complete, set load line flag to check for new move. // Line move is complete, set load line flag to check for new move.
// Check if last line move in planner block. Discard if so. // Check if last line move in planner block. Discard if so.
if (st_current_segment->flag == ST_END_OF_BLOCK || st_current_segment->flag == ST_DECEL_EOB) { if (st_current_segment->flag & ST_END_OF_BLOCK) {
plan_discard_current_block(); plan_discard_current_block();
st.load_flag = LOAD_BLOCK; st.load_flag = LOAD_BLOCK;
} else { } else {
st.load_flag = LOAD_LINE; st.load_flag = LOAD_SEGMENT;
} }
// Discard current segment // Discard current segment
segment_buffer_tail = next_block_index( segment_buffer_tail ); segment_buffer_tail = next_block_index( segment_buffer_tail );
// NOTE: sys.position updates could be done here. The bresenham counters can have
// their own fast 8-bit addition-only counters. Here we would check the direction and
// apply it to sys.position accordingly. However, this could take too much time.
} }
st.out_bits ^= settings.invert_mask; // Apply step port invert mask st.out_bits ^= settings.invert_mask; // Apply step port invert mask
@ -531,13 +575,17 @@ void st_prep_buffer()
{ {
while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
st_segment_t *prep_segment = &segment_buffer[segment_buffer_head];
prep_segment->flag = 0;
// Determine if we need to load a new planner block. // Determine if we need to load a new planner block.
if (pl_prep_block == NULL) { if (pl_prep_block == NULL) {
pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block
if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. if (pl_prep_block == NULL) { return; } // No planner blocks. Exit.
// Check if the planner has re-computed this block mid-execution. If so, push the old segment block // Check if the planner has re-computed this block mid-execution. If so, push the old segment block
// data Otherwise, prepare a new segment block data. // data. Otherwise, prepare a new segment block data for the new planner block.
if (pl_partial_block_flag) { if (pl_partial_block_flag) {
// Prepare new shared segment block data and copy the relevant last segment block data. // Prepare new shared segment block data and copy the relevant last segment block data.
@ -553,6 +601,8 @@ void st_prep_buffer()
st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; st_prep_data->mm_per_step = last_st_prep_data->mm_per_step;
prep_segment->flag |= ST_ACCEL;
pl_partial_block_flag = false; // Reset flag pl_partial_block_flag = false; // Reset flag
// TODO: If the planner updates this block, particularly from a deceleration to an acceleration, // TODO: If the planner updates this block, particularly from a deceleration to an acceleration,
@ -588,11 +638,7 @@ void st_prep_buffer()
// Calculate the planner block velocity profile type and determine deceleration point. // Calculate the planner block velocity profile type and determine deceleration point.
float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index);
if (mm_decelerate_after == pl_prep_block->millimeters) { st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step );
st_prep_data->decelerate_after = st_prep_data->step_events_remaining;
} else {
st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step );
}
} }
@ -612,8 +658,7 @@ void st_prep_buffer()
- From deceleration to acceleration, i.e. common with jogging when new blocks are added. - From deceleration to acceleration, i.e. common with jogging when new blocks are added.
*/ */
st_segment_t *new_segment = &segment_buffer[segment_buffer_head]; prep_segment->st_data_index = st_data_prep_index;
new_segment->st_data_index = st_data_prep_index;
// TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'.
// The basic equation is: s = u*t + 0.5*a*t^2 // The basic equation is: s = u*t + 0.5*a*t^2
@ -623,21 +668,21 @@ void st_prep_buffer()
// are constants and would get compiled out. // are constants and would get compiled out.
//!!! Doesn't work as is. Requires last_velocity and acceleration in terms of steps, not mm. //!!! Doesn't work as is. Requires last_velocity and acceleration in terms of steps, not mm.
// new_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step); // prep_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step);
// if (st_prep_data->decelerate_after > 0) { // if (st_prep_data->decelerate_after > 0) {
// new_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); // prep_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step);
// } else { // } else {
// new_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); // prep_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step);
// } // }
new_segment->n_step = 7; //floor( (exit_speed*approx_time)/mm_per_step ); prep_segment->n_step = 15; //floor( (exit_speed*approx_time)/mm_per_step );
// new_segment->n_step = max(new_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? // prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions?
// new_segment->n_step = min(new_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow.
// Check if n_step exceeds steps remaining in planner block. If so, truncate. // Check if n_step exceeds steps remaining in planner block. If so, truncate.
if (new_segment->n_step > st_prep_data->step_events_remaining) { if (prep_segment->n_step > st_prep_data->step_events_remaining) {
new_segment->n_step = st_prep_data->step_events_remaining; prep_segment->n_step = st_prep_data->step_events_remaining;
// Don't need to compute last velocity, since it will be refreshed with a new block. // Don't need to compute last velocity, since it will be refreshed with a new block.
} }
@ -646,8 +691,8 @@ void st_prep_buffer()
// ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should
// be OK since it is likely moving at a fast rate already. // be OK since it is likely moving at a fast rate already.
if (st_prep_data->decelerate_after > 0) { if (st_prep_data->decelerate_after > 0) {
if (new_segment->n_step > st_prep_data->decelerate_after) { if (prep_segment->n_step > st_prep_data->decelerate_after) {
new_segment->n_step = st_prep_data->decelerate_after; prep_segment->n_step = st_prep_data->decelerate_after;
} }
// !!! Doesn't work. Remove if not using. // !!! Doesn't work. Remove if not using.
// if (last_velocity < last_nominal_v) { // if (last_velocity < last_nominal_v) {
@ -657,30 +702,20 @@ void st_prep_buffer()
// } // }
// } else { // In deceleration ramp // } else { // In deceleration ramp
// last_velocity -= pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); // last_velocity -= pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60));
} else {
if (st_prep_data->decelerate_after == 0) { prep_segment->flag |= ST_DECEL; }
} }
st_prep_data->decelerate_after -= prep_segment->n_step;
// Update stepper block variables. // Update stepper block variables.
st_prep_data->step_events_remaining -= new_segment->n_step; st_prep_data->step_events_remaining -= prep_segment->n_step;
if ( st_prep_data->step_events_remaining == 0 ) { if ( st_prep_data->step_events_remaining == 0 ) {
// Move planner pointer to next block prep_segment->flag |= ST_END_OF_BLOCK;
if (st_prep_data->decelerate_after == 0) { // Move planner pointer to next block and flag to load a new block for the next segment.
new_segment->flag = ST_DECEL_EOB; // Flag when deceleration begins and ends at EOB. Could rewrite to use bit flags too.
} else {
new_segment->flag = ST_END_OF_BLOCK;
}
pl_prep_index = next_block_pl_index(pl_prep_index); pl_prep_index = next_block_pl_index(pl_prep_index);
pl_prep_block = NULL; pl_prep_block = NULL;
} else {
// Current segment is mid-planner block. Just set the DECEL/NOOP acceleration flags.
if (st_prep_data->decelerate_after == 0) {
new_segment->flag = ST_DECEL;
} else {
new_segment->flag = ST_NOOP;
}
st_prep_data->decelerate_after -= new_segment->n_step;
} }
// New step segment completed. Increment segment buffer indices. // New step segment completed. Increment segment buffer indices.
segment_buffer_head = segment_next_head; segment_buffer_head = segment_next_head;
segment_next_head = next_block_index(segment_buffer_head); segment_next_head = next_block_index(segment_buffer_head);
@ -705,7 +740,7 @@ void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_r
// correctly, much in the same fashion as the deceleration counters. Need to think about this // correctly, much in the same fashion as the deceleration counters. Need to think about this
// make sure this is right, but i'm pretty sure it is. // make sure this is right, but i'm pretty sure it is.
// TODO: NULL means that the segment buffer has completed the block. Need to clean this up a bit. // TODO: NULL means that the segment buffer has just completed a planner block. Clean up!
if (pl_prep_block != NULL) { if (pl_prep_block != NULL) {
*millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step; *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step;
if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; }