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

Push old dev_2 draft to work on other things.

Browse Source 
- **NON-FUNCTIONAL**
- Contains an old draft of separating the stepper driver direct access
to the planner buffer. This is designed to keep the stepper and planner
modules independent and prevent overwriting or other complications. In
this way, feedrate override should be able to be installed as well.
- A number of planner optimizations are installed too.
- Not sure where the bugs are. Either in the new planner optimizations,
new stepper module updates, or in both. Or it just could be that the
Arduino AVR is choking with the new things it has to do.
This commit is contained in:
Sonny Jeon
2013-08-19 09:24:22 -06:00
parent 1fa3dad206
commit 7a175bd2db
23 changed files with 3160 additions and 704 deletions
Download Patch File Download Diff File Expand all files Collapse all files

571
stepper.c
View File

@ -19,20 +19,30 @@
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
and Philipp Tiefenbacher. */
#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 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
#define ST_DECEL 2
#define ST_DECEL_EOB 3
#define SEGMENT_BUFFER_SIZE 10
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
@ -40,44 +50,92 @@ typedef struct {
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z;
uint32_t event_count; // Total event count. Retained for feed holds.
uint32_t step_events_remaining; // Steps remaining in motion
uint8_t segment_steps_remaining; // Steps remaining in line 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.
// Used by inverse time algorithm to track step rate
int32_t counter_d; // Inverse time distance traveled since last step event
uint32_t delta_d; // Inverse time distance traveled per interrupt tick
uint32_t d_per_tick;
// 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;
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
// 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
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;
float mm_per_step;
} st_data_t;
static st_data_t segment_data[SEGMENT_BUFFER_SIZE];
// 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.
// Primary stepper motion buffer
typedef struct {
uint8_t n_step;
uint8_t st_data_index;
uint8_t flag;
} st_segment_t;
static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
// __________________________
// /| |\ _________________ ^
// / | | \ /| |\ |
// / | | \ / | | \ 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.
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);
}
static uint8_t next_block_pl_index(uint8_t block_index)
{
block_index++;
if (block_index == 18) { 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.
@ -91,14 +149,17 @@ void st_wake_up()
}
if (sys.state == STATE_CYCLE) {
// Initialize stepper output bits
out_bits = settings.invert_mask;
st.out_bits = settings.invert_mask;
// Initialize step pulse timing from settings.
step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
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;
TCNT2 = 0; // Clear Timer2
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
}
}
@ -125,20 +186,20 @@ 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.
//
/* "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)
@ -150,150 +211,171 @@ ISR(TIMER2_COMPA_vect)
// 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;
TCNT0 = step_pulse_time; // Reload Timer0 counter.
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. This ISR will still finish before returning to main program.
// 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 Ranade timer 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.
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) {
// Initialize direction bits for block
out_bits = current_block->direction_bits ^ settings.invert_mask;
st.execute_step = true; // Set flag to set direction bits.
// NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is
// a small chance that this will load at the same time as a step event. Hopefully,
// the overhead for this loading event isn't too much.. possibly 2-5 usec.
// NOTE: The stepper algorithm must control the planner buffer tail as it completes
// the block moves. Otherwise, a feed hold can leave a few step buffer line moves
// without the correct planner block information.
// Initialize 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;
st_current_segment = &segment_buffer[segment_buffer_tail];
// During feed hold, do not update Ranade 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;
// Load number of steps to execute from stepper buffer
st.segment_steps_remaining = st_current_segment->n_step;
// Check if the counters need to be reset for a new planner block
if (st.load_flag == LOAD_BLOCK) {
pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this.
st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index];
// 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; }
// 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.
// 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) {
// st.ramp_type = RAMP_NOOP_CRUISE;
st.ramp_type = RAMP_ACCEL;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
// }
// }
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
else { st.d_per_tick = st.delta_d; }
}
// Acceleration and cruise handled by ramping. Just check for deceleration.
if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) {
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;
}
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.
}
}
// 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--; // 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 == 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
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 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 { // 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; }
}
}
// Iterate Pramod Ranade 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; }
// Iterate inverse time counter. Triggers each Bresenham step event.
st.counter_d -= st.d_per_tick;
// Execute Bresenham step event, when it's time to do so.
if (st.d_counter < 0) {
st.d_counter += current_block->d_next;
// 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;
}
}
// TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz).
out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits
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 -= current_block->steps_x;
st.counter_x -= pl_current_block->steps[X_AXIS];
if (st.counter_x < 0) {
out_bits |= (1<<X_STEP_BIT);
st.counter_x += st.event_count;
if (out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
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 -= current_block->steps_y;
st.counter_y -= pl_current_block->steps[Y_AXIS];
if (st.counter_y < 0) {
out_bits |= (1<<Y_STEP_BIT);
st.counter_y += st.event_count;
if (out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
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 -= current_block->steps_z;
st.counter_z -= pl_current_block->steps[Z_AXIS];
if (st.counter_z < 0) {
out_bits |= (1<<Z_STEP_BIT);
st.counter_z += st.event_count;
if (out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
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.step_events_remaining--; // Decrement step events count
if (st.step_events_remaining) {
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
}
}
}
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 || st_current_segment->flag == ST_DECEL_EOB) {
plan_discard_current_block();
st.load_flag = LOAD_BLOCK;
} else {
st.load_flag = LOAD_LINE;
}
} else {
// If current block is finished, reset pointer
current_block = NULL;
plan_discard_current_block();
// Discard current segment
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.
}
out_bits ^= settings.invert_mask; // Apply step port invert mask
st.out_bits ^= settings.invert_mask; // Apply step port invert mask
}
busy = false;
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
@ -314,8 +396,14 @@ ISR(TIMER0_OVF_vect)
void st_reset()
{
memset(&st, 0, sizeof(st));
current_block = NULL;
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;
}
@ -374,14 +462,179 @@ void st_feed_hold()
// Only the planner de/ac-celerations profiles and stepper rates have been updated.
void st_cycle_reinitialize()
{
if (current_block != NULL) {
// if (pl_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 {
// 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;
// }
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;
// Calculate trapezoid data from planner.
st_prep_data->decelerate_after = calculate_trapezoid_for_block(pl_prep_index);
}
/*
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_segment = &segment_buffer[segment_buffer_head];
st_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'.
st_prep_segment->n_step = 250; //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_segment->n_step > st_prep_data->step_events_remaining) {
st_prep_segment->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_data->decelerate_after > 0) {
if (st_prep_segment->n_step > st_prep_data->decelerate_after) {
st_prep_segment->n_step = st_prep_data->decelerate_after;
}
}
// float distance, exit_speed_sqr;
// distance = st_prep_segment->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;
// } 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;
// }
// }
// 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_segment->n_step;
if ( st_prep_data->step_events_remaining == 0 ) {
// Move planner pointer to next block
if (st_prep_data->decelerate_after == 0) {
st_prep_segment->flag = ST_DECEL_EOB;
} else {
st_prep_segment->flag = ST_END_OF_BLOCK;
}
pl_prep_index = next_block_pl_index(pl_prep_index);
pl_prep_block = NULL;
printString("EOB");
} else {
if (st_prep_data->decelerate_after == 0) {
st_prep_segment->flag = ST_DECEL;
} else {
st_prep_segment->flag = ST_NOOP;
}
printString("x");
}
st_prep_data->decelerate_after -= st_prep_segment->n_step;
// New step block completed. Increment step buffer indices.
segment_buffer_head = segment_next_head;
segment_next_head = next_block_index(segment_buffer_head);
printInteger((long)st_prep_segment->n_step);
printString(" ");
printInteger((long)st_prep_data->decelerate_after);
printString(" ");
printInteger((long)st_prep_data->step_events_remaining);
}
}