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

Fine tuning of new stepper algorithm with protected planner. Adaptive step prediction for segment buffer.

Browse Source 
- Cleaned up the new stepper algorithm code with more commenting and
better logic flow.

- The new segment buffer now predicts the number of steps each segment
should have to execute over about 8 milliseconds each (based on the
ACCELERATION_TICKS_PER_SECOND setting). So, for when the whole segment
buffer is full, the stepper algorithm has roughly 40 milliseconds of
steps queued before it needs to refilled by the main program.

- Readjusted the max supported step rate back to 30kHz from the lower
development 20kHz. Everything still works amazing great and the test
CNC machine still runs twice as fast with the new stepper algorithm and
planner.

- Upped the standard serial baudrate to 115200 baud, as it is clear
that the bottleneck is the serial interface. Will now support this, as
well as the old 9600 baud, in new firmware builds.
This commit is contained in:
Sonny Jeon 2013-10-14 21:21:56 -06:00
parent 8a10654b1c
commit 0cb5756b53
7 changed files with 122 additions and 1821 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
config.h
View File

@ -57,7 +57,7 @@
// interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but
// CPU overhead increases exponentially as this frequency goes up. So there will be little left for
// other processes like arcs.
#define ISR_TICKS_PER_SECOND 20000L // Integer (Hz)
#define ISR_TICKS_PER_SECOND 30000L // Integer (Hz)
// The temporal resolution of the acceleration management subsystem. Higher number give smoother
// acceleration but may impact performance. If you run at very high feedrates (>15kHz or so) and
@ -66,7 +66,7 @@
// is machine dependent, so it's advised to set this only as high as needed. Approximate successful
// values can widely range from 50 to 200 or more. Cannot be greater than ISR_TICKS_PER_SECOND/2.
// NOTE: Ramp count variable type in stepper module may need to be updated if changed.
#define ACCELERATION_TICKS_PER_SECOND 100L
#define ACCELERATION_TICKS_PER_SECOND 120L
// NOTE: Make sure this value is less than 256, when adjusting both dependent parameters.
#define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)

4
defaults.h
View File

@ -140,9 +140,9 @@
#define DEFAULT_Z_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV)
#define DEFAULT_STEP_PULSE_MICROSECONDS 10
#define DEFAULT_ARC_TOLERANCE 0.005 // mm
#define DEFAULT_RAPID_FEEDRATE 2500.0 // mm/min
#define DEFAULT_RAPID_FEEDRATE 4000.0 // mm/min
#define DEFAULT_FEEDRATE 1000.0 // mm/min
#define DEFAULT_ACCELERATION 150.0*60*60 // 150 mm/min^2
#define DEFAULT_ACCELERATION 400.0*60*60 // 150 mm/min^2
#define DEFAULT_JUNCTION_DEVIATION 0.05 // mm
#define DEFAULT_STEPPING_INVERT_MASK (1<<Y_DIRECTION_BIT)
#define DEFAULT_REPORT_INCHES 0 // false

3
planner.c
View File

@ -343,6 +343,9 @@ printString("x");
// Any acceleration detected in the forward pass automatically moves the optimal planned
// pointer forward, since everything before this is all optimal. In other words, nothing
// can improve the plan from the buffer tail to the planned pointer by logic.
// TODO: Need to check if the planned flag logic is correct for all scenarios. It may not
// be for certain conditions. However, if the block reaches nominal speed, it can be a valid
// breakpoint substitute.
if (current->entry_speed_sqr < next->entry_speed_sqr) {
entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
// If true, current block is full-acceleration and we can move the planned pointer forward.

226
stepper.c
View File

@ -37,9 +37,12 @@
#define LOAD_SEGMENT 1
#define LOAD_BLOCK 2
#define ST_END_OF_BLOCK bit(0)
#define ST_ACCEL bit(1)
#define ST_DECEL bit(2)
#define SEGMENT_NOOP 0
#define SEGMENT_END_OF_BLOCK bit(0)
#define SEGMENT_ACCEL bit(1)
#define SEGMENT_DECEL bit(2)
#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change
#define SEGMENT_BUFFER_SIZE 6
@ -67,22 +70,25 @@ typedef struct {
} stepper_t;
static stepper_t st;
// Stores stepper buffer common data for a planner block. Data can change mid-block when the planner
// updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs.
// NOTE: Normally, this buffer is only partially used, but can fill up completely in certain conditions.
// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the
// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs.
// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed
// 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 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)
int32_t decelerate_after;
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 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)
uint32_t current_approx_rate; // Tracks the approximate segment rate to predict steps per segment to execute
int32_t decelerate_after; // Tracks when to initiate deceleration according to the planner block
float mm_per_step;
} st_data_t;
static st_data_t segment_data[SEGMENT_BUFFER_SIZE];
static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1];
// Primary stepper buffer. Contains small, short line segments for the stepper algorithm to execute checked
// out incrementally from the first block in the planner buffer. These step segments
// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute,
// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps
// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can.
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.
@ -90,6 +96,7 @@ typedef struct {
} st_segment_t;
static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
// Step segment ring buffer indices
static volatile uint8_t segment_buffer_tail;
static volatile uint8_t segment_buffer_head;
static uint8_t segment_next_head;
@ -101,13 +108,11 @@ static st_data_t *st_current_data;
// Pointers for the step segment being prepped from the planner buffer. Accessed only by the
// main program. Pointers may be planning segments or planner blocks ahead of what being executed.
static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer
static uint8_t pl_prep_index;
static st_data_t *st_prep_data;
static uint8_t st_data_prep_index;
static uint8_t pl_partial_block_flag;
static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped
static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped
static uint8_t pl_prep_index; // Index of planner block being prepped
static uint8_t st_data_prep_index; // Index of stepper common data block being prepped
static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block
// Returns the index of the next block in the ring buffer
@ -194,21 +199,24 @@ void st_go_idle()
/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
on the Pramod Ranade 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 any
step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
meaning that some axes steps may not execute for a given multi-axis motion.
Grbl's algorithm slightly differs by using a single Ranade 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, it uses a Bresenham within a Bresenham algorithm,
where one tracks time(Ranade) and the other steps.
This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the
stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper
outputs simultaneously with these two interrupts.
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.
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
algorithm, where one tracks time for step events and the other steps for multi-axis moves.
Grbl specifically uses the Bresenham algorithm due to its innate mathematical exactness and
low computational overhead, requiring simple integer +,- counters only.
This interrupt pops blocks from the step segment buffer and executes them by pulsing the
stepper pins appropriately. It is supported by The Stepper Port Reset Interrupt which it uses
to reset the stepper port after each pulse. The bresenham line tracer algorithm controls all
three stepper outputs simultaneously with these two interrupts.
*/
/* TODO:
- Measure time in ISR. Typical and worst-case.
- Measure time in ISR. Typical and worst-case. Should be virtually identical to last algorithm.
There are no major changes to the base operations of this ISR with the new segment buffer.
- 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.
@ -278,23 +286,22 @@ ISR(TIMER2_COMPA_vect)
// 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
if (st.delta_d == st_current_data->nominal_rate) { st.ramp_type = RAMP_NOOP_CRUISE; }
else { st.ramp_type = RAMP_ACCEL; }
st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary.
// }
}
// Acceleration and cruise handled by ramping. Just check if deceleration needs to begin.
if ( st_current_segment->flag & (ST_DECEL | ST_ACCEL) ) {
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 has been pre-initialized
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;
if ( st_current_segment->flag & ST_DECEL ) { st.ramp_type = RAMP_DECEL; }
if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; }
else { st.ramp_type = RAMP_ACCEL; }
}
@ -415,7 +422,7 @@ ISR(TIMER2_COMPA_vect)
// 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) {
if (st_current_segment->flag & SEGMENT_END_OF_BLOCK) {
plan_discard_current_block();
st.load_flag = LOAD_BLOCK;
} else {
@ -434,8 +441,8 @@ ISR(TIMER2_COMPA_vect)
}
// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the
// step pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the step
// pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
// finish, if Timer2 is disabled after completing a move.
ISR(TIMER0_OVF_vect)
{
@ -569,51 +576,61 @@ void st_cycle_reinitialize()
events like deceleration initialization and end of block.
*/
// !!! Need to make sure when a single partially completed block can be re-computed here with
// new deceleration point and the segment manager begins accelerating again immediately.
/*
TODO: Figure out how to enforce a deceleration when a feedrate override is reduced.
The problem is that when an override is reduced, the planner may not plan back to
the current rate. Meaning that the velocity profiles for certain conditions no longer
are trapezoidal or triangular. For example, if the current block is cruising at a
nominal rate and the feedrate override is reduced, the new nominal rate will now be
lower. The velocity profile must first decelerate to the new nominal rate and then
follow on the new plan. So the remaining velocity profile will have a decelerate,
cruise, and another decelerate.
Another issue is whether or not a feedrate override reduction causes a deceleration
that acts over several planner blocks. For example, say that the plan is already
heavily decelerating throughout it, reducing the feedrate will not do much to it. So,
how do we determine when to resume the new plan? How many blocks do we have to wait
until the new plan intersects with the deceleration curve? One plus though, the
deceleration will never be more than the number of blocks in the entire planner buffer,
but it theoretically can be equal to it when all planner blocks are decelerating already.
*/
void st_prep_buffer()
{
while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
// Initialize new segment
st_segment_t *prep_segment = &segment_buffer[segment_buffer_head];
prep_segment->flag = 0;
prep_segment->flag = SEGMENT_NOOP;
// 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 for the new planner block.
// Increment stepper common data buffer index
if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; }
// Check if the planner has re-computed this block mid-execution. If so, push the previous segment
// data. Otherwise, prepare a new segment 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.
st_data_t *last_st_prep_data;
last_st_prep_data = &segment_data[st_data_prep_index];
st_data_prep_index = next_block_index(st_data_prep_index);
last_st_prep_data = st_prep_data;
st_prep_data = &segment_data[st_data_prep_index];
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->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Recompute with feedrate overrides.
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;
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,
// we must reload the initial rate data, such that the velocity profile is re-constructed correctly.
// The stepper algorithm must be flagged to adjust the acceleration counters.
} else {
// Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since
// the planner buffer can dynamically change the velocity profile data as blocks are added.
st_data_prep_index = next_block_index(st_data_prep_index);
st_prep_data = &segment_data[st_data_prep_index];
// Initialize Bresenham variables
@ -636,81 +653,64 @@ void st_prep_buffer()
// TODO: Nominal rate changes with feedrate override.
// 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)
// Calculate the planner block velocity profile type and determine deceleration point.
st_prep_data->current_approx_rate = st_prep_data->initial_rate;
// Calculate the planner block velocity profile type, determine deceleration point, and initial ramp.
float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index);
st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step );
if (st_prep_data->decelerate_after > 0) { // If 0, SEGMENT_DECEL flag is set later.
if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = SEGMENT_ACCEL; }
}
}
/*
TODO: Need to check for a planner flag to indicate a change to this planner block.
If so, need to check for a change in acceleration state, from deceleration to acceleration,
to reset the stepper ramp counters and the initial_rate data to trace the new
ac/de-celeration profile correctly.
No change conditions:
- From nominal speed to acceleration from feedrate override
- From nominal speed to new deceleration.
- From acceleration to new deceleration point later or cruising point.
- From acceleration to immediate deceleration? Can happen during feedrate override
and slowing down, but likely ok by enforcing the normal ramp counter protocol.
Change conditions:
- From deceleration to acceleration, i.e. common with jogging when new blocks are added.
*/
// Set new segment to point to the current segment data block.
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
// For the most part, we can store the acceleration portion in the st_data buffer and all
// we would need to do is track the current approximate speed per loop with: v = u + a*t
// Each loop would require 3 multiplication and 2 additions, since most of the variables
// are constants and would get compiled out.
// Approximate the velocity over the new segment
if (st_prep_data->decelerate_after <= 0) {
if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; }
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 {
if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) {
st_prep_data->current_approx_rate += st_prep_data->rate_delta;
if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) {
st_prep_data->current_approx_rate = st_prep_data->nominal_rate;
}
}
}
//!!! Doesn't work as is. Requires last_velocity and acceleration in terms of steps, not mm.
// prep_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step);
// if (st_prep_data->decelerate_after > 0) {
// prep_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step);
// } else {
// prep_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 = 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.
// Compute the number of steps in the prepped segment based on the approximate current rate. The execution
// time of each segment should be about every ACCELERATION_TICK.
// NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps.
// NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255.
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);
prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); // 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 (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.
}
// Check if n_step exceeds decelerate point in block. Need to perform this so that the
// 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.
// Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration
// ramp counters are set correctly during execution.
if (st_prep_data->decelerate_after > 0) {
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) {
// // !!! Doesn't work since distance changes and gets truncated.
// last_velocity += pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); // In acceleration ramp.
// if {last_velocity > last_nominal_v) { last_velocity = last_nominal_v; } // Set to cruising.
// }
// } 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->decelerate_after -= prep_segment->n_step;
st_prep_data->step_events_remaining -= prep_segment->n_step;
// Check for end of planner block
if ( st_prep_data->step_events_remaining == 0 ) {
prep_segment->flag |= ST_END_OF_BLOCK;
// 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.
pl_prep_index = next_block_pl_index(pl_prep_index);
pl_prep_block = NULL;
@ -719,6 +719,10 @@ void st_prep_buffer()
// New step segment completed. Increment segment buffer indices.
segment_buffer_head = segment_next_head;
segment_next_head = next_block_index(segment_buffer_head);
// long a = prep_segment->n_step;
// printInteger(a);
// printString(" ");
}
}

601
stepper_new2.c
View File

@ -1,601 +0,0 @@
/*
stepper.c - stepper motor driver: executes motion plans using stepper motors
Part of Grbl
Copyright (c) 2011-2013 Sungeun K. Jeon
Copyright (c) 2009-2011 Simen Svale Skogsrud
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#include <avr/interrupt.h>
#include "stepper.h"
#include "config.h"
#include "settings.h"
#include "planner.h"
#include "nuts_bolts.h"
// Some useful constants
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
#define CRUISE_RAMP 0
#define ACCEL_RAMP 1
#define DECEL_RAMP 2
#define LOAD_NOOP 0
#define LOAD_LINE 1
#define LOAD_BLOCK 2
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
// Used by the bresenham line algorithm
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z;
int32_t step_events_remaining; // Steps remaining in line motion
// Used by inverse time algorithm
int32_t delta_d; // Inverse time distance traveled per interrupt tick
int32_t d_counter; // Inverse time distance traveled since last step event
int32_t d_per_tick;
// Used by the stepper driver interrupt
uint8_t execute_step; // Flags step execution for each interrupt.
uint8_t step_pulse_time; // Step pulse reset time after step rise
uint8_t out_bits; // The next stepping-bits to be output
uint8_t load_flag;
} stepper_t;
static stepper_t st;
#define STEPPER_BUFFER_SIZE 5
typedef struct {
int32_t event_count;
int32_t rate;
uint8_t end_of_block;
uint8_t tick_count;
int32_t initial_rate; // The step rate at start of block
int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
int32_t decelerate_after; // The index of the step event on which to start decelerating
int32_t nominal_rate; // The nominal step rate for this block in step_events/minute
int32_t d_next; // Scaled distance to next step
} stepper_buffer_t;
static stepper_buffer_t step_buffer[STEPPER_BUFFER_SIZE];
static volatile uint8_t step_buffer_tail;
static uint8_t step_buffer_head;
static uint8_t step_next_head;
// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then
// this blocking variable is no longer needed. Only here for safety reasons.
static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler.
static plan_block_t *plan_current_block; // A pointer to the planner block currently being traced
/* __________________________
/| |\ _________________ ^
/ | | \ /| |\ |
/ | | \ / | | \ s
/ | | | | | \ p
/ | | | | | \ e
+-----+------------------------+---+--+---------------+----+ e
| BLOCK 1 | BLOCK 2 | d
time ----->
The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
+/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
*/
// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
// enabled. Startup init and limits call this function but shouldn't start the cycle.
void st_wake_up()
{
// Enable steppers by resetting the stepper disable port
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
}
if (sys.state == STATE_CYCLE) {
// Initialize stepper output bits
st.out_bits = settings.invert_mask;
// Initialize step pulse timing from settings.
st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
// Enable stepper driver interrupt
st.execute_step = false;
TCNT2 = 0; // Clear Timer2
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
}
}
// Stepper shutdown
void st_go_idle()
{
// Disable stepper driver interrupt. Allow Timer0 to finish. It will disable itself.
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt
TCCR2B = 0; // Disable Timer2
busy = false;
// Disable steppers only upon system alarm activated or by user setting to not be kept enabled.
if ((settings.stepper_idle_lock_time != 0xff) || bit_istrue(sys.execute,EXEC_ALARM)) {
// Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
// stop and not drift from residual inertial forces at the end of the last movement.
delay_ms(settings.stepper_idle_lock_time);
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
}
}
}
/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
on the Pramod Ranade 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 any
step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
meaning that some axes steps may not execute for a given multi-axis motion.
Grbl's algorithm slightly differs by using a single Ranade 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, it uses a Bresenham within a Bresenham algorithm,
where one tracks time(Ranade) and the other steps.
This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the
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.
ISR(TIMER2_COMPA_vect)
{
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
// Pulse stepper port pins, if flagged. New block dir will always be set one timer tick
// before any step pulse due to algorithm design.
if (st.execute_step) {
st.execute_step = false;
STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | st.out_bits;
TCNT0 = st.step_pulse_time; // Reload Timer0 counter.
TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
}
busy = true;
sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time.
// NOTE: The remaining code in this ISR will finish before returning to main program.
// This loading step is important. Allows the stepper ISR to only handle its own variables
// hence no volatiles needed. Otherwise a preloading step is required by the main program
// or some other means to get the line motions started, and volatile would be required.
// If there is no current block, attempt to pop one from the buffer
if (st.load_flag != LOAD_NOOP) {
// Anything in the buffer? If so, initialize next motion.
if (step_buffer_head != step_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.
// Load line motion from stepper buffer
st.step_events_remaining = step_buffer[step_buffer_tail].event_count;
st.delta_d = step_buffer[step_buffer_tail].rate;
// Check if the counters need to be reset
if (st.load_flag == LOAD_BLOCK) {
plan_current_block = plan_get_current_block();
// Initialize direction bits for block
st.out_bits = plan_current_block->direction_bits ^ settings.invert_mask;
st.execute_step = true; // Set flag to set direction bits.
st.counter_x = (plan_current_block->step_event_count >> 1);
st.counter_y = st.counter_x;
st.counter_z = st.counter_x;
// This is correct. Sets the total time before the next step occurs.
st.counter_d = plan_current_block->d_next; // d_next always greater than delta_d.
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
}
st.load_flag = LOAD_NOOP; // Line motion loaded. Set no-operation flag until complete.
} else {
// Can't discard planner block here if a feed hold stops in middle of block.
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
return; // Nothing to do but exit.
}
}
// Iterate inverse time counter. Triggers each Bresenham step event.
st.counter_d -= st.delta_d;
// Execute Bresenham step event, when it's time to do so.
if (st.counter_d < 0) {
st.counter_d += plan_current_block->d_next; // Reload inverse time counter
st.out_bits = plan_current_block->direction_bits; // Reset out_bits and reload direction bits
st.execute_step = true;
// Execute step displacement profile by Bresenham line algorithm
st.counter_x -= plan_current_block->steps[X_AXIS];
if (st.counter_x < 0) {
st.out_bits |= (1<<X_STEP_BIT);
st.counter_x += plan_current_block->step_event_count;
if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; }
}
st.counter_y -= plan_current_block->steps[Y_AXIS];
if (st.counter_y < 0) {
st.out_bits |= (1<<Y_STEP_BIT);
st.counter_y += plan_current_block->step_event_count;
if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
else { sys.position[Y_AXIS]++; }
}
st.counter_z -= plan_current_block->steps[Z_AXIS];
if (st.counter_z < 0) {
st.out_bits |= (1<<Z_STEP_BIT);
st.counter_z += plan_current_block->step_event_count;
if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
else { sys.position[Z_AXIS]++; }
}
// Check step events for trapezoid change or end of block.
st.step_events_remaining--; // Decrement step events count
// Tracking step events may not be the right thing. Need to track time instead.
// If a step completes and the inverse counters are reset, then the line motion time
// to execute gets truncated. Thus, screwing up the acceleration.
// Time can change at the first block, a transition point or end of block. It is not constant.
// Time must be truncated when the last step in the block is executed. Ensures there are
// no step phase issues with the following block. Or, the ISR timer count must be computed
// to be exact, such that the last step occurs correctly.
// This should always/automatically occur for a true trapezoid. The first acceleration
// should be 1/2 accel_tick and the last decel should also. The difference is taken up by
// the transition from cruise to decel.
// For a cruising entry, the transition block takes care of incompatibility.
// For a cruising exit, the exit block needs to truncate. Especially when the cruising
// continues onward to the next block.
if (st.step_events_remaining == 0) {
// 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 (step_buffer[step_buffer_tail].end_of_block) {
plan_discard_current_block();
st.load_flag = LOAD_BLOCK;
} else {
st.load_flag = LOAD_LINE;
}
// Discard current block
if (step_buffer_head != step_buffer_tail) {
step_buffer_tail = next_block_index( step_buffer_tail );
}
// NOTE: sys.position updates could be done here. The bresenham counters can have
// their own incremental counters. Here we would check the direction and apply it
// to sys.position accordingly. However, this could take some time.
}
st.out_bits ^= settings.invert_mask; // Apply step port invert mask
}
busy = false;
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
}
// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the
// step pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
// finish, if Timer2 is disabled after completing a move.
ISR(TIMER0_OVF_vect)
{
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK);
TCCR0B = 0; // Disable timer until needed.
}
// Reset and clear stepper subsystem variables
void st_reset()
{
memset(&st, 0, sizeof(st));
st.load_flag = LOAD_BLOCK;
busy = false;
}
// Initialize and start the stepper motor subsystem
void st_init()
{
// Configure directions of interface pins
STEPPING_DDR |= STEPPING_MASK;
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
// Configure Timer 2
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt while configuring it
TCCR2B = 0; // Disable Timer2 until needed
TCNT2 = 0; // Clear Timer2 counter
TCCR2A = (1<<WGM21); // Set CTC mode
OCR2A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
// Configure Timer 0
TIMSK0 &= ~(1<<TOIE0);
TCCR0A = 0; // Normal operation
TCCR0B = 0; // Disable Timer0 until needed
TIMSK0 |= (1<<TOIE0); // Enable overflow interrupt
// Start in the idle state, but first wake up to check for keep steppers enabled option.
st_wake_up();
st_go_idle();
}
// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
// by the main program functions: planner auto-start and run-time command execution.
void st_cycle_start()
{
if (sys.state == STATE_QUEUED) {
sys.state = STATE_CYCLE;
st_wake_up();
}
}
// Execute a feed hold with deceleration, only during cycle. Called by main program.
void st_feed_hold()
{
if (sys.state == STATE_CYCLE) {
sys.state = STATE_HOLD;
sys.auto_start = false; // Disable planner auto start upon feed hold.
}
}
// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by
// runtime command execution in the main program, ensuring that the planner re-plans safely.
// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
// Only the planner de/ac-celerations profiles and stepper rates have been updated.
void st_cycle_reinitialize()
{
if (current_block != NULL) {
// Replan buffer from the feed hold stop location.
plan_cycle_reinitialize(st.step_events_remaining);
st.ramp_type = ACCEL_RAMP;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
st.delta_d = 0;
sys.state = STATE_QUEUED;
} else {
sys.state = STATE_IDLE;
}
}
// Preps stepper buffer. Called from main program. Sends short line segments of constant
// velocity to the stepper driver. Ac/de-celeration calculations are performed here.
// NOTE: Line motions can range from 1-300
// TODO: Could be very easy to install adaptive Bresenham resolution with this prep function.\
That is, if the acceleration calculations are performed here rather than in the ISR.
void st_prep_buffer()
{
while (st.buffer_tail != st.next_head) { // Check if we need to fill the buffer.
step_block_t *step_block = &step_buffer[st.buffer_head];
// Determine if we need to load a new planner block.
if (plan_current_block == NULL) {
plan_current_block = plan_get_current_block();
if (plan_current_block == NULL) { return; } // No more planner blocks. Let stepper finish out.
// Initialize Bresenham variables
step_buffer[st.buffer_head] = plan_current_block->step_event_count;
//st.step_events_remaining = st.event_count;
// Convert new block to stepper variables.
// NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must
// be maintained as these execute.
// TODO: The initial rate needs to be sent back to the planner to update the entry speed
block->initial_rate = ceil(sqrt(plan_current_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
block->nominal_rate = ceil(plan_current_block->nominal_speed*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// This data doesn't change. Could be performed in the planner, but fits nicely here.
// Although, acceleration can change for S-curves. So keep it here.
block->rate_delta = ceil(plan_current_block->acceleration*
((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
// This definitely doesn't change, but could be precalculated in a way to help some of the
// math in this handler, i.e. millimeters per step event data.
block->d_next = ceil((plan_current_block->millimeters*INV_TIME_MULTIPLIER)/plan_current_block->step_event_count); // (mult*mm/step)
// During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating.
if (sys.state == STATE_CYCLE) { }
}
// Track instead the trapezoid line and use the average of the entry and exit velocities
// to determine step rate. This should take care of the deceleration issue automatically...
// i think.
// First need to figure out what type of profile is segment is, i.e. acceleration only, accel
// to decel triangle, cruise to decel, or all three. May need more profile data to compute this
// from the planner itself, like accelerate until.
// Another issue. This is only tracking the velocity profile, not the distance covered over that
// time period. This can lead to an unsynchronized velocity profile and steps executed. But,
// how much drift is there really? Enough to be a problem? Not sure. I would think typical
// drift would be on the order of a few steps, more depending on step resolution.
entry_rate = last_exit_rate;
time = 250 ISR ticks per acceleration tick.
distance = 0;
if (distance_traveled < accelerate_until)
exit_rate = entry_rate + acceleration*time;
if (exit_rate > nominal_rate) {
exit_rate = nominal_rate;
time = 2*(accelerate_until-distance_travel)/(entry_rate+nominal_rate);
// distance = accelerate_until; // Enforce distance?
// Truncate this segment.
}
} else if (distance_traveled >= decelerate_after) {
if (accelerate_until == decelerate_after) {
time = last time;
exit_rate = entry_rate;
} else {
exit_rate = entry_rate - acceleration*time;
}
} else {
exit_rate = nominal_rate; // Just cruise
distance = nominal_rate*time;
if (distance > decelerate_after) { // Truncate segment at nominal rate.
time = (decelerate_after-distance_traveled)/(nominal_rate);
distance = decelerate_after;
}
}
mean_rate = 0.5*(entry_rate+exit_rate);
distance = mean_rate*time;
if (entry_rate < nominal_rate) {
if (entry_distance < decelerate_after) { // Acceleration case
exit_rate = entry_rate + acceleration*time
exit_rate = min(exit_rate,nominal_rate);
mean_rate = 0.5*(entry_rate + exit_rate);
distance = mean_rate*time;
if (distance > decelerate_after) {
exit_rate =
// If the MINIMUM_STEP_RATE is less than ACCELERATION_TICKS_PER_SECOND then there can be
// rate adjustements that have less than one step per tick.
// How do you deal with the remainer?
time = 250 ISR ticks per acceleration tick. (30000/120)
delta_d*time // mm per acceleration tick
delta_d*time/d_next // number of steps/acceleration_tick. Chance of integer overflow.
delta_d*time/d_next + last_remainder. // steps/acceleration_tick.
n_step*d_next/delta_d // number of ISR ticks for enforced n_steps.
// In floating point? Then convert?
// Requires exact millimeters. Roundoff might be a problem. But could be corrected by just
// checking if the total step event counts are performed.
// Could be limited by float conversion and about 1e7 steps per block.
line_mm = feed_rate / acc_tick // mm per acc_tick
n_steps = floor(line_mm * step_event_remaining/millimeters_remaining) // steps. float 7.2 digits|int32 10 digits
millimeters_remaining -= line_mm;
step_events_remaining -= n_steps;
// There doesn't seem to be a way to avoid this divide here.
line_mm = feed_rate / acc_tick // mm per acc_tick
n_steps = floor( (line_mm+line_remainder) * step_event_count/millimeters) // steps. float 7.2 digits|int32 10 digits
line_remainder = line_mm - n_steps*(millimeters/step_event_count);
// Need to handle when rate is very very low, i.e. less than one step per accel tick.
// Could be bounded by MINIMUM_STEP_RATE.
// 1. Figure out how many steps occur exactly within n ISR ticks.
// 2. Account for step-time remainder for next line motion exactly.
// 3. At the end of block, determine exact number of ISR ticks to finish the steps. Or,\
have the ISR track steps to exit on time. It would require an extra counter.
// NOTE: There doesn't seem to be a great way to figure out how many steps occur within
// a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a
// critical problem. So, either numerical round-off checks could be made to account for
// them, while CPU overhead could be minimized in some way, or we can flip the algorithm
// around to have the stepper algorithm track number of steps over an indeterminant amount
// of time instead.
// In other words, we use the planner velocity floating point data to get an estimate of
// the number of steps we want to execute. We then back out the approximate velocity for
// the planner to use, which should be much more robust to round-off error. The main problem
// now is that we are loading the stepper algorithm to handle acceleration now, rather than
// pre-calculating with the main program. This approach does make sense in the way that
// planner velocities and stepper profiles can be traced more accurately.
// Which is better? Very hard to tell. The time-based algorithm would be able to handle
// Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would
// require some additional math in the stepper algorithm to adjust on the fly, plus adaptation
// would occur in a non-deterministic manner.
// I suppose it wouldn't hurt to build both to see what's better. Just a lot more work.
feed_rate/120 = millimeters per acceleration tick
steps?
d_next // (mult*mm/step)
rate // (mult*mm/isr_tic)
rate/d_next // step/isr_tic
if (plan_current_block->step_events_remaining <= plan_current_block->decelerate_after) {
// Determine line segment velocity and associated inverse time counter.
if (step_block.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration
step_block.delta_d += plan_current_block->rate_delta;
if (step_block.delta_d >= plan_current_block->nominal_rate) { // Reached cruise state.
step_block.ramp_type = CRUISE_RAMP;
step_block.delta_d = plan_current_block->nominal_rate; // Set cruise velocity
}
}
} else { // Adjust velocity for deceleration
if (step_block.delta_d > plan_current_block->rate_delta) {
step_block.delta_d -= plan_current_block->rate_delta;
} else {
step_block.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
}
}
// Incorrect. Can't overwrite delta_d. Needs to override instead.
if (step_block.delta_d < MINIMUM_STEP_RATE) { step_block.delta_d = MINIMUM_STEP_RATE; }
/* - Compute the number of steps needed to complete this move over the move time, i.e.
ISR_TICKS_PER_ACCELERATION_TICK.
- The first block in the buffer is half of the move time due to midpoint rule.
- Check if this reaches the deceleration after location. If so, truncate move. Also,
if this is a triangle move, double the truncated move to stay with midpoint rule.
NOTE: This can create a stepper buffer move down to just one step in length.
- Update the planner block entry speed for the planner to compute from end of the
stepper buffer location.
- If a feed hold occurs, begin to enforce deceleration, while enforcing the above rules.
When the deceleration is complete, all we need to do is update the planner block
entry speed and force a replan.
*/
// Planner block move completed.
// TODO: planner buffer tail no longer needs to be volatile. only accessed by main program.
if (st.step_events_remaining == 0) {
plan_current_block = NULL; // Set flag that we are done with this planner block.
plan_discard_current_block();
}
step_buffer_head = step_next_head;
step_next_head = next_block_index(step_buffer_head);
}
}

425
stepper_new_dual_ISR.c
View File

@ -1,425 +0,0 @@
/*
stepper.c - stepper motor driver: executes motion plans using stepper motors
Part of Grbl
Copyright (c) 2011-2013 Sungeun K. Jeon
Copyright (c) 2009-2011 Simen Svale Skogsrud
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#include <avr/interrupt.h>
#include "stepper.h"
#include "config.h"
#include "settings.h"
#include "planner.h"
// Some useful constants
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
#define CRUISE_RAMP 0
#define ACCEL_RAMP 1
#define DECEL_RAMP 2
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
// Used by the bresenham line algorithm
int32_t counter[N_AXIS]; // Counter variables for the bresenham line tracer
uint32_t event_count; // Total event count. Retained for feed holds.
uint32_t step_events_remaining; // Steps remaining in motion
// Used by Pramod Ranade inverse time algorithm
int32_t delta_d; // Ranade distance traveled per interrupt tick
int32_t d_counter; // Ranade distance traveled since last step event
uint8_t ramp_count; // Acceleration interrupt tick counter.
uint8_t ramp_type; // Ramp type variable.
uint8_t execute_step; // Flags step execution for each interrupt.
} stepper_t;
static stepper_t st;
static block_t *current_block; // A pointer to the block currently being traced
// Used by the stepper driver interrupt
static uint8_t step_pulse_time; // Step pulse reset time after step rise
static uint8_t out_bits; // The next stepping-bits to be output
// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then
// this blocking variable is no longer needed. Only here for safety reasons.
static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler.
// __________________________
// /| |\ _________________ ^
// / | | \ /| |\ |
// / | | \ / | | \ s
// / | | | | | \ p
// / | | | | | \ e
// +-----+------------------------+---+--+---------------+----+ e
// | BLOCK 1 | BLOCK 2 | d
//
// time ----->
//
// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
// enabled. Startup init and limits call this function but shouldn't start the cycle.
void st_wake_up()
{
// Enable steppers by resetting the stepper disable port
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
}
if (sys.state == STATE_CYCLE) {
// Initialize stepper output bits
out_bits = settings.invert_mask;
// Initialize step pulse timing from settings.
step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
// Enable stepper driver interrupt
st.execute_step = false;
TCNT0 = 0; // Clear Timer2
TIMSK0 |= (1<<OCIE0A); // Enable Timer0 Compare Match A interrupt
TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
}
}
// Stepper shutdown
void st_go_idle()
{
// Disable stepper driver interrupt. Allow Timer2 to finish. It will disable itself.
TIMSK0 &= ~(1<<OCIE0A); // Disable Timer0 interrupt
TCCR0B = 0; // Disable Timer0
busy = false;
// Disable steppers only upon system alarm activated or by user setting to not be kept enabled.
if ((settings.stepper_idle_lock_time != 0xff) || bit_istrue(sys.execute,EXEC_ALARM)) {
// Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
// stop and not drift from residual inertial forces at the end of the last movement.
delay_ms(settings.stepper_idle_lock_time);
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
}
}
}
/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
on the Pramod Ranade 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 any
step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
meaning that some axes steps may not execute/cause a phasing drift error between multiple axes.
Grbl's algorithm differs by using a single Ranade-type 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 and always in phase by inherent algorithm design. In other
words, it uses a Bresenham within a Bresenham algorithm, where one tracks time(Ranade) and
the other steps.
This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the
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.
ISR(TIMER0_COMPA_vect)
{
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
// Pulse stepper port pins, if flagged. New block dir will always be set one timer tick
// before any step pulse due to algorithm design.
if (st.execute_step) {
st.execute_step = false;
STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | out_bits;
TCNT2 = step_pulse_time; // Reload Timer2 counter.
TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
}
// Assume that this takes less than 5 usec. If not, then this might not work on an 328p.
// Two sei() commands in two different interrupts will be hard to manage. If the main program
// can push fast enough, then this might be ok.
// sei(); // ??? The falling edge interrupt needs to fire before the rest of this executes.
/*
1. Upon start, load segment/block.
- Set direction bit for entire block early. This never changes.
- Load Bresenham variables. Initialize their counters.
- If using segments, counters cannot be updated, but this breaks the direction bit? No. Only set when block begins.
(3) Generate step event. Can take up to an additional 10-15usec for the math.
Override idea: Main program can request the step event count from the stepper algorithm, which will
check for the request and write it to a safe variable for the main program. The main program will
then wait until the request is fulfilled via a flag. From there, the main program can determine
the safe point from which it can plan. This may require a snapshot of variables. Hopefully this
won't take too much time in the interrupt.
*/
// Iterate inverse time counter. Triggers each Bresenham step event.
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; }
else { st.d_counter -= st.delta_d; }
// Prepare Bresenham step event, when it's time to do so.
if (st.d_counter < 0) {
st.d_counter += current_block->d_next;
st.execute_step = true;
// Configure next step
out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits
// Execute step displacement profile by Bresenham line algorithm
st.counter[X_AXIS] -= current_block->steps_x; // Doesn't change when set up.
if (st.counter[X_AXIS] < 0) {
out_bits |= (1<<X_STEP_BIT);
st.counter[X_AXIS] += st.event_count;
st.n_step[X_AXIS]; // Track number of steps
}
st.counter[Y_AXIS] -= current_block->steps_y;
if (st.counter[Y_AXIS] < 0) {
out_bits |= (1<<Y_STEP_BIT);
st.counter[Y_AXIS] += st.event_count;
st.n_step[Y_AXIS]++;
}
st.counter[Z_AXIS] -= current_block->steps_z;
if (st.counter[Z_AXIS] < 0) {
out_bits |= (1<<Z_STEP_BIT);
st.counter[Z_AXIS] += st.event_count;
st.n_step[Z_AXIS]++;
}
// Check step events for trapezoid change or end of block.
st.step_events_remaining--; // Decrement step events count
if (st.step_events_remaining == 0) {
// Load next line motion
}
out_bits ^= settings.invert_mask; // Apply step port invert mask
// TIMSK2 |= (1<<OCIE2B); // Enable Timer2 Compare Match B interrupt
}
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
}
// This needs to complete and load before the next timer?
ISR(TIMER0_COMPB_vect)
{
if (busy) { return; }
busy = true;
TIMSK0 &= ~(1<<OCIE0B); // Disable Timer2 Compare Match B interrupt
sei();
if (out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; }
// If current block is finished, reset pointer
current_block = NULL;
plan_discard_current_block();
// If there is no current block, attempt to pop one from the buffer
if (current_block == NULL) {
// Anything in the buffer? If so, initialize next motion.
current_block = plan_get_current_block();
if (current_block != NULL) {
// By algorithm design, the loading of the next block never coincides with a step event,
// since there is always one inverse time tick before a step event occurs. This means
// that the Bresenham counter math never is performed at the same time as the loading
// of a block, hence helping minimize total time spent in this interrupt. Also, this
// allows the direction bits for the block to be always set one timer tick before the
// first step event.
// Initialize direction bits for block
out_bits = current_block->direction_bits ^ settings.invert_mask;
st.execute_step = true; // Set flag to set direction bits.
// Initialize Bresenham variables
st.counter_x = (current_block->step_event_count >> 1);
st.counter_y = st.counter_x;
st.counter_z = st.counter_x;
st.event_count = current_block->step_event_count;
st.step_events_remaining = st.event_count;
// During feed hold, do not update inverse time counter, rate, or ramp type. Keep decelerating.
if (sys.state == STATE_CYCLE) {
// Initialize Ranade variables
st.d_counter = current_block->d_next;
st.delta_d = current_block->initial_rate;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
// Initialize ramp type.
if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; }
else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; }
else { st.ramp_type = ACCEL_RAMP; }
}
} else {
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
return; // Nothing to do but exit.
}
}
// Adjust inverse time counter for ac/de-celerations
if (st.ramp_type) {
// Tick acceleration ramp counter
st.ramp_count--;
if (st.ramp_count == 0) {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration
st.delta_d += current_block->rate_delta;
if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state.
st.ramp_type = CRUISE_RAMP;
st.delta_d = current_block->nominal_rate; // Set cruise velocity
}
} else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration
if (st.delta_d > current_block->rate_delta) {
st.delta_d -= current_block->rate_delta;
} else {
st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
}
}
}
}
// Check for feed hold state and execute accordingly.
if (sys.state == STATE_HOLD) {
if (st.ramp_type != DECEL_RAMP) {
st.ramp_type = DECEL_RAMP;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
}
if (st.delta_d <= current_block->rate_delta) {
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP);
return;
}
}
if (st.ramp_type != DECEL_RAMP) {
// Acceleration and cruise handled by ramping. Just check for deceleration.
if (st.step_events_remaining <= current_block->decelerate_after) {
st.ramp_type = DECEL_RAMP;
if (st.step_events_remaining == current_block->decelerate_after) {
if (st.delta_d == current_block->nominal_rate) {
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
}
}
}
}
} else {
busy = false;
}
// The Stepper Port Reset Interrupt: Timer2 OVF interrupt handles the falling edge of the
// step pulse. This should always trigger before the next Timer0 COMPA interrupt and independently
// finish, if Timer0 is disabled after completing a move.
ISR(TIMER2_OVF_vect)
{
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK);
TCCR2B = 0; // Disable timer until needed.
}
// Reset and clear stepper subsystem variables
void st_reset()
{
memset(&st, 0, sizeof(st));
current_block = NULL;
busy = false;
}
// Initialize and start the stepper motor subsystem
void st_init()
{
// Configure directions of interface pins
STEPPING_DDR |= STEPPING_MASK;
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
// Configure Timer 0
TIMSK0 &= ~(1<<OCIE0A); // Disable Timer0 interrupt while configuring it
TCCR0B = 0; // Disable Timer2 until needed
TCNT0 = 0; // Clear Timer2 counter
TCCR0A = (1<<WGM21); // Set CTC mode
OCR0A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
// Configure Timer 2
TIMSK2 &= ~(1<<TOIE2);
TCCR2A = 0; // Normal operation
TCCR2B = 0; // Disable Timer2 until needed
TIMSK2 |= (1<<TOIE2); // Enable overflow interrupt
// Start in the idle state, but first wake up to check for keep steppers enabled option.
st_wake_up();
st_go_idle();
}
// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
// by the main program functions: planner auto-start and run-time command execution.
void st_cycle_start()
{
if (sys.state == STATE_QUEUED) {
sys.state = STATE_CYCLE;
st_wake_up();
}
}
// Execute a feed hold with deceleration, only during cycle. Called by main program.
void st_feed_hold()
{
if (sys.state == STATE_CYCLE) {
sys.state = STATE_HOLD;
sys.auto_start = false; // Disable planner auto start upon feed hold.
}
}
// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by
// runtime command execution in the main program, ensuring that the planner re-plans safely.
// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
// Only the planner de/ac-celerations profiles and stepper rates have been updated.
void st_cycle_reinitialize()
{
if (current_block != NULL) {
// Replan buffer from the feed hold stop location.
plan_cycle_reinitialize(st.step_events_remaining);
st.ramp_type = ACCEL_RAMP;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
st.delta_d = 0;
sys.state = STATE_QUEUED;
} else {
sys.state = STATE_IDLE;
}
}

680
stepper_new_time.c
View File

@ -1,680 +0,0 @@
/*
stepper.c - stepper motor driver: executes motion plans using stepper motors
Part of Grbl
Copyright (c) 2011-2013 Sungeun K. Jeon
Copyright (c) 2009-2011 Simen Svale Skogsrud
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#include <avr/interrupt.h>
#include "stepper.h"
#include "config.h"
#include "settings.h"
#include "planner.h"
#include "nuts_bolts.h"
// Some useful constants
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
#define RAMP_NOOP_CRUISE 0
#define RAMP_ACCEL 1
#define RAMP_DECEL 2
#define LOAD_NOOP 0
#define LOAD_LINE 1
#define LOAD_BLOCK 2
#define ST_NOOP 0
#define ST_END_OF_BLOCK 1
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
// Used by the bresenham line algorithm
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z;
int8_t segment_steps_remaining; // Steps remaining in line motion
// Used by inverse time algorithm to track step rate
int32_t counter_d; // Inverse time distance traveled since last step event
int32_t delta_d; // Inverse time distance traveled per interrupt tick
int32_t d_per_tick;
// Used by the stepper driver interrupt
uint8_t execute_step; // Flags step execution for each interrupt.
uint8_t step_pulse_time; // Step pulse reset time after step rise
uint8_t out_bits; // The next stepping-bits to be output
uint8_t load_flag;
uint8_t ramp_count;
uint8_t ramp_type;
} stepper_t;
static stepper_t st;
#define SEGMENT_BUFFER_SIZE 5
// Stores stepper buffer common data. Can change planner mid-block in special conditions.
typedef struct {
int32_t step_events_remaining; // Tracks step event count for the executing planner block
int32_t d_next; // Scaled distance to next step
float mm_per_step;
} st_data_t;
static st_data_t segment_data[SEGMENT_BUFFER_SIZE];
// Primary stepper motion buffer
typedef struct {
uint8_t n_step;
int32_t rate;
uint8_t st_data_index;
uint8_t flag;
} st_segment_t;
static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
static volatile uint8_t segment_buffer_tail;
static uint8_t segment_buffer_head;
static uint8_t segment_next_head;
static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced
static st_segment_t *st_current_segment;
static st_data_t *st_current_data;
static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer
static uint8_t pl_prep_index;
static st_data_t *st_prep_data;
static uint8_t st_data_prep_index;
// Returns the index of the next block in the ring buffer
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
static uint8_t next_block_index(uint8_t block_index)
{
block_index++;
if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; }
return(block_index);
}
/* __________________________
/| |\ _________________ ^
/ | | \ /| |\ |
/ | | \ / | | \ s
/ | | | | | \ p
/ | | | | | \ e
+-----+------------------------+---+--+---------------+----+ e
| BLOCK 1 | BLOCK 2 | d
time ----->
The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
+/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
*/
// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
// enabled. Startup init and limits call this function but shouldn't start the cycle.
void st_wake_up()
{
// Enable steppers by resetting the stepper disable port
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
}
if (sys.state == STATE_CYCLE) {
// Initialize stepper output bits
st.out_bits = settings.invert_mask;
// Initialize step pulse timing from settings.
st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
// Enable stepper driver interrupt
st.execute_step = false;
TCNT2 = 0; // Clear Timer2
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
}
}
// Stepper shutdown
void st_go_idle()
{
// Disable stepper driver interrupt. Allow Timer0 to finish. It will disable itself.
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt
TCCR2B = 0; // Disable Timer2
busy = false;
// Disable steppers only upon system alarm activated or by user setting to not be kept enabled.
if ((settings.stepper_idle_lock_time != 0xff) || bit_istrue(sys.execute,EXEC_ALARM)) {
// Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
// stop and not drift from residual inertial forces at the end of the last movement.
delay_ms(settings.stepper_idle_lock_time);
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
}
}
}
/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
on the Pramod Ranade 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 any
step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
meaning that some axes steps may not execute for a given multi-axis motion.
Grbl's algorithm slightly differs by using a single Ranade 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, it uses a Bresenham within a Bresenham algorithm,
where one tracks time(Ranade) and the other steps.
This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the
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.
ISR(TIMER2_COMPA_vect)
{
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
// Pulse stepper port pins, if flagged. New block dir will always be set one timer tick
// before any step pulse due to algorithm design.
if (st.execute_step) {
st.execute_step = false;
STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | st.out_bits;
TCNT0 = st.step_pulse_time; // Reload Timer0 counter.
TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
}
busy = true;
sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time.
// NOTE: The remaining code in this ISR will finish before returning to main program.
// If there is no step segment, attempt to pop one from the stepper buffer
if (st.load_flag != LOAD_NOOP) {
// 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.
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;
st.delta_d = st_current_segment->rate;
// Check if the counters need to be reset for a new planner block
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[st_current_segment->st_data_index];
// Initialize direction bits for block
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.
// 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.
}
st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete.
} else {
// Can't discard planner block here if a feed hold stops in middle of block.
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
return; // Nothing to do but exit.
}
}
// Iterate inverse time counter. Triggers each Bresenham step event.
st.counter_d -= st.delta_d;
// 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
st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits
st.execute_step = true;
// Execute step displacement profile by Bresenham line algorithm
st.counter_x -= pl_current_block->steps[X_AXIS];
if (st.counter_x < 0) {
st.out_bits |= (1<<X_STEP_BIT);
st.counter_x += pl_current_block->step_event_count;
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;
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;
if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
else { sys.position[Z_AXIS]++; }
}
// Check step events for trapezoid change or end of block.
st.segment_steps_remaining--; // Decrement step events count
if (st.segment_steps_remaining == 0) {
// 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) {
plan_discard_current_block();
st.load_flag = LOAD_BLOCK;
} else {
st.load_flag = LOAD_LINE;
}
// Discard current block
if (segment_buffer_head != 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
}
busy = false;
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
}
// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the
// step pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
// finish, if Timer2 is disabled after completing a move.
ISR(TIMER0_OVF_vect)
{
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK);
TCCR0B = 0; // Disable timer until needed.
}
// Reset and clear stepper subsystem variables
void st_reset()
{
memset(&st, 0, sizeof(st));
pl_current_block = NULL;
pl_prep_block = NULL;
pl_prep_index = 0;
st_data_prep_index = 0;
st.load_flag = LOAD_BLOCK;
busy = false;
segment_buffer_tail = 0;
segment_next_head = 1;
}
// Initialize and start the stepper motor subsystem
void st_init()
{
// Configure directions of interface pins
STEPPING_DDR |= STEPPING_MASK;
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
// Configure Timer 2
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt while configuring it
TCCR2B = 0; // Disable Timer2 until needed
TCNT2 = 0; // Clear Timer2 counter
TCCR2A = (1<<WGM21); // Set CTC mode
OCR2A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
// Configure Timer 0
TIMSK0 &= ~(1<<TOIE0);
TCCR0A = 0; // Normal operation
TCCR0B = 0; // Disable Timer0 until needed
TIMSK0 |= (1<<TOIE0); // Enable overflow interrupt
// Start in the idle state, but first wake up to check for keep steppers enabled option.
st_wake_up();
st_go_idle();
}
// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
// by the main program functions: planner auto-start and run-time command execution.
void st_cycle_start()
{
if (sys.state == STATE_QUEUED) {
sys.state = STATE_CYCLE;
st_wake_up();
}
}
// Execute a feed hold with deceleration, only during cycle. Called by main program.
void st_feed_hold()
{
if (sys.state == STATE_CYCLE) {
sys.state = STATE_HOLD;
sys.auto_start = false; // Disable planner auto start upon feed hold.
}
}
// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by
// runtime command execution in the main program, ensuring that the planner re-plans safely.
// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
// Only the planner de/ac-celerations profiles and stepper rates have been updated.
void st_cycle_reinitialize()
{
if (pl_current_block != NULL) {
// Replan buffer from the feed hold stop location.
// TODO: Need to add up all of the step events in the current planner block to give
// back to the planner. Should only need it for the current block.
// BUT! The planner block millimeters is all changed and may be changed into the next
// planner block. The block millimeters would need to be recalculated via step counts
// and the mm/step variable.
// OR. Do we plan the feed hold itself down with the planner.
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;
sys.state = STATE_QUEUED;
} else {
sys.state = STATE_IDLE;
}
}
/* Preps stepper buffer. Called from main program.
NOTE: There doesn't seem to be a great way to figure out how many steps occur within
a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a
critical problem. So, either numerical round-off checks could be made to account for
them, while CPU overhead could be minimized in some way, or we can flip the algorithm
around to have the stepper algorithm track number of steps over an indeterminant amount
of time instead.
In other words, we use the planner velocity floating point data to get an estimate of
the number of steps we want to execute. We then back out the approximate velocity for
the planner to use, which should be much more robust to round-off error. The main problem
now is that we are loading the stepper algorithm to handle acceleration now, rather than
pre-calculating with the main program. This approach does make sense in the way that
planner velocities and stepper profiles can be traced more accurately.
Which is better? Very hard to tell. The time-based algorithm would be able to handle
Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would
require some additional math in the stepper algorithm to adjust on the fly, plus adaptation
would occur in a non-deterministic manner.
I suppose it wouldn't hurt to build both to see what's better. Just a lot more work.
TODO: Need to describe the importance of continuations of step pulses between ramp states
and planner blocks. This has to do with Alden's problem with step "phase". The things I've
been doing here limit this phase issue by truncating some of the ramp timing for certain
events like deceleration initialization and end of block.
*/
void st_prep_buffer()
{
while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
// 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);
if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out.
// Prepare commonly shared planner block data for the ensuing step buffer moves
st_data_prep_index = next_block_index(st_data_prep_index);
st_prep_data = &segment_data[st_data_prep_index];
// Initialize Bresenham variables
st_prep_data->step_events_remaining = pl_prep_block->step_event_count;
// Convert new block to stepper variables.
// NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must
// be maintained as these execute.
// TODO: If the planner updates this block, particularly from a deceleration to an acceleration,
// we must reload the initial rate data, such that the velocity profile is re-constructed correctly.
// st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// 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)
// This data doesn't change. Could be performed in the planner, but fits nicely here.
// Although, acceleration can change for S-curves. So keep it here.
// 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)
// This definitely doesn't change, but could be precalculated in a way to help some of the
// math in this handler, i.e. millimeters per step event data.
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->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count;
}
/*
// Check if planner has changed block exit parameters. If so, then we have to update the
// velocity profile for the remainder of this block. Otherwise, the block hasn't changed.
if (exit_speed_sqr != last_exit_speed_sqr) {
intersect_distance = 0.5*( millimeters + (entry_speed_sqr-exit_speed_sqr)/(2*acceleration) );
if (intersect_distance <= 0) { // Deceleration only.
final_rate_sqr = initial_rate_sqr - 2*acceleration*segment_distance;
block->decelerate_after = 0;
} else {
decelerate_after = (block->nominal_speed_sqr - exit_speed_sqr)/(2*block->acceleration);
if (decelerate_after > intersect_distance) { decelerate_after = intersect_distance; }
if (decelerate_after > block->millimeters) { decelerate_after = block->millimeters; }
}
}
n_step = 100; // Estimate distance we're going to travel in this segment
if (n_step > step_events_remaining) { n_step = step_events_remaining; };
segment_distance = n_step*mm_per_step; // True distance traveled
// ISSUE: Either weighted average speed or truncated segments must be used. Otherwise
// accelerations, in particular high value, will not perform correctly.
if (distance_traveled < decelerate_after) {
if (segment_distance + distance_traveled > decelerate_after) {
n_step = ceil((decelerate_after-distance_traveled)/mm_per_step);
segment_distance = n_step*mm_per_step;
}
}
// based on time?
time = incremental time;
v_exit = v_entry + acceleration*time;
if (v_exit > v_nominal) {
time_accel = (v_nominal-v_entry)/acceleration;
distance = v_entry*time_accel + 0.5*acceleration*time_accel**2;
time_remaining = time - time_accel;
}
distance = v_entry*time + 0.5*acceleration*time*time;
t_accel = (v_nominal - v_entry) / acceleration;
t_decel = (v_exit - v_nominal) / -acceleration;
dt = (v_entry-v_exit)/acceleration;
if
// What's the speed of this segment? Computing per segment can get expensive, but this
// would allow S-curves to be installed pretty easily here.
if (initial_speed < nominal_speed) {
if (initial_distance < decelerate_after) { // Acceleration
exit_speed = sqrt(initial_speed*initial_speed + 2*acceleration*distance);
if (exit_speed > nominal_speed) { exit_speed = nominal_speed; }
} else { // Deceleration
exit_speed = sqrt(initial_speed*initial_speed - 2*acceleration*distance);
}
average_speed = 0.5*(initial_speed + exit_speed);
} else { // Cruise
average_speed = nominal_speed;
}
segment_rate = ceil(average_speed*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
if (segment_rate < MINIMUM_STEP_RATE) { segment_rate = MINIMUM_STEP_RATE; }
*/
/*
TODO: Need to check for a planner flag to indicate a change to this planner block.
If so, need to check for a change in acceleration state, from deceleration to acceleration,
to reset the stepper ramp counters and the initial_rate data to trace the new
ac/de-celeration profile correctly.
No change conditions:
- From nominal speed to acceleration from feedrate override
- From nominal speed to new deceleration.
- From acceleration to new deceleration point later or cruising point.
- From acceleration to immediate deceleration? Can happen during feedrate override
and slowing down, but likely ok by enforcing the normal ramp counter protocol.
Change conditions:
- From deceleration to acceleration, i.e. common with jogging when new blocks are added.
*/
st_segment_t *st_prep_block = &segment_buffer[segment_buffer_head];
st_prep_block->st_data_index = st_data_prep_index;
// TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'.
st_prep_block->n_step = 100; //floor( (exit_speed*approx_time)/mm_per_step );
// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions?
// st_segment->n_step = min(st_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 (st_prep_block->n_step > st_prep_data->step_events_remaining) {
st_prep_block->n_step = st_prep_data->step_events_remaining;
}
// Check if n_step exceeds decelerate point in block. Need to perform this so that the
// 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_block->n_step > pl_prep_block->decelerate_after) {
st_prep_block->n_step = pl_prep_block->decelerate_after;
}
float distance, exit_speed_sqr;
distance = st_prep_block->n_step*st_prep_data->mm_per_step; // Always greater than zero
if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) {
exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance;
// Set ISR tick reset flag for deceleration ramp.
} else { // Acceleration or cruising ramp
if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) {
exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance;
if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; }
} else {
exit_speed_sqr = pl_prep_block->nominal_speed_sqr;
}
}
// Adjust inverse time counter for ac/de-celerations
if (st.ramp_type) {
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.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
}
} else { // Adjust velocity for deceleration
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.
}
}
// 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; }
}
}
// During feed hold, do not update rate or ramp type. Keep decelerating.
if (sys.state == STATE_CYCLE) {
st.delta_d = st_current_data->initial_rate;
if (st.delta_d == st_current_data->nominal_rate) {
ramp_type = RAMP_NOOP_CRUISE;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
}
// Acceleration and cruise handled by ramping. Just check for deceleration.
if (st_current_segment->flag == ST_NOOP) {
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;
}
// Update planner block variables.
pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr);
// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable.
pl_prep_block->millimeters -= distance; // Potential round-off error near end of block.
pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter.
// Update stepper block variables.
st_prep_data->step_events_remaining -= st_prep_block->n_step;
if ( st_prep_data->step_events_remaining == 0 ) {
// Move planner pointer to next block
st_prep_block->flag = ST_END_OF_BLOCK;
pl_prep_index = next_block_index(pl_prep_index);
pl_prep_block = NULL;
} else {
st_prep_block->flag = ST_NOOP;
}
// New step block completed. Increment step buffer indices.
segment_buffer_head = segment_next_head;
segment_next_head = next_block_index(segment_buffer_head);
}
}