Mirrors_Framework/grbl-Mega
1
0
Fork 0
mirror of https://github.com/gnea/grbl-Mega.git synced 2026-09-23 19:54:01 +08:00
Code Issues Actions Packages Projects Releases 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
Download Patch File Download Diff File Expand all files Collapse all files
config.h
+2 -2
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)
defaults.h
+2 -2
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
planner.c
+3
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.
stepper.c
+115 -111
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(" ");
}
}
stepper_new2.c
-601
View File
File diff suppressed because it is too large Load Diff
stepper_new_dual_ISR.c
-425
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;
}
}
stepper_new_time.c
-680
View File
File diff suppressed because it is too large Load Diff