Skip to content

Commit

Permalink
New stepper subsystem bug fixes.
Browse files Browse the repository at this point in the history 
- 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!)
  • Loading branch information
@chamnit
chamnit committed Oct 12, 2013
1 parent 805f0f2 commit 8a10654
Showing 1 changed file with 104 additions and 69 deletions.
173 changes: 104 additions & 69 deletions stepper.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,13 +34,12 @@
#define RAMP_DECEL 2

#define LOAD_NOOP 0
#define LOAD_LINE 1
#define LOAD_SEGMENT 1
#define LOAD_BLOCK 2

#define ST_NOOP 0
#define ST_END_OF_BLOCK 1
#define ST_DECEL 2
#define ST_DECEL_EOB 3
#define ST_END_OF_BLOCK bit(0)
#define ST_ACCEL bit(1)
#define ST_DECEL bit(2)

#define SEGMENT_BUFFER_SIZE 6

Expand Down Expand Up @@ -87,7 +86,7 @@ static st_data_t segment_data[SEGMENT_BUFFER_SIZE];
typedef struct {
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 flag; // Stepper algorithm execution flag to notify special conditions.
uint8_t flag; // Stepper algorithm bit-flag for special execution conditions.
} st_segment_t;
static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];

Expand Down Expand Up @@ -208,8 +207,17 @@ void st_go_idle()
stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper
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
// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this.
/* TODO:
- 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)
{
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
Expand Down Expand Up @@ -266,27 +274,31 @@ ISR(TIMER2_COMPA_vect)
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
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) {
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; }
else { st.ramp_type = RAMP_ACCEL; }
// }

}

// 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.ramp_type == RAMP_NOOP_CRUISE) {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
} else {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle
}
st.ramp_type = RAMP_DECEL;
if ( st_current_segment->flag & (ST_DECEL | ST_ACCEL) ) {
/* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration,
or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to
the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the
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
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 {
// Can't discard planner block here if a feed hold stops in middle of block.
Expand All @@ -299,26 +311,32 @@ ISR(TIMER2_COMPA_vect)

// Adjust inverse time counter for ac/de-celerations
// 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.
if (st.ramp_type) {
// efficient on the Arduino AVR. This could may not be true with higher ISR frequencies or faster CPUs.
if (st.ramp_type) { // Ignored when ramp type is NOOP_CRUISE
st.ramp_count--; // Tick acceleration ramp counter
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
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
st.delta_d += st_current_data->rate_delta;
if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate.
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) {
st.delta_d -= st_current_data->rate_delta;
} else {

// Moving near zero feed rate. Gracefully slow down.
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;

}
}
Expand All @@ -343,20 +361,23 @@ ISR(TIMER2_COMPA_vect)
if (st.counter_x < 0) {
st.out_bits |= (1<<X_STEP_BIT);
st.counter_x += pl_current_block->step_event_count;
// st.steps_x++;
if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; }
}
st.counter_y -= pl_current_block->steps[Y_AXIS];
if (st.counter_y < 0) {
st.out_bits |= (1<<Y_STEP_BIT);
st.counter_y += pl_current_block->step_event_count;
// st.steps_y++;
if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
else { sys.position[Y_AXIS]++; }
}
st.counter_z -= pl_current_block->steps[Z_AXIS];
if (st.counter_z < 0) {
st.out_bits |= (1<<Z_STEP_BIT);
st.counter_z += pl_current_block->step_event_count;
// st.steps_z++;
if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
else { sys.position[Z_AXIS]++; }
}
Expand All @@ -365,22 +386,45 @@ ISR(TIMER2_COMPA_vect)
st.segment_steps_remaining--; // Decrement step events count
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.
// 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();
st.load_flag = LOAD_BLOCK;
} else {
st.load_flag = LOAD_LINE;
st.load_flag = LOAD_SEGMENT;
}

// Discard current segment
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
Expand Down Expand Up @@ -531,13 +575,17 @@ void st_prep_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.
if (pl_prep_block == NULL) {
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.

// 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) {

// Prepare new shared segment block data and copy the relevant last segment block data.
Expand All @@ -553,6 +601,8 @@ void st_prep_buffer()

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

// TODO: If the planner updates this block, particularly from a deceleration to an acceleration,
Expand Down Expand Up @@ -588,11 +638,7 @@ void st_prep_buffer()

// Calculate the planner block velocity profile type and determine deceleration point.
float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index);
if (mm_decelerate_after == pl_prep_block->millimeters) {
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 );
}
st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step );

}

Expand All @@ -612,8 +658,7 @@ void st_prep_buffer()
- From deceleration to acceleration, i.e. common with jogging when new blocks are added.
*/

st_segment_t *new_segment = &segment_buffer[segment_buffer_head];
new_segment->st_data_index = st_data_prep_index;
prep_segment->st_data_index = st_data_prep_index;

// 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
Expand All @@ -623,21 +668,21 @@ void st_prep_buffer()
// are constants and would get compiled out.

//!!! 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) {
// 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 {
// 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 );
// new_segment->n_step = max(new_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 = 15; //floor( (exit_speed*approx_time)/mm_per_step );
// prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions?
// 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.
if (new_segment->n_step > st_prep_data->step_events_remaining) {
new_segment->n_step = st_prep_data->step_events_remaining;
if (prep_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.
}
Expand All @@ -646,8 +691,8 @@ void st_prep_buffer()
// 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.
if (st_prep_data->decelerate_after > 0) {
if (new_segment->n_step > st_prep_data->decelerate_after) {
new_segment->n_step = st_prep_data->decelerate_after;
if (prep_segment->n_step > st_prep_data->decelerate_after) {
prep_segment->n_step = st_prep_data->decelerate_after;
}
// !!! Doesn't work. Remove if not using.
// if (last_velocity < last_nominal_v) {
Expand All @@ -657,29 +702,19 @@ void st_prep_buffer()
// }
// } else { // In deceleration ramp
// 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.
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 ) {
// Move planner pointer to next block
if (st_prep_data->decelerate_after == 0) {
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;
}
prep_segment->flag |= ST_END_OF_BLOCK;
// Move planner pointer to next block and flag to load a new block for the next segment.
pl_prep_index = next_block_pl_index(pl_prep_index);
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.
segment_buffer_head = segment_next_head;
Expand All @@ -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
// 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) {
*millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step;
if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; }
Expand Down

0 comments on commit 8a10654

Please sign in to comment.