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

New stepper algorithm. Optimized planner.

Browse Source 
- Brand-new stepper algorithm. Based on the Pramod Ranade inverse time
algorithm, but modified to ensure step events are exact. Currently
limited to about 15kHz step rates, much more to be done to enable 30kHz
again.

- Removed Timer1. Stepper algorithm now uses Timer0 and Timer2.

- Much improved step generation during accelerations. Smoother. Allows
much higher accelerations (and speeds) than before on the same machine.

- Cleaner algorithm that is more easily portable to other CPU types.

- Streamlined planner calculations. Removed accelerate_until and
final_rate variables from block buffer since the new stepper algorithm
is that much more accurate.

- Improved planner efficiency by about 15-20% during worst case
scenarios (arcs).

- New config.h options to tune new stepper algorithm.
This commit is contained in:
Sonny Jeon
2012-12-08 15:00:58 -07:00
parent bba633101a
commit 9ba117c1bb
7 changed files with 394 additions and 520 deletions
Download Patch File Download Diff File Expand all files Collapse all files

449
stepper.c
View File

@ -2,8 +2,8 @@
stepper.c - stepper motor driver: executes motion plans using stepper motors
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Copyright (c) 2011-2012 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
@ -30,7 +30,11 @@
// Some useful constants
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
#define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND)
// #define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND)
#define INTERRUPTS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)
#define CRUISE_RAMP 0
#define ACCEL_RAMP 1
#define DECEL_RAMP 2
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
@ -38,28 +42,27 @@ typedef struct {
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z;
uint32_t event_count;
uint32_t step_events_completed; // The number of step events left in current motion
uint32_t event_count; // Total event count. Retained for feed holds.
uint32_t step_events_remaining; // Steps remaining in motion
// Used by the trapezoid generator
uint32_t cycles_per_step_event; // The number of machine cycles between each step event
uint32_t trapezoid_tick_cycle_counter; // The cycles since last trapezoid_tick. Used to generate ticks at a steady
// pace without allocating a separate timer
uint32_t trapezoid_adjusted_rate; // The current rate of step_events according to the trapezoid generator
uint32_t min_safe_rate; // Minimum safe rate for full deceleration rate reduction step. Otherwise halves step_rate.
// 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
uint16_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
static volatile uint8_t busy; // True when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
#if STEP_PULSE_DELAY > 0
static uint8_t step_bits; // Stores out_bits output to complete the step pulse delay
#endif
// 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.
// __________________________
// /| |\ _________________ ^
@ -73,12 +76,9 @@ static volatile uint8_t busy; // True when SIG_OUTPUT_COMPARE1A is being servi
// time ----->
//
// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
// during the first block->accelerate_until step_events_completed, then keeps going at constant speed until
// step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
// The slope of acceleration is always +/- block->rate_delta and is applied at a constant rate following the midpoint rule
// by the trapezoid generator, which is called ACCELERATION_TICKS_PER_SECOND times per second.
static void set_step_events_per_minute(uint32_t steps_per_minute);
// 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.
@ -92,27 +92,25 @@ void st_wake_up()
}
if (sys.state == STATE_CYCLE) {
// Initialize stepper output bits
out_bits = (0) ^ (settings.invert_mask);
// Initialize step pulse timing from settings. Here to ensure updating after re-writing.
#ifdef STEP_PULSE_DELAY
// Set total step pulse time after direction pin set. Ad hoc computation from oscilloscope.
step_pulse_time = -(((settings.pulse_microseconds+STEP_PULSE_DELAY-2)*TICKS_PER_MICROSECOND) >> 3);
// Set delay between direction pin write and step command.
OCR2A = -(((settings.pulse_microseconds)*TICKS_PER_MICROSECOND) >> 3);
#else // Normal operation
// Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement.
step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
#endif
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
TIMSK1 |= (1<<OCIE1A);
st.execute_step = false;
TCNT2 = 0; // Clear Timer2
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
}
}
// Stepper shutdown
void st_go_idle()
{
// Disable stepper driver interrupt
TIMSK1 &= ~(1<<OCIE1A);
// Disable stepper driver interrupt. Allow Timer2 to continue counting since COMPB may not be
// finished. COMPB will disable itself when finished.
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt
TCCR2B = 0; // Disable Timer2
// 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
@ -126,217 +124,194 @@ void st_go_idle()
}
}
// This function determines an acceleration velocity change every CYCLES_PER_ACCELERATION_TICK by
// keeping track of the number of elapsed cycles during a de/ac-celeration. The code assumes that
// step_events occur significantly more often than the acceleration velocity iterations.
inline static uint8_t iterate_trapezoid_cycle_counter()
{
st.trapezoid_tick_cycle_counter += st.cycles_per_step_event;
if(st.trapezoid_tick_cycle_counter > CYCLES_PER_ACCELERATION_TICK) {
st.trapezoid_tick_cycle_counter -= CYCLES_PER_ACCELERATION_TICK;
return(true);
} else {
return(false);
}
}
// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with
// config_step_timer. It 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.
ISR(TIMER1_COMPA_vect)
{
// "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 and still ensure number of steps for
// each axis are executed exactly. In other words, it uses a Bresenham within a Bresenham algorithm,
// where one tracks time(Ranade) and the other steps.
// This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
// appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the
// stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper
// outputs simultaneously with these two interrupts.
//
// NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or
// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this.
ISR(TIMER2_COMPA_vect)
{
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
// Set the direction pins a couple of nanoseconds before we step the steppers
// Set direction pins. Direction pins will always be set one timer tick before any step pulse.
STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
// Then pulse the stepping pins
#ifdef STEP_PULSE_DELAY
step_bits = (STEPPING_PORT & ~STEP_MASK) | out_bits; // Store out_bits to prevent overwriting.
#else // Normal operation
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits;
#endif
// Enable step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after
// exactly settings.pulse_microseconds microseconds, independent of the main Timer1 prescaler.
TCNT2 = step_pulse_time; // Reload timer counter
TCCR2B = (1<<CS21); // Begin timer2. Full speed, 1/8 prescaler
// Pulse stepper pins, if flagged.
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.
TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
}
busy = true;
// Re-enable interrupts to allow ISR_TIMER2_OVERFLOW to trigger on-time and allow serial communications
// regardless of time in this handler. The following code prepares the stepper driver for the next
// step interrupt compare and will always finish before returning to the main program.
sei();
// 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) {
if (sys.state == STATE_CYCLE) {
// During feed hold, do not update rate and trap counter. Keep decelerating.
st.trapezoid_adjusted_rate = current_block->initial_rate;
set_step_events_per_minute(st.trapezoid_adjusted_rate); // Initialize cycles_per_step_event
st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule.
}
st.min_safe_rate = current_block->rate_delta + (current_block->rate_delta >> 1); // 1.5 x rate_delta
st.counter_x = -(current_block->step_event_count >> 1);
// 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.
// Initialize direction bits for block
out_bits = current_block->direction_bits ^ (settings.invert_mask & DIRECTION_MASK);
// 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_completed = 0;
st.step_events_remaining = st.event_count;
// 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 = INTERRUPTS_PER_ACCELERATION_TICK/2;
// Initialize ramp type.
if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; }
else if (current_block->entry_speed == current_block->nominal_speed) { 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
}
busy = false;
return; // Nothing to do but exit.
}
}
if (current_block != NULL) {
// Execute step displacement profile by bresenham line algorithm
out_bits = current_block->direction_bits;
st.counter_x += current_block->steps_x;
if (st.counter_x > 0) {
// 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 = INTERRUPTS_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 = MINIMUM_STEP_RATE; // Prevent integer overflow
}
}
}
}
// 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; }
// 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 = INTERRUPTS_PER_ACCELERATION_TICK/2;
}
if (st.delta_d <= current_block->rate_delta) {
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP);
busy = false;
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
st.execute_step = true;
// Execute step displacement profile by Bresenham line algorithm
st.counter_x -= current_block->steps_x;
if (st.counter_x < 0) {
out_bits |= (1<<X_STEP_BIT);
st.counter_x -= st.event_count;
st.counter_x += st.event_count;
if (out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; }
}
st.counter_y += current_block->steps_y;
if (st.counter_y > 0) {
st.counter_y -= current_block->steps_y;
if (st.counter_y < 0) {
out_bits |= (1<<Y_STEP_BIT);
st.counter_y -= st.event_count;
st.counter_y += st.event_count;
if (out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
else { sys.position[Y_AXIS]++; }
}
st.counter_z += current_block->steps_z;
if (st.counter_z > 0) {
st.counter_z -= current_block->steps_z;
if (st.counter_z < 0) {
out_bits |= (1<<Z_STEP_BIT);
st.counter_z -= st.event_count;
st.counter_z += st.event_count;
if (out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
else { sys.position[Z_AXIS]++; }
}
st.step_events_completed++; // Iterate step events
// While in block steps, check for de/ac-celeration events and execute them accordingly.
if (st.step_events_completed < current_block->step_event_count) {
if (sys.state == STATE_HOLD) {
// Check for and execute feed hold by enforcing a steady deceleration from the moment of
// execution. The rate of deceleration is limited by rate_delta and will never decelerate
// faster or slower than in normal operation. If the distance required for the feed hold
// deceleration spans more than one block, the initial rate of the following blocks are not
// updated and deceleration is continued according to their corresponding rate_delta.
// NOTE: The trapezoid tick cycle counter is not updated intentionally. This ensures that
// the deceleration is smooth regardless of where the feed hold is initiated and if the
// deceleration distance spans multiple blocks.
if ( iterate_trapezoid_cycle_counter() ) {
// If deceleration complete, set system flags and shutdown steppers.
if (st.trapezoid_adjusted_rate <= current_block->rate_delta) {
// Just go idle. Do not NULL current block. The bresenham algorithm variables must
// remain intact to ensure the stepper path is exactly the same. Feed hold is still
// active and is released after the buffer has been reinitialized.
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program that feed hold is complete.
} else {
st.trapezoid_adjusted_rate -= current_block->rate_delta;
set_step_events_per_minute(st.trapezoid_adjusted_rate);
}
// 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) {
if (st.step_events_remaining == current_block->decelerate_after) {
st.ramp_count = INTERRUPTS_PER_ACCELERATION_TICK/2;
}
st.ramp_type = DECEL_RAMP;
}
} else {
// The trapezoid generator always checks step event location to ensure de/ac-celerations are
// executed and terminated at exactly the right time. This helps prevent over/under-shooting
// the target position and speed.
// NOTE: By increasing the ACCELERATION_TICKS_PER_SECOND in config.h, the resolution of the
// discrete velocity changes increase and accuracy can increase as well to a point. Numerical
// round-off errors can effect this, if set too high. This is important to note if a user has
// very high acceleration and/or feedrate requirements for their machine.
if (st.step_events_completed < current_block->accelerate_until) {
// Iterate cycle counter and check if speeds need to be increased.
if ( iterate_trapezoid_cycle_counter() ) {
st.trapezoid_adjusted_rate += current_block->rate_delta;
if (st.trapezoid_adjusted_rate >= current_block->nominal_rate) {
// Reached nominal rate a little early. Cruise at nominal rate until decelerate_after.
st.trapezoid_adjusted_rate = current_block->nominal_rate;
}
set_step_events_per_minute(st.trapezoid_adjusted_rate);
}
} else if (st.step_events_completed >= current_block->decelerate_after) {
// Reset trapezoid tick cycle counter to make sure that the deceleration is performed the
// same every time. Reset to CYCLES_PER_ACCELERATION_TICK/2 to follow the midpoint rule for
// an accurate approximation of the deceleration curve.
if (st.step_events_completed == current_block-> decelerate_after) {
st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2;
} else {
// Iterate cycle counter and check if speeds need to be reduced.
if ( iterate_trapezoid_cycle_counter() ) {
// NOTE: We will only do a full speed reduction if the result is more than the minimum safe
// rate, initialized in trapezoid reset as 1.5 x rate_delta. Otherwise, reduce the speed by
// half increments until finished. The half increments are guaranteed not to exceed the
// CNC acceleration limits, because they will never be greater than rate_delta. This catches
// small errors that might leave steps hanging after the last trapezoid tick or a very slow
// step rate at the end of a full stop deceleration in certain situations. The half rate
// reductions should only be called once or twice per block and create a nice smooth
// end deceleration.
if (st.trapezoid_adjusted_rate > st.min_safe_rate) {
st.trapezoid_adjusted_rate -= current_block->rate_delta;
} else {
st.trapezoid_adjusted_rate >>= 1; // Bit shift divide by 2
}
if (st.trapezoid_adjusted_rate < current_block->final_rate) {
// Reached final rate a little early. Cruise to end of block at final rate.
st.trapezoid_adjusted_rate = current_block->final_rate;
}
set_step_events_per_minute(st.trapezoid_adjusted_rate);
}
}
} else {
// No accelerations. Make sure we cruise exactly at the nominal rate.
if (st.trapezoid_adjusted_rate != current_block->nominal_rate) {
st.trapezoid_adjusted_rate = current_block->nominal_rate;
set_step_events_per_minute(st.trapezoid_adjusted_rate);
}
}
}
} else {
}
} else {
// If current block is finished, reset pointer
current_block = NULL;
plan_discard_current_block();
}
out_bits ^= settings.invert_mask; // Apply step port invert mask
}
out_bits ^= settings.invert_mask; // Apply step and direction invert mask
busy = false;
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
}
// This interrupt is set up by ISR_TIMER1_COMPAREA when it sets the motor port bits. It resets
// the motor port after a short period (settings.pulse_microseconds) completing one step cycle.
// NOTE: Interrupt collisions between the serial and stepper interrupts can cause delays by
// a few microseconds, if they execute right before one another. Not a big deal, but can
// cause issues at high step rates if another high frequency asynchronous interrupt is
// added to Grbl.
ISR(TIMER2_OVF_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
// finish, if Timer2 is disabled after completing a move.
ISR(TIMER0_OVF_vect)
{
// Reset stepping pins (leave the direction pins)
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK);
TCCR2B = 0; // Disable Timer2 to prevent re-entering this interrupt when it's not needed.
TCNT0 = 0; // Disable timer until needed.
}
#ifdef STEP_PULSE_DELAY
// This interrupt is used only when STEP_PULSE_DELAY is enabled. Here, the step pulse is
// initiated after the STEP_PULSE_DELAY time period has elapsed. The ISR TIMER2_OVF interrupt
// will then trigger after the appropriate settings.pulse_microseconds, as in normal operation.
// The new timing between direction, step pulse, and step complete events are setup in the
// st_wake_up() routine.
ISR(TIMER2_COMPA_vect)
{
STEPPING_PORT = step_bits; // Begin step pulse.
}
#endif
// Reset and clear stepper subsystem variables
void st_reset()
{
memset(&st, 0, sizeof(st));
set_step_events_per_minute(MINIMUM_STEPS_PER_MINUTE);
current_block = NULL;
busy = false;
}
@ -348,75 +323,25 @@ void st_init()
STEPPING_DDR |= STEPPING_MASK;
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
// waveform generation = 0100 = CTC
TCCR1B &= ~(1<<WGM13);
TCCR1B |= (1<<WGM12);
TCCR1A &= ~(1<<WGM11);
TCCR1A &= ~(1<<WGM10);
// output mode = 00 (disconnected)
TCCR1A &= ~(3<<COM1A0);
TCCR1A &= ~(3<<COM1B0);
// Configure Timer 2
TCCR2A = 0; // Normal operation
TCCR2B = 0; // Disable timer until needed.
TIMSK2 |= (1<<TOIE2); // Enable Timer2 Overflow interrupt
#ifdef STEP_PULSE_DELAY
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
#endif
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt while configuring it
TCCR2B = 0; // Disable Timer2 until needed
TCNT2 = 0; // Clear Timer2 counter
TCCR2A = (1<<WGM21); // Set CTC mode
OCR2A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
// Configure Timer 0
TIMSK0 &= ~(1<<TOIE0);
TCCR0A = 0; // Normal operation
TCCR0B = 0; // Disable Timer0 until needed
TIMSK0 |= (1<<TOIE0); // Enable overflow interrupt
// Start in the idle state, but first wake up to check for keep steppers enabled option.
st_wake_up();
st_go_idle();
}
// Configures the prescaler and ceiling of timer 1 to produce the given rate as accurately as possible.
// Returns the actual number of cycles per interrupt
static uint32_t config_step_timer(uint32_t cycles)
{
uint16_t ceiling;
uint8_t prescaler;
uint32_t actual_cycles;
if (cycles <= 0xffffL) {
ceiling = cycles;
prescaler = 1; // prescaler: 0
actual_cycles = ceiling;
} else if (cycles <= 0x7ffffL) {
ceiling = cycles >> 3;
prescaler = 2; // prescaler: 8
actual_cycles = ceiling * 8L;
} else if (cycles <= 0x3fffffL) {
ceiling = cycles >> 6;
prescaler = 3; // prescaler: 64
actual_cycles = ceiling * 64L;
} else if (cycles <= 0xffffffL) {
ceiling = (cycles >> 8);
prescaler = 4; // prescaler: 256
actual_cycles = ceiling * 256L;
} else if (cycles <= 0x3ffffffL) {
ceiling = (cycles >> 10);
prescaler = 5; // prescaler: 1024
actual_cycles = ceiling * 1024L;
} else {
// Okay, that was slower than we actually go. Just set the slowest speed
ceiling = 0xffff;
prescaler = 5;
actual_cycles = 0xffff * 1024;
}
// Set prescaler
TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (prescaler<<CS10);
// Set ceiling
OCR1A = ceiling;
return(actual_cycles);
}
static void set_step_events_per_minute(uint32_t steps_per_minute)
{
if (steps_per_minute < MINIMUM_STEPS_PER_MINUTE) { steps_per_minute = MINIMUM_STEPS_PER_MINUTE; }
st.cycles_per_step_event = config_step_timer((TICKS_PER_MICROSECOND*1000000*60)/steps_per_minute);
}
// 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.
@ -446,12 +371,10 @@ void st_cycle_reinitialize()
{
if (current_block != NULL) {
// Replan buffer from the feed hold stop location.
plan_cycle_reinitialize(current_block->step_event_count - st.step_events_completed);
// Update initial rate and timers after feed hold.
st.trapezoid_adjusted_rate = 0; // Resumes from rest
set_step_events_per_minute(st.trapezoid_adjusted_rate);
st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule.
st.step_events_completed = 0;
plan_cycle_reinitialize(st.step_events_remaining);
st.ramp_type = ACCEL_RAMP;
st.ramp_count = INTERRUPTS_PER_ACCELERATION_TICK/2;
st.delta_d = 0;
sys.state = STATE_QUEUED;
} else {
sys.state = STATE_IDLE;