Updated comments. Changed stepper variable names to be more understandable. Added step locking note.
- 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:
164
stepper.c
164
stepper.c
@ -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()
|
||||
|
Reference in New Issue
Block a user