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

Yet another major stepper algorithm and planner overhaul.

Browse Source 
- Overhauled the stepper algorithm and planner again. This time
concentrating on the decoupling of the stepper ISR completely. It is
now dumb, relying on the segment generator to provide the number of
steps to execute and how fast it needs to go. This freed up lots of
memory as well because it made a lot tracked variables obsolete.

- The segment generator now computes the velocity profile of the
executing planner block on the fly in floating point math, instead of
allowing the stepper algorithm to govern accelerations in the previous
code. What this accomplishes is the ability and framework to (somewhat)
easily install a different physics model for generating a velocity
profile, i.e. s-curves.

- Made some more planner enhancements and increased efficiency a bit.

- The changes also did not increase the compiled size of Grbl, but
decreased it slightly as well.

- Cleaned up a lot of the commenting.

- Still much to do, but this push works and still is missing feedholds
(coming next.)
This commit is contained in:
Sonny Jeon
2013-11-22 17:35:58 -07:00
parent 2eb5acaa33
commit b36e30de2e
15 changed files with 4697 additions and 710 deletions
Download Patch File Download Diff File Expand all files Collapse all files

769
stepper.c
View File

@ -26,25 +26,41 @@
#include "planner.h"
#include "nuts_bolts.h"
// Some useful constants
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
#define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) // min/segment
#define RAMP_NOOP_CRUISE 0
#define RAMP_ACCEL 1
#define RAMP_ACCEL 0
#define RAMP_CRUISE 1
#define RAMP_DECEL 2
#define LOAD_NOOP 0
#define LOAD_SEGMENT 1
#define LOAD_BLOCK 2
#define SEGMENT_NOOP 0
#define SEGMENT_END_OF_BLOCK bit(0)
#define RAMP_CHANGE_ACCEL bit(1)
#define RAMP_CHANGE_DECEL bit(2)
// Stores the planner block Bresenham algorithm execution data for the segments in the segment
// buffer. 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).
// NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be
// discarded when entirely consumed and completed by the segment buffer.
typedef struct {
uint8_t direction_bits;
int32_t steps[N_AXIS];
int32_t step_event_count;
} st_block_t;
static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1];
// TODO: Directly adjust this parameters to stop motion of individual axes for the homing cycle.
// But this may require this to be volatile if it is controlled by an interrupt.
#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change
#define SEGMENT_BUFFER_SIZE 6
// 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_block_index; // Stepper block data index. Uses this information to execute this segment.
int32_t phase_dist; // Remaining step fraction to tick before completing segment.
int32_t dist_per_tick; // Step distance traveled per ISR tick, aka step rate.
} segment_t;
static segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
@ -52,84 +68,90 @@ typedef struct {
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z;
uint8_t segment_steps_remaining; // Steps remaining in line segment motion
// Used by inverse time algorithm to track step rate
int32_t counter_dist; // Inverse time distance traveled since last step event
uint32_t ramp_rate; // Inverse time distance traveled per interrupt tick
uint32_t dist_per_tick;
int32_t counter_dist; // Inverse time distance traveled since last step event
// 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 counter_ramp;
uint8_t ramp_type;
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 step_count; // Steps remaining in line segment motion
uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block.
st_block_t *exec_block; // Pointer to the block data for the segment being executed
segment_t *exec_segment; // Pointer to the segment being executed
} stepper_t;
static stepper_t st;
// 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 dist_per_step; // Scaled distance to next step
uint32_t initial_rate; // Initialized step rate at re/start of a planner block
uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
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-1];
// 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.
uint8_t flag; // Stepper algorithm bit-flag for special execution conditions.
} 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_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;
// Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
static volatile uint8_t busy;
// 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; // 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
static plan_block_t *pl_block; // Pointer to the planner block being prepped
static st_block_t *st_prep_block; // Pointer to the stepper block data being prepped
// Segment preparation data struct. Contains all the necessary information to compute new segments
// based on the current executing planner block.
typedef struct {
uint8_t st_block_index; // Index of stepper common data block being prepped
uint8_t partial_block_flag; // Flag indicating the planner has modified the prepped planner block
float step_per_mm; // Current planner block step/millimeter conversion scalar
float step_events_remaining; // Tracks step event count for the executing planner block
uint8_t ramp_type; // Current segment ramp state
float current_speed; // Current speed at the end of the segment buffer (mm/min)
float maximum_speed; // Maximum speed of executing block. Not always nominal speed. (mm/min)
float exit_speed; // Exit speed of executing block (mm/min)
float accelerate_until; // Acceleration ramp end measured from end of block (mm)
float decelerate_after; // Deceleration ramp start measured from end of block (mm)
} st_prep_t;
static st_prep_t prep;
/* __________________________
/* BLOCK VELOCITY PROFILE DEFINITION
__________________________
/| |\ _________________ ^
/ | | \ /| |\ |
/ | | \ / | | \ s
/ | | | | | \ p
/ | | | | | \ e
+-----+------------------------+---+--+---------------+----+ e
| BLOCK 1 | BLOCK 2 | d
time ----->
| BLOCK 1 ^ BLOCK 2 | d
|
time -----> EXAMPLE: Block 2 entry speed is at max junction velocity
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.
The planner block buffer is planned assuming constant acceleration velocity profiles and are
continuously joined at block junctions as shown above. However, the planner only actively computes
the block entry speeds for an optimal velocity plan, but does not compute the block internal
velocity profiles. These velocity profiles are computed ad-hoc as they are executed by the
stepper algorithm and consists of only 7 possible types of profiles: cruise-only, cruise-
deceleration, acceleration-cruise, acceleration-only, deceleration-only, full-trapezoid, and
triangle(no cruise).
maximum_speed (< nominal_speed) -> +
+--------+ <- maximum_speed (= nominal_speed) /|\
/ \ / | \
current_speed -> + \ / | + <- exit_speed
| + <- exit_speed / | |
+-------------+ current_speed -> +----+--+
time --> ^ ^ ^ ^
| | | |
decelerate_after(in mm) decelerate_after(in mm)
^ ^ ^ ^
| | | |
accelerate_until(in mm) accelerate_until(in mm)
The step segment buffer computes the executing block velocity profile and tracks the critical
parameters for the stepper algorithm to accurately trace the profile. These critical parameters
are shown and defined in the above illustration.
*/
// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
@ -147,10 +169,8 @@ void st_wake_up()
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;
st.load_flag = LOAD_BLOCK;
// Enable stepper driver interrupt
TCNT2 = 0; // Clear Timer2
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
@ -199,14 +219,8 @@ void st_go_idle()
/* TODO:
- 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.
- Write a blurb about how the acceleration should be handled within the ISR. All of the
time/step/ramp counters accurately keep track of the remainders and phasing of the variables
with time. This means we do not have to compute them via expensive floating point beforehand.
- Need to do an analysis to determine if these counters are really that much cheaper. At least
find out when it isn't anymore. Particularly when the ISR is at a very high frequency.
- Create NOTE: to describe that the total time in this ISR must be less than the ISR frequency
in its worst case scenario.
*/
@ -229,150 +243,94 @@ ISR(TIMER2_COMPA_vect)
// 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) {
if (st.exec_segment == NULL) {
// Anything in the buffer? If so, load and initialize next step segment.
if (segment_buffer_head != segment_buffer_tail) {
// Initialize new step segment and load number of steps to execute
st_current_segment = &segment_buffer[segment_buffer_tail];
st.segment_steps_remaining = st_current_segment->n_step;
st.exec_segment = &segment_buffer[segment_buffer_tail];
st.step_count = st.exec_segment->n_step;
// If the new segment starts a new planner block, initialize stepper variables and counters.
// NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous.
if (st.load_flag == LOAD_BLOCK) {
pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this.
st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index];
// NOTE: When the segment data index changes, this indicates a new planner block.
if ( st.exec_block_index != st.exec_segment->st_block_index ) {
st.exec_block_index = st.exec_segment->st_block_index;
st.exec_block = &st_block_buffer[st.exec_block_index];
// Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick.
st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask;
st.out_bits = st.exec_block->direction_bits ^ settings.invert_mask;
st.execute_step = true;
// Initialize Bresenham line counters
st.counter_x = (pl_current_block->step_event_count >> 1);
// Initialize Bresenham line and distance counters
st.counter_x = (st.exec_block->step_event_count >> 1);
st.counter_y = st.counter_x;
st.counter_z = st.counter_x;
// Initialize inverse time, step rate data, and acceleration ramp counters
st.counter_dist = st_current_data->dist_per_step; // dist_per_step always greater than ramp_rate.
st.ramp_rate = st_current_data->initial_rate;
st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule
st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary.
// Ensure the initial step rate exceeds the MINIMUM_STEP_RATE.
if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; }
else { st.dist_per_tick = st.ramp_rate; }
}
// Check if ramp conditions have changed. If so, update ramp counters and control variables.
if ( st_current_segment->flag & (RAMP_CHANGE_DECEL | RAMP_CHANGE_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 the latter conditions, the ramp count have been
initialized such that the following computation is still correct. */
st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp;
if ( st_current_segment->flag & RAMP_CHANGE_DECEL ) { st.ramp_type = RAMP_DECEL; }
else { st.ramp_type = RAMP_ACCEL; }
st.counter_dist = 0;
}
st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution.
} else {
// Can't discard planner block here if a feed hold stops in middle of block.
// Segment buffer empty. Shutdown.
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) { // Ignored when ramp type is RAMP_NOOP_CRUISE
st.counter_ramp--; // Tick acceleration ramp counter
if (st.counter_ramp == 0) { // Adjust step rate when its time
st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration
st.ramp_rate += st_current_data->rate_delta;
if (st.ramp_rate >= st_current_data->nominal_rate) { // Reached nominal rate.
st.ramp_rate = st_current_data->nominal_rate; // Set cruising velocity
st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising
st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp change.
}
} else { // Adjust velocity for deceleration.
if (st.ramp_rate > st_current_data->rate_delta) {
st.ramp_rate -= st_current_data->rate_delta;
} else { // Moving near zero feed rate. Gracefully slow down.
st.ramp_rate >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
}
}
// Adjust for minimum step rate, but retain operating ramp rate for accurate velocity tracing.
if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; }
else { st.dist_per_tick = st.ramp_rate; }
}
}
// Iterate inverse time counter. Triggers each Bresenham step event.
st.counter_dist -= st.dist_per_tick;
st.counter_dist += st.exec_segment->dist_per_tick;
// Execute Bresenham step event, when it's time to do so.
if (st.counter_dist < 0) {
st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter
if (st.counter_dist > INV_TIME_MULTIPLIER) {
if (st.step_count > 0) { // Block phase correction from executing step.
st.counter_dist -= INV_TIME_MULTIPLIER; // Reload inverse time counter
st.step_count--; // Decrement step events count
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 & SEGMENT_END_OF_BLOCK) {
plan_discard_current_block();
st.load_flag = LOAD_BLOCK;
} else {
st.load_flag = LOAD_SEGMENT;
// Execute step displacement profile by Bresenham line algorithm
st.execute_step = true;
st.out_bits = st.exec_block->direction_bits; // Reset out_bits and reload direction bits
st.counter_x -= st.exec_block->steps[X_AXIS];
if (st.counter_x < 0) {
st.out_bits |= (1<<X_STEP_BIT);
st.counter_x += st.exec_block->step_event_count;
if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; }
}
// Discard current segment by advancing buffer tail index
st.counter_y -= st.exec_block->steps[Y_AXIS];
if (st.counter_y < 0) {
st.out_bits |= (1<<Y_STEP_BIT);
st.counter_y += st.exec_block->step_event_count;
if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
else { sys.position[Y_AXIS]++; }
}
st.counter_z -= st.exec_block->steps[Z_AXIS];
if (st.counter_z < 0) {
st.out_bits |= (1<<Z_STEP_BIT);
st.counter_z += st.exec_block->step_event_count;
if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
else { sys.position[Z_AXIS]++; }
}
st.out_bits ^= settings.invert_mask; // Apply step port invert mask
}
}
if (st.step_count == 0) {
if (st.counter_dist > st.exec_segment->phase_dist) {
// Segment is complete. Discard current segment and advance segment indexing.
st.exec_segment = NULL;
if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; }
}
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.
/* 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);
@ -383,21 +341,15 @@ ISR(TIMER0_OVF_vect)
// Reset and clear stepper subsystem variables
void st_reset()
{
memset(&prep, 0, sizeof(prep));
memset(&st, 0, sizeof(st));
st.load_flag = LOAD_BLOCK;
busy = false;
pl_current_block = NULL; // Planner block pointer used by stepper algorithm
pl_prep_block = NULL; // Planner block pointer used by segment buffer
pl_prep_index = 0; // Planner buffer indices are also reset to zero.
st_data_prep_index = 0;
st.exec_segment = NULL;
pl_block = NULL; // Planner block pointer used by segment buffer
segment_buffer_tail = 0;
segment_buffer_head = 0; // empty = tail
segment_next_head = 1;
pl_partial_block_flag = false;
busy = false;
}
@ -445,6 +397,8 @@ void st_feed_hold()
{
if (sys.state == STATE_CYCLE) {
sys.state = STATE_HOLD;
st_update_plan_block_parameters();
st_prep_buffer();
sys.auto_start = false; // Disable planner auto start upon feed hold.
}
}
@ -457,17 +411,8 @@ void st_feed_hold()
// 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);
// if (pl_current_block != NULL) {
// plan_cycle_reinitialize(st_exec_block->step_events_remaining);
// st.ramp_type = RAMP_ACCEL;
// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2;
// st.ramp_rate = 0;
@ -480,6 +425,17 @@ void st_cycle_reinitialize()
}
// Called by planner_recalculate() when the executing block is updated by the new plan.
void st_update_plan_block_parameters()
{
if (pl_block != NULL) { // Ignore if at start of a new block.
pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; // Update entry speed.
pl_block = NULL; // Flag st_prep_segment() to load new velocity profile.
prep.partial_block_flag = true; // Flag st_prep_segment() to indicate block continuation.
}
}
/* Prepares step segment buffer. Continuously called from main program.
The segment buffer is an intermediary buffer interface between the execution of steps
@ -490,214 +446,247 @@ void st_cycle_reinitialize()
The number of steps "checked-out" from the planner buffer and the number of segments in
the segment buffer is sized and computed such that no operation in the main program takes
longer than the time it takes the stepper algorithm to empty it before refilling it.
Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps.
Currently, the segment buffer conservatively holds roughly up to 40-50 msec of steps.
NOTE: The segment buffer executes a set number of steps over an approximate time period.
If we try to execute over a fixed time period, it is difficult to guarantee or predict
how many steps will execute over it, especially when the step pulse phasing between the
neighboring segments must also be kept consistent. Meaning that, if the last segment step
pulses right before a segment end, the next segment must delay its first pulse so that the
step pulses are consistently spaced apart over time to keep the step pulse train nice and
smooth. Keeping track of phasing and ensuring that the exact number of steps are executed
as defined by the planner block, the related computational overhead can get quickly and
prohibitively expensive, especially in real-time.
Since the stepper algorithm automatically takes care of the step pulse phasing with
its ramp and inverse time counters by retaining the count remainders, we don't have to
explicitly and expensively track and synchronize the exact number of steps, time, and
phasing of steps. All we need to do is approximate the number of steps in each segment
such that the segment buffer has enough execution time for the main program to do what
it needs to do and refill it when it comes back. In other words, we just need to compute
a cheap approximation of the current velocity and the number of steps over it.
NOTE: The segment buffer executes a computed number of steps over a configured segment
execution time period, except at an end of a planner block where the segment execution
gets truncated by the lack of travel distance. Since steps are integer values and to keep
the distance traveled over the segment exact, a fractional step remaining after the last
executed step in a segment is handled by allowing the stepper algorithm distance counters
to tick to this fractional value without triggering a full step. So, when the next segment
is loaded for execution, its first full step will already have the distance counters primed
with the previous segment fractional step and will execute exactly on time according to
the planner block velocity profile. This ensures the step phasing between segments are kept
in sync and prevents artificially created accelerations between segments if they are not
accounted for. This allows the stepper algorithm to run at very high step rates without
losing steps.
*/
/*
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.
TODO: With feedrate overrides, increases to the override value will not significantly
change the planner and stepper current operation. When the value increases, we simply
need to recompute the block plan with new nominal speeds and maximum junction velocities.
However with a decreasing feedrate override, this gets a little tricky. The current block
plan is optimal, so if we try to reduce the feed rates, it may be impossible to create
a feasible plan at its current operating speed and decelerate down to zero at the end of
the buffer. We first have to enforce a deceleration to meet and intersect with the reduced
feedrate override plan. 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.
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.
that acts over several planner blocks. For example, say that the plan is already heavily
decelerating throughout it, reducing the feedrate override will not do much to it. So,
how do we determine when to resume the new plan? One solution is to tie into the feed hold
handling code to enforce a deceleration, but check when the current speed is less than or
equal to the block maximum speed and is in an acceleration or cruising ramp. At this
point, we know that we can recompute the block velocity profile to meet and continue onto
the new block plan.
*/
void st_prep_buffer()
{
if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued
while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
// Initialize new segment
st_segment_t *prep_segment = &segment_buffer[segment_buffer_head];
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.
// Increment stepper common data 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 = 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->dist_per_step = last_st_prep_data->dist_per_step;
st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this.
st_prep_data->mm_per_step = last_st_prep_data->mm_per_step;
pl_partial_block_flag = false; // Reset flag
// Determine if we need to load a new planner block. If so, prepare step data.
if (pl_block == NULL) {
pl_block = plan_get_current_block(); // Query planner for a queued block
if (pl_block == NULL) { return; } // No planner blocks. Exit.
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
// Check if the planner has re-computed this block mid-execution.
if (prep.partial_block_flag) {
// Retain last Bresenham data, but recompute the velocity profile.
prep.partial_block_flag = false; // Reset flag
} 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_prep_data = &segment_data[st_data_prep_index];
// Initialize Bresenham variables
st_prep_data->step_events_remaining = pl_prep_block->step_event_count;
// Increment stepper common data index
if ( ++prep.st_block_index == (SEGMENT_BUFFER_SIZE-1) ) { prep.st_block_index = 0; }
// Convert planner block velocity profile data to stepper rate and step distance data.
st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
st_prep_data->rate_delta = ceil(pl_prep_block->acceleration*
((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
st_prep_data->dist_per_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
// TODO: Check if we really need to store this.
st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count;
// Prepare and copy Bresenham algorithm segment data from the new planner block, so that
// when the segment buffer completes the planner block, it may be discarded immediately.
st_prep_block = &st_block_buffer[prep.st_block_index];
st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS];
st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS];
st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS];
st_prep_block->direction_bits = pl_block->direction_bits;
st_prep_block->step_event_count = pl_block->step_event_count;
// Initialize planner block step count, unit distance data, and remainder tracker.
prep.step_per_mm = st_prep_block->step_event_count/pl_block->millimeters;
prep.step_events_remaining = st_prep_block->step_event_count;
}
// Convert planner entry speed to stepper initial rate.
st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// 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)
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, RAMP_CHANGE_DECEL flag is set later.
if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = RAMP_CHANGE_ACCEL; }
// Compute velocity profile for a feed hold in-process.
// prep.current_speed = sqrt(pl_block->entry_speed_sqr); // !! For a new block, this needs to be updated.
// float inv_2_accel = 0.5/pl_block->acceleration;
// if (sys.state == STATE_HOLD) {
// prep.maximum_speed = prep.current_speed;
// prep.decelerate_after = pl_block->millimeters;
// prep.accelerate_until = pl_block->millimeters;
// prep.ramp_type = DECEL_RAMP; // or FEED_HOLD_RAMP?
// float decelerate_distance = inv_2_accel*pl_block->entry_speed_sqr;
// if (decelerate_distance > pl_block->millimeters) {
// // Keep decelerating through to the end of the block.
// // !! Need to update the next block's entry speed with current speed when loaded.
// prep.exit_speed = sqrt(pl_block->entry_speed_sqr - 2*pl_block->acceleration*pl_block->millimeters);
// } else {
// prep.exit_speed = 0.0;
// ***millimeters = decelerate_distance; // !! Need separate millimeters to track.
// // !! When target mm is reached, don't discard block until it really is at its end.
// // Return state to QUEUED and replan remaining buffer. That's about it.
// }
// } else {
// Compute the critical velocity profile parameters of the prepped planner block.
prep.current_speed = sqrt(pl_block->entry_speed_sqr);
prep.exit_speed = plan_get_exec_block_exit_speed();
prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp.
float exit_speed_sqr = prep.exit_speed*prep.exit_speed;
float inv_2_accel = 0.5/pl_block->acceleration;
float intersection_dist =
0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr));
if (intersection_dist > 0.0) {
if (intersection_dist < pl_block->millimeters) { // Either trapezoid or triangle types
// NOTE: For acceleration-cruise trapezoid, following calculation will be 0.0.
prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr);
if (prep.decelerate_after < intersection_dist) { // Trapezoid type
prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr);
if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr) {
// Cruise-deceleration or cruise-only type.
prep.ramp_type = RAMP_CRUISE;
prep.accelerate_until = pl_block->millimeters;
} else {
// Full-trapezoid or acceleration-cruise types
prep.accelerate_until =
pl_block->millimeters-inv_2_accel*(pl_block->nominal_speed_sqr-pl_block->entry_speed_sqr);
}
} else { // Triangle type
prep.accelerate_until = intersection_dist;
prep.decelerate_after = intersection_dist;
prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersection_dist+exit_speed_sqr);
}
} else { // Deceleration-only type
prep.ramp_type = RAMP_DECEL;
prep.maximum_speed = prep.current_speed;
prep.accelerate_until = pl_block->millimeters;
prep.decelerate_after = pl_block->millimeters;
}
} else { // Acceleration-only type
prep.maximum_speed = prep.exit_speed;
prep.accelerate_until = 0.0;
prep.decelerate_after = 0.0;
}
}
// Initialize new segment
segment_t *prep_segment = &segment_buffer[segment_buffer_head];
// Set new segment to point to the current segment data block.
prep_segment->st_data_index = st_data_prep_index;
prep_segment->st_block_index = prep.st_block_index;
// Approximate the velocity over the new segment using the already computed rate values.
// NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK.
// We do this to minimize memory and computational requirements. However, this could easily be replaced with
// a more exact approximation or have an user-defined time per segment, if CPU and memory overhead allows.
if (st_prep_data->decelerate_after <= 0) {
if (st_prep_data->decelerate_after == 0) { prep_segment->flag = RAMP_CHANGE_DECEL; } // Set segment deceleration flag
else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; }
if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; }
} else {
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;
}
/* -----------------------------------------------------------------------------------
Compute the average velocity of this new segment by determining the total distance
traveled over the segment time DT_SEGMENT. The follow code first attempts to create
a full segment based on the current ramp conditions. If the segment time is incomplete
by terminating at a ramp state change, the code will continue to loop through the
progressing ramp states to fill the remaining segment execution time. However, if
an incomplete segment terminates at the end of the planner block, the segment is
considered completed despite having a truncated execution time less than DT_SEGMENT.
*/
float dt = 0.0;
float mm_remaining = pl_block->millimeters;
float time_var = DT_SEGMENT; // Time worker variable
float mm_var; // mm distance worker variable
do {
switch (prep.ramp_type) {
case RAMP_ACCEL:
// NOTE: Acceleration ramp always computes during first loop only.
mm_remaining -= DT_SEGMENT*(prep.current_speed + pl_block->acceleration*(0.5*DT_SEGMENT));
if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp.
// Acceleration-cruise, acceleration-deceleration ramp junction, or end of block.
mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB
time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed);
if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; }
else { prep.ramp_type = RAMP_CRUISE; }
prep.current_speed = prep.maximum_speed;
} else { // Acceleration only.
prep.current_speed += pl_block->acceleration*time_var;
}
break;
case RAMP_CRUISE:
// NOTE: mm_var used to retain the last mm_remaining for incomplete segment time_var calculations.
mm_var = mm_remaining - prep.maximum_speed*time_var;
if (mm_var < prep.decelerate_after) { // End of cruise.
// Cruise-deceleration junction or end of block.
time_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed;
mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB
prep.ramp_type = RAMP_DECEL;
} else { // Cruising only.
mm_remaining = mm_var;
}
break;
default: // case RAMP_DECEL:
// NOTE: mm_var used to catch negative decelerate distance values near zero speed.
mm_var = time_var*(prep.current_speed - 0.5*pl_block->acceleration*time_var);
if ((mm_var > 0.0) && (mm_var < mm_remaining)) { // Deceleration only.
prep.current_speed -= pl_block->acceleration*time_var;
// Check for near-zero speed and prevent divide by zero in rare scenarios.
if (prep.current_speed > prep.exit_speed) { mm_remaining -= mm_var; }
else { mm_remaining = 0.0; } // NOTE: Force EOB for now. May change later.
} else { // End of block.
time_var = 2.0*mm_remaining/(prep.current_speed+prep.exit_speed);
mm_remaining = 0.0;
// prep.current_speed = prep.exit_speed;
}
}
}
// TODO: Look into replacing the following dist_per_step divide with multiplying its inverse to save cycles.
// Compute the number of steps in the prepped segment based on the approximate current rate.
// NOTE: The dist_per_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps.
prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)*
(ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_per_step);
// NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps
// a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed.
prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT);
// NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow.
// 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;
dt += time_var; // Add computed ramp time to total segment time.
if (dt < DT_SEGMENT) { time_var = DT_SEGMENT - dt; } // **Incomplete** At ramp junction.
else { break; } // **Complete** Exit loop. Segment execution time maxed.
} while ( mm_remaining > 0.0 ); // **Complete** Exit loop. End of planner block.
/* -----------------------------------------------------------------------------------
Compute segment step rate, steps to execute, and step phase correction parameters.
NOTE: Steps are computed by direct scalar conversion of the millimeter distance
remaining in the block, rather than incrementally tallying the steps executed per
segment. This helps in removing floating point round-off issues of several additions.
However, since floats have only 7.2 significant digits, long moves with extremely
high step counts can exceed the precision of floats, which can lead to lost steps.
Fortunately, this scenario is highly unlikely and unrealistic in CNC machines
supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm).
*/
// Use time_var to pre-compute dt inversion with integer multiplier.
time_var = (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))/dt; // (mult/isr_tic)
if (mm_remaining > 0.0) {
float steps_remaining = prep.step_per_mm*mm_remaining;
prep_segment->dist_per_tick = ceil( (prep.step_events_remaining-steps_remaining)*time_var ); // (mult*step/isr_tic)
// Compute number of steps to execute and segment step phase correction.
prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER*(ceil(steps_remaining)-steps_remaining));
prep_segment->n_step = ceil(prep.step_events_remaining)-ceil(steps_remaining);
// Update step execution variables
pl_block->millimeters = mm_remaining;
prep.step_events_remaining = steps_remaining;
} else { // End of block.
// Set to execute the remaining steps and no phase correction upon finishing the block.
prep_segment->dist_per_tick = ceil( prep.step_events_remaining*time_var ); // (mult*step/isr_tic)
prep_segment->phase_dist = 0;
prep_segment->n_step = ceil(prep.step_events_remaining);
// The planner block is complete. All steps are set to be executed in the segment buffer.
// TODO: Ignore this for feed holds. Need to recalculate the planner buffer at this time.
pl_block = NULL;
plan_discard_current_block();
}
// 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;
}
}
// Update stepper common data 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 ) {
// TODO: When a feed hold ends, the step_events_remaining will also be zero, even though a block
// have partially been completed. We need to flag the stepper algorithm to indicate a stepper shutdown
// when complete, but not remove the planner block unless it truly is the end of the block (rare).
// Set EOB bitflag so stepper algorithm discards the planner block after this segment completes.
prep_segment->flag |= SEGMENT_END_OF_BLOCK;
// Move planner pointer to next block and flag to load a new block for the next segment.
pl_prep_index = plan_next_block_index(pl_prep_index);
pl_prep_block = NULL;
}
// New step segment completed. Increment segment buffer indices.
// New step segment initialization completed. Increment segment buffer indices.
segment_buffer_head = segment_next_head;
if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; }
// long a = prep_segment->n_step;
// printInteger(a);
// printString(" ");
// int32_t blength = segment_buffer_head - segment_buffer_tail;
// if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; }
// printInteger(blength);
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
}
}
uint8_t st_get_prep_block_index()
{
// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this?
return(pl_prep_index);
}
void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating)
{
// if called, can we assume that this always changes and needs to be updated? if so, then
// we can perform all of the segment buffer setup tasks here to make sure the next time
// the segments are loaded, the st_data buffer is updated correctly.
// !!! Make sure that this is always pointing to the correct st_prep_data block.
// When a mid-block acceleration occurs, we have to make sure the ramp counters are updated
// correctly, much in the same fashion as the deceleration counters. Need to think about this
// make sure this is right, but i'm pretty sure it is.
// TODO: NULL means that the segment buffer has just completed a planner block. Clean up!
if (pl_prep_block != NULL) {
*millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step;
if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; }
else { *is_decelerating = true; }
// Flag for new prep_block when st_prep_buffer() is called after the planner recomputes.
pl_partial_block_flag = true;
pl_prep_block = NULL;
}
return;
}