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

Updated comments. Changed stepper variable names to be more understandable. Added step locking note.

Browse Source 
- Updated config comments and stepper code comments for the new changes.

- Changed stepper algorithm variable names to be more understandable in
what they actually do.

- Added a stepper lock note in default.h per user request.

- Started some code layout in handling feed holds and refactoring the
homing routine to use the main stepper algorithm instead of a seperate
version.
This commit is contained in:
Sonny Jeon
2013-10-29 08:31:48 -06:00
parent f7429ec79b
commit 27297d444b
7 changed files with 998 additions and 286 deletions
Download Patch File Download Diff File Expand all files Collapse all files

164
stepper.c
View File

@ -55,9 +55,9 @@ typedef struct {
uint8_t segment_steps_remaining; // Steps remaining in line segment motion
// Used by inverse time algorithm to track step rate
int32_t counter_d; // Inverse time distance traveled since last step event
uint32_t delta_d; // Inverse time distance traveled per interrupt tick
uint32_t d_per_tick;
int32_t counter_dist; // Inverse time distance traveled since last step event
uint32_t ramp_rate; // Inverse time distance traveled per interrupt tick
uint32_t dist_per_tick;
// Used by the stepper driver interrupt
uint8_t execute_step; // Flags step execution for each interrupt.
@ -65,7 +65,7 @@ typedef struct {
uint8_t out_bits; // The next stepping-bits to be output
uint8_t load_flag;
uint8_t ramp_count;
uint8_t counter_ramp;
uint8_t ramp_type;
} stepper_t;
static stepper_t st;
@ -76,7 +76,7 @@ static stepper_t st;
// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1).
typedef struct {
int32_t step_events_remaining; // Tracks step event count for the executing planner block
uint32_t d_next; // Scaled distance to next step
uint32_t dist_next_step; // Scaled distance to next step
uint32_t initial_rate; // Initialized step rate at re/start of a planner block
uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
@ -154,7 +154,6 @@ void st_wake_up()
TCNT2 = 0; // Clear Timer2
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
}
}
@ -184,8 +183,8 @@ void st_go_idle()
/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
on an inverse time stepper algorithm, where a timer ticks at a constant frequency and uses
time-distance counters to track when its the approximate time for a step event. For reference,
a similar inverse-time algorithm by Pramod Ranade is susceptible to numerical round-off,
meaning that some axes steps may not execute correctly for a given multi-axis motion.
a similar inverse-time algorithm by Pramod Ranade is susceptible to numerical round-off, as
described, meaning that some axes steps may not execute correctly for a given multi-axis motion.
Grbl's algorithm differs by using a single inverse time-distance counter to manage a
Bresenham line algorithm for multi-axis step events, which ensures the number of steps for
each axis are executed exactly. In other words, Grbl uses a Bresenham within a Bresenham
@ -208,6 +207,8 @@ void st_go_idle()
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.
- Create NOTE: to describe that the total time in this ISR must be less than the ISR frequency
in its worst case scenario.
*/
ISR(TIMER2_COMPA_vect)
{
@ -232,58 +233,47 @@ ISR(TIMER2_COMPA_vect)
// Anything in the buffer? If so, load and initialize next step segment.
if (segment_buffer_head != segment_buffer_tail) {
// NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is
// a small chance that this will load at the same time as a step event. Hopefully,
// the overhead for this loading event isn't too much.. possibly 2-5 usec.
// NOTE: The stepper algorithm must control the planner buffer tail as it completes
// the block moves. Otherwise, a feed hold can leave a few step buffer line moves
// without the correct planner block information.
// Initialize new step segment and load number of steps to execute
st_current_segment = &segment_buffer[segment_buffer_tail];
// Load number of steps to execute from stepper buffer
st.segment_steps_remaining = st_current_segment->n_step;
// Check if the counters need to be reset for a new planner block
// If the new segment starts a new planner block, initialize stepper variables and counters.
// NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous.
if (st.load_flag == LOAD_BLOCK) {
pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this.
st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index];
st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index];
// Initialize direction bits for block
// Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick.
st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask;
st.execute_step = true; // Set flag to set direction bits upon next ISR tick.
st.execute_step = true;
// Initialize Bresenham line counters
st.counter_x = (pl_current_block->step_event_count >> 1);
st.counter_y = st.counter_x;
st.counter_z = st.counter_x;
// Initialize inverse time and step rate counter data
st.counter_d = st_current_data->d_next; // d_next always greater than delta_d.
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, 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; // Initialize ramp counter via midpoint rule
st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary.
// }
// Initialize inverse time, step rate data, and acceleration ramp counters
st.counter_dist = st_current_data->dist_next_step; // dist_next_step always greater than ramp_rate.
st.ramp_rate = st_current_data->initial_rate;
st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule
st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary.
// Ensure the initial step rate exceeds the MINIMUM_STEP_RATE.
if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; }
else { st.dist_per_tick = st.ramp_rate; }
}
// Acceleration and cruise handled by ramping. Just check if deceleration needs to begin.
// Check if ramp conditions have changed. If so, update ramp counters and control variables.
if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_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 have been initialized
such that the following computation is still correct. */
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count;
as mandated by the mid-point rule. For the latter conditions, the ramp count have been
initialized such that the following computation is still correct. */
st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp;
if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; }
else { st.ramp_type = RAMP_ACCEL; }
}
@ -300,46 +290,36 @@ 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 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
if (st.ramp_type) { // Ignored when ramp type is RAMP_NOOP_CRUISE
st.counter_ramp--; // Tick acceleration ramp counter
if (st.counter_ramp == 0) { // Adjust step rate when its time
st.counter_ramp = 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_rate += st_current_data->rate_delta;
if (st.ramp_rate >= st_current_data->nominal_rate) { // Reached nominal rate.
st.ramp_rate = st_current_data->nominal_rate; // Set cruising velocity
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.
st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp change.
}
} 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;
if (st.ramp_rate > st_current_data->rate_delta) {
st.ramp_rate -= 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.
// 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;
st.ramp_rate >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
}
}
// Finalize adjusted step rate. Ensure minimum.
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
else { st.d_per_tick = st.delta_d; }
// Adjust for minimum step rate, but retain operating ramp rate for accurate velocity tracing.
if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; }
else { st.dist_per_tick = st.ramp_rate; }
}
}
// Iterate inverse time counter. Triggers each Bresenham step event.
st.counter_d -= st.d_per_tick;
st.counter_dist -= st.dist_per_tick;
// Execute Bresenham step event, when it's time to do so.
if (st.counter_d < 0) {
st.counter_d += st_current_data->d_next; // Reload inverse time counter
if (st.counter_dist < 0) {
st.counter_dist += st_current_data->dist_next_step; // Reload inverse time counter
st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits
st.execute_step = true;
@ -349,7 +329,6 @@ 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]++; }
}
@ -357,7 +336,6 @@ ISR(TIMER2_COMPA_vect)
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]++; }
}
@ -365,7 +343,6 @@ ISR(TIMER2_COMPA_vect)
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]++; }
}
@ -373,34 +350,6 @@ ISR(TIMER2_COMPA_vect)
// Check step events for trapezoid change or end of block.
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 & SEGMENT_END_OF_BLOCK) {
@ -412,7 +361,6 @@ ISR(TIMER2_COMPA_vect)
// Discard current segment by advancing buffer tail index
if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; }
}
st.out_bits ^= settings.invert_mask; // Apply step port invert mask
@ -486,6 +434,7 @@ void st_cycle_start()
{
if (sys.state == STATE_QUEUED) {
sys.state = STATE_CYCLE;
st_prep_buffer(); // Initialize step segment buffer before beginning cycle.
st_wake_up();
}
}
@ -520,8 +469,8 @@ void st_cycle_reinitialize()
// plan_cycle_reinitialize(st_current_data->step_events_remaining);
// st.ramp_type = RAMP_ACCEL;
// st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
// st.delta_d = 0;
// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2;
// st.ramp_rate = 0;
// sys.state = STATE_QUEUED;
// } else {
// sys.state = STATE_IDLE;
@ -580,6 +529,7 @@ void st_cycle_reinitialize()
*/
void st_prep_buffer()
{
if (sys.state != STATE_QUEUED) { // Block until a motion state is issued
while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
// Initialize new segment
@ -605,7 +555,7 @@ void st_prep_buffer()
st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining;
st_prep_data->rate_delta = last_st_prep_data->rate_delta;
st_prep_data->d_next = last_st_prep_data->d_next;
st_prep_data->dist_next_step = last_st_prep_data->dist_next_step;
st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this.
st_prep_data->mm_per_step = last_st_prep_data->mm_per_step;
@ -625,7 +575,7 @@ void st_prep_buffer()
st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
st_prep_data->rate_delta = ceil(pl_prep_block->acceleration*
((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
st_prep_data->dist_next_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
// TODO: Check if we really need to store this.
st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count;
@ -656,7 +606,7 @@ void st_prep_buffer()
// We do this to minimize memory and computational requirements. However, this could easily be replaced with
// a more exact approximation or have a unique time per segment, if CPU and memory overhead allows.
if (st_prep_data->decelerate_after <= 0) {
if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; }
if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } // Set segment deceleration flag
else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; }
if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; }
} else {
@ -669,9 +619,9 @@ void st_prep_buffer()
}
// Compute the number of steps in the prepped segment based on the approximate current rate.
// NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps.
// NOTE: The dist_next_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps.
prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)*
(ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next);
(ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_next_step);
// NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps
// a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed.
prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT);
@ -697,6 +647,11 @@ void st_prep_buffer()
// Check for end of planner block
if ( st_prep_data->step_events_remaining == 0 ) {
// TODO: When a feed hold ends, the step_events_remaining will also be zero, even though a block
// have partially been completed. We need to flag the stepper algorithm to indicate a stepper shutdown
// when complete, but not remove the planner block unless it truly is the end of the block (rare).
// Set EOB bitflag so stepper algorithm discards the planner block after this segment completes.
prep_segment->flag |= SEGMENT_END_OF_BLOCK;
// Move planner pointer to next block and flag to load a new block for the next segment.
@ -713,6 +668,7 @@ void st_prep_buffer()
// printString(" ");
}
}
}
uint8_t st_get_prep_block_index()