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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
config.h
View File

@ -25,7 +25,7 @@
// IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them.
// Default settings. Used when resetting EEPROM. Change to desired name in defaults.h
#define DEFAULTS_GENERIC
#define DEFAULTS_SHERLINE_5400
// Serial baud rate
#define BAUD_RATE 9600
@ -106,25 +106,46 @@
#define CMD_RESET 0x18 // ctrl-x
// The temporal resolution of the acceleration management subsystem. Higher number give smoother
// acceleration but may impact performance.
// NOTE: Increasing this parameter will help any resolution related issues, especially with machines
// requiring very high accelerations and/or very fast feedrates. In general, this will reduce the
// error between how the planner plans the motions and how the stepper program actually performs them.
// However, at some point, the resolution can be high enough, where the errors related to numerical
// round-off can be great enough to cause problems and/or it's too fast for the Arduino. The correct
// value for this parameter is machine dependent, so it's advised to set this only as high as needed.
// Approximate successful values can range from 30L to 100L or more.
#define ACCELERATION_TICKS_PER_SECOND 50L
// acceleration but may impact performance. If you run at very high feedrates (>15kHz or so) and
// very high accelerations, this will reduce the error between how the planner plans the velocity
// profiles and how the stepper program actually performs them. The correct value for this parameter
// is machine dependent, so it's advised to set this only as high as needed. Approximate successful
// values can widely range from 50 to 200 or more. Cannot be greater than ISR_TICKS_PER_SECOND/2.
#define ACCELERATION_TICKS_PER_SECOND 100L
// The "Stepper Driver Interrupt" employs the Pramod Ranade inverse time algorithm to manage the
// Bresenham line stepping algorithm. The value ISR_TICKS_PER_SECOND is the frequency(Hz) at which
// the Ranade algorithm ticks at. Maximum step frequencies are limited by the Ranade frequency by
// approximately 0.75-0.9 * ISR_TICK_PER_SECOND. Meaning for 20kHz, the max step frequency is roughly
// 15-18kHz. An Arduino can safely complete a single interrupt of the current stepper driver algorithm
// theoretically up to a frequency of 35-40kHz, but CPU overhead increases exponentially as this
// frequency goes up. So there will be little left for other processes like arcs.
// In future versions, more work will be done to increase the step rates but still stay around
// 20kHz by performing two steps per step event, rather than just one.
#define ISR_TICKS_PER_SECOND 20000L // Integer (Hz)
// The Ranade algorithm can use either floating point or long integers for its counters, but for
// integers the counter values must be scaled since these values can be very small (10^-6). This
// multiplier value scales the floating point counter values for use in a long integer. Long integers
// are finite so select the multiplier value high enough to avoid any numerical round-off issues and
// still have enough range to account for all motion types. However, in most all imaginable CNC
// applications, the following multiplier value will work more than well enough. If you do have
// happened to weird stepper motion issues, try modifying this value by adding or subtracting a
// zero and report it to the Grbl administrators.
#define RANADE_MULTIPLIER 100000000.0
// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end
// of the buffer and all stops. This should not be much greater than zero and should only be changed
// if unwanted behavior is observed on a user's machine when running at very slow speeds.
#define MINIMUM_PLANNER_SPEED 0.0 // (mm/min)
// Minimum stepper rate. Sets the absolute minimum stepper rate in the stepper program and never runs
// slower than this value, except when sleeping. This parameter overrides the minimum planner speed.
// This is primarily used to guarantee that the end of a movement is always reached and not stop to
// never reach its target. This parameter should always be greater than zero.
// Minimum stepper rate for the "Stepper Driver Interrupt". Sets the absolute minimum stepper rate
// in the stepper program and never runs slower than this value. If the RANADE_MULTIPLIER value
// changes, it will affect how this value works. So, if a zero is add/subtracted from the
// RANADE_MULTIPLIER value, do the same to this value if you want to same response.
#define MINIMUM_STEP_RATE 1000L // Integer (mult*mm/isr_tic)
// Minimum stepper rate. Only used by homing at this point. May be removed in later releases.
#define MINIMUM_STEPS_PER_MINUTE 800 // (steps/min) - Integer value only
// Time delay increments performed during a dwell. The default value is set at 50ms, which provides
@ -210,23 +231,6 @@
// case, please report any successes to grbl administrators!
// #define ENABLE_XONXOFF // Default disabled. Uncomment to enable.
// Creates a delay between the direction pin setting and corresponding step pulse by creating
// another interrupt (Timer2 compare) to manage it. The main Grbl interrupt (Timer1 compare)
// sets the direction pins, and does not immediately set the stepper pins, as it would in
// normal operation. The Timer2 compare fires next to set the stepper pins after the step
// pulse delay time, and Timer2 overflow will complete the step pulse, except now delayed
// by the step pulse time plus the step pulse delay. (Thanks langwadt for the idea!)
// This is an experimental feature that should only be used if your setup requires a longer
// delay between direction and step pin settings (some opto coupler based drivers), as it may
// adversely effect Grbl's high-end performance (>10kHz). Please notify Grbl administrators
// of your successes or difficulties, as we will monitor this and possibly integrate this as a
// standard feature for future releases. However, we suggest to first try our direction delay
// hack/solution posted in the Wiki involving inverting the stepper pin mask.
// NOTE: Uncomment to enable. The recommended delay must be > 3us and the total step pulse
// time, which includes the Grbl settings pulse microseconds, must not exceed 127us. Reported
// successful values for certain setups have ranged from 10 to 20us.
// #define STEP_PULSE_DELAY 10 // Step pulse delay in microseconds. Default disabled.
// ---------------------------------------------------------------------------------------
// TODO: Install compile-time option to send numeric status codes rather than strings.

10
defaults.h
View File

@ -57,12 +57,12 @@
#ifdef DEFAULTS_SHERLINE_5400
// Description: Sherline 5400 mill with three NEMA 23 185 oz-in stepper motors, driven by
// three Pololu A4988 stepper drivers with a 30V, 6A power supply at 1.5A per winding.
#define MICROSTEPS 4
#define MICROSTEPS 2
#define STEPS_PER_REV 200.0
#define MM_PER_REV (0.050*MM_PER_INCH)) // 0.050 inch/rev leadscrew
#define DEFAULT_X_STEPS_PER_MM (STEP_PER_REV*MICROSTEPS/MM_PER_REV)
#define DEFAULT_Y_STEPS_PER_MM (STEP_PER_REV*MICROSTEPS/MM_PER_REV)
#define DEFAULT_Z_STEPS_PER_MM (STEP_PER_REV*MICROSTEPS/MM_PER_REV)
#define MM_PER_REV (0.050*MM_PER_INCH) // 0.050 inch/rev leadscrew
#define DEFAULT_X_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV)
#define DEFAULT_Y_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV)
#define DEFAULT_Z_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV)
#define DEFAULT_STEP_PULSE_MICROSECONDS 10
#define DEFAULT_MM_PER_ARC_SEGMENT 0.1
#define DEFAULT_RAPID_FEEDRATE 635.0 // mm/min (25ipm)

2
nuts_bolts.h
View File

@ -39,6 +39,8 @@
#define MM_PER_INCH (25.40)
#define INCH_PER_MM (0.0393701)
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
// Useful macros
#define clear_vector(a) memset(a, 0, sizeof(a))
#define clear_vector_float(a) memset(a, 0.0, sizeof(float)*N_AXIS)

365
planner.c
View File

@ -65,169 +65,67 @@ static uint8_t prev_block_index(uint8_t block_index)
}
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// given acceleration:
static float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration)
{
return( (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration) );
}
/* + <- some maximum rate we don't care about
/|\
/ | \
/ | + <- final_rate
/ | |
initial_rate -> +----+--+
^ ^
| |
intersection_distance distance */
// This function gives you the point at which you must start braking (at the rate of -acceleration) if
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
// a total travel of distance. This can be used to compute the intersection point between acceleration and
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
static float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance)
{
return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration) );
}
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity
// using the acceleration within the allotted distance.
// NOTE: sqrt() reimplimented here from prior version due to improved planner logic. Increases speed
// in time critical computations, i.e. arcs or rapid short lines from curves. Guaranteed to not exceed
// BLOCK_BUFFER_SIZE calls per planner cycle.
// NOTE: Guaranteed to not exceed BLOCK_BUFFER_SIZE calls per planner cycle.
static float max_allowable_speed(float acceleration, float target_velocity, float distance)
{
return( sqrt(target_velocity*target_velocity-2*acceleration*distance) );
}
// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next)
{
if (!current) { return; } // Cannot operate on nothing.
/* STEPPER VELOCITY PROFILE DEFINITION
less than nominal rate-> +
+--------+ <- nominal_rate /|\
/ \ / | \
initial_rate -> + \ / | + <- next->initial_rate
| + <- next->initial_rate / | |
+-------------+ initial_rate -> +----+--+
time --> ^ ^ ^ ^
| | | |
decelerate distance decelerate distance
if (next) {
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
// If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
// check for maximum allowable speed reductions to ensure maximum possible planned speed.
if (current->entry_speed != current->max_entry_speed) {
Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be
described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block
reaches the nominal speed of the block and cruises for a period of time. A triangle occurs
when the nominal speed is not reached within the block. Some other special cases exist,
such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that
has no deceleration period when the next block resumes acceleration.
// If nominal length true, max junction speed is guaranteed to be reached. Only compute
// for max allowable speed if block is decelerating and nominal length is false.
if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
current->entry_speed = min( current->max_entry_speed,
max_allowable_speed(-settings.acceleration,next->entry_speed,current->millimeters));
} else {
current->entry_speed = current->max_entry_speed;
}
current->recalculate_flag = true;
}
} // Skip last block. Already initialized and set for recalculation.
}
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the reverse pass.
static void planner_reverse_pass()
{
uint8_t block_index = block_buffer_head;
block_t *block[3] = {NULL, NULL, NULL};
while(block_index != block_buffer_tail) {
block_index = prev_block_index( block_index );
block[2]= block[1];
block[1]= block[0];
block[0] = &block_buffer[block_index];
planner_reverse_pass_kernel(block[0], block[1], block[2]);
}
// Skip buffer tail/first block to prevent over-writing the initial entry speed.
}
// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next)
{
if(!previous) { return; } // Begin planning after buffer_tail
// If the previous block is an acceleration block, but it is not long enough to complete the
// full speed change within the block, we need to adjust the entry speed accordingly. Entry
// speeds have already been reset, maximized, and reverse planned by reverse planner.
// If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
if (!previous->nominal_length_flag) {
if (previous->entry_speed < current->entry_speed) {
float entry_speed = min( current->entry_speed,
max_allowable_speed(-settings.acceleration,previous->entry_speed,previous->millimeters) );
// Check for junction speed change
if (current->entry_speed != entry_speed) {
current->entry_speed = entry_speed;
current->recalculate_flag = true;
}
}
}
}
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the forward pass.
static void planner_forward_pass()
{
uint8_t block_index = block_buffer_tail;
block_t *block[3] = {NULL, NULL, NULL};
while(block_index != block_buffer_head) {
block[0] = block[1];
block[1] = block[2];
block[2] = &block_buffer[block_index];
planner_forward_pass_kernel(block[0],block[1],block[2]);
block_index = next_block_index( block_index );
}
planner_forward_pass_kernel(block[1], block[2], NULL);
}
/* STEPPER RATE DEFINITION
+--------+ <- nominal_rate
/ \
nominal_rate*entry_factor -> + \
| + <- nominal_rate*exit_factor
+-------------+
time -->
The following function determines the type of velocity profile and stores the minimum required
information for the stepper algorithm to execute the calculated profiles. In this case, only
the new initial rate and steps until deceleration are computed, since the stepper algorithm
already handles acceleration and cruising and just needs to know when to start decelerating.
*/
// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
// The factors represent a factor of braking and must be in the range 0.0-1.0.
// This converts the planner parameters to the data required by the stepper controller.
// NOTE: Final rates must be computed in terms of their respective blocks.
static void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exit_factor)
static void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed)
{
block->initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min)
block->final_rate = ceil(block->nominal_rate*exit_factor); // (step/min)
int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; // (step/min^2)
int32_t accelerate_steps =
ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute));
int32_t decelerate_steps =
floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute));
// Compute new initial rate for stepper algorithm
block->initial_rate = ceil(entry_speed*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// Calculate the size of Plateau of Nominal Rate.
int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
// First determine intersection distance from the exit point for a triangular profile.
float steps_per_mm = block->step_event_count/block->millimeters;
int32_t intersect_distance = ceil( steps_per_mm *
(0.5*block->millimeters+(entry_speed*entry_speed-exit_speed*exit_speed)/(4*settings.acceleration)) );
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
// have to use intersection_distance() to calculate when to abort acceleration and start braking
// in order to reach the final_rate exactly at the end of this block.
if (plateau_steps < 0) {
accelerate_steps = ceil(
intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count));
accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off
accelerate_steps = min(accelerate_steps,block->step_event_count);
plateau_steps = 0;
// Check if this is a pure acceleration block by a intersection distance less than zero. Also
// prevents signed and unsigned integer conversion errors.
if (intersect_distance <= 0) {
block->decelerate_after = 0;
} else {
// Determine deceleration distance from nominal speed to exit speed for a trapezoidal profile.
// Value is never negative. Nominal speed is always greater than or equal to the exit speed.
block->decelerate_after = ceil(steps_per_mm *
(block->nominal_speed*block->nominal_speed-exit_speed*exit_speed)/(2*settings.acceleration));
// The lesser of the two triangle and trapezoid distances always defines the velocity profile.
if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; }
// Finally, check if this is a pure deceleration block.
if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; }
}
block->accelerate_until = accelerate_steps;
block->decelerate_after = accelerate_steps+plateau_steps;
}
/* PLANNER SPEED DEFINITION
+--------+ <- current->nominal_speed
/ \
@ -235,18 +133,108 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_factor, fl
| + <- next->entry_speed
+-------------+
time -->
*/
// Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the
// entry_speed for each junction and the entry_speed of the next junction. Must be called by
// planner_recalculate() after updating the blocks. Any recalulate flagged junction will
// compute the two adjacent trapezoids to the junction, since the junction speed corresponds
// to exit speed and entry speed of one another.
static void planner_recalculate_trapezoids()
{
uint8_t block_index = block_buffer_tail;
block_t *current;
block_t *next = NULL;
Recalculates the motion plan according to the following algorithm:
1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed)
so that:
a. The junction speed is equal to or less than the maximum junction speed limit
b. No speed reduction within one block requires faster deceleration than the acceleration limits.
c. The last (or newest appended) block is planned from a complete stop.
2. Go over every block in chronological (forward) order and dial down junction speed values if
a. The speed increase within one block would require faster acceleration than the acceleration limits.
When these stages are complete, all blocks have a junction entry speed that will allow all speed changes
to be performed using the overall limiting acceleration value, and where no junction speed is greater
than the max limit. In other words, it just computed the fastest possible velocity profile through all
buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully
executed. Finally it will:
3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a
a planner-modified junction speed with be updated, the others are assumed ok as is.
All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only
when planned values are converted to stepper rate parameters(3), these are integers. If another motion block
is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes.
NOTE: As executing blocks complete and incoming streaming blocks are appended to the planner buffer, this
function is constantly re-calculating and must be as efficient as possible. For example, in situations like
arc generation or complex curves, the short, rapid line segments can execute faster than new blocks can be
added, and the planner buffer will starve and empty, leading to weird hiccup-like jerky motions.
*/
static void planner_recalculate()
{
// TODO: No over-write protection exists for the executing block. For most cases this has proven to be ok, but
// for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to
// find out where in the planner buffer is the a safe place to begin re-planning from.
// Perform reverse planner pass.
uint8_t block_index = block_buffer_head;
block_t *next = NULL;
block_t *current = NULL;
block_t *previous = NULL;
while(block_index != block_buffer_tail) {
block_index = prev_block_index( block_index );
next = current;
current = previous;
previous = &block_buffer[block_index];
if (current) { // Cannot operate on nothing.
if (next) {
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
// If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
// check for maximum allowable speed reductions to ensure maximum possible planned speed.
if (current->entry_speed != current->max_entry_speed) {
// If nominal length true, max junction speed is guaranteed to be reached. Only compute
// for max allowable speed if block is decelerating and nominal length is false.
if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
current->entry_speed = min( current->max_entry_speed,
max_allowable_speed(-settings.acceleration,next->entry_speed,current->millimeters));
} else {
current->entry_speed = current->max_entry_speed;
}
current->recalculate_flag = true;
}
} // Skip last block. Already initialized and set for recalculation.
}
}
// Skip buffer tail/first block to prevent over-writing the initial entry speed.
// Perform forward planner pass.
block_index = block_buffer_tail;
next = NULL;
while(block_index != block_buffer_head) {
current = next;
next= &block_buffer[block_index];
// Begin planning after buffer_tail
if (current) {
// If the previous block is an acceleration block, but it is not long enough to complete the
// full speed change within the block, we need to adjust the entry speed accordingly. Entry
// speeds have already been reset, maximized, and reverse planned by reverse planner.
// If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
if (!current->nominal_length_flag) {
if (current->entry_speed < current->entry_speed) {
float entry_speed = min( next->entry_speed,
max_allowable_speed(-settings.acceleration,current->entry_speed,current->millimeters) );
// Check for junction speed change
if (next->entry_speed != entry_speed) {
next->entry_speed = entry_speed;
next->recalculate_flag = true;
}
}
}
}
block_index = next_block_index( block_index );
}
// Recalculate stepper algorithm data for any adjacent blocks with a modified junction speed.
block_index = block_buffer_tail;
next = NULL;
while(block_index != block_buffer_head) {
current = next;
next = &block_buffer[block_index];
@ -254,47 +242,17 @@ static void planner_recalculate_trapezoids()
// Recalculate if current block entry or exit junction speed has changed.
if (current->recalculate_flag || next->recalculate_flag) {
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed,
next->entry_speed/current->nominal_speed);
calculate_trapezoid_for_block(current, current->entry_speed, next->entry_speed);
current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
}
}
block_index = next_block_index( block_index );
}
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed,
MINIMUM_PLANNER_SPEED/next->nominal_speed);
calculate_trapezoid_for_block(next, next->entry_speed, MINIMUM_PLANNER_SPEED);
next->recalculate_flag = false;
}
// Recalculates the motion plan according to the following algorithm:
//
// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed)
// so that:
// a. The junction speed is equal to or less than the maximum junction speed limit
// b. No speed reduction within one block requires faster deceleration than the one, true constant
// acceleration.
// 2. Go over every block in chronological order and dial down junction speed values if
// a. The speed increase within one block would require faster acceleration than the one, true
// constant acceleration.
//
// When these stages are complete all blocks have an entry speed that will allow all speed changes to
// be performed using only the one, true constant acceleration, and where no junction speed is greater
// than the max limit. Finally it will:
//
// 3. Recalculate trapezoids for all blocks using the recently updated junction speeds. Block trapezoids
// with no updated junction speeds will not be recalculated and assumed ok as is.
//
// All planner computations are performed with doubles (float on Arduinos) to minimize numerical round-
// off errors. Only when planned values are converted to stepper rate parameters, these are integers.
static void planner_recalculate()
{
planner_reverse_pass();
planner_forward_pass();
planner_recalculate_trapezoids();
}
void plan_reset_buffer()
{
block_buffer_tail = block_buffer_head;
@ -380,28 +338,17 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
// Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
// NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
float inverse_minute;
if (!invert_feed_rate) {
inverse_minute = feed_rate * inverse_millimeters;
} else {
inverse_minute = 1.0 / feed_rate;
}
block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0
if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }
block->nominal_speed = feed_rate; // (mm/min) Always > 0
// Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
// average travel per step event changes. For a line along one axis the travel per step event
// is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
// axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
// To generate trapezoids with contant acceleration between blocks the rate_delta must be computed
// specifically for each line to compensate for this phenomenon:
// Convert universal acceleration for direction-dependent stepper rate change parameter
block->rate_delta = ceil( block->step_event_count*inverse_millimeters *
settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick)
// Compute the acceleration, nominal rate, and distance traveled per step event for the stepper algorithm.
block->rate_delta = ceil(settings.acceleration*
((RANADE_MULTIPLIER/(60*60))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
block->nominal_rate = ceil(block->nominal_speed*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
// Compute path unit vector
float unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
@ -428,8 +375,8 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
- pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {

8
planner.h
View File

@ -41,17 +41,15 @@ typedef struct {
float entry_speed; // Entry speed at previous-current block junction in mm/min
float max_entry_speed; // Maximum allowable junction entry speed in mm/min
float millimeters; // The total travel of this block in mm
uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction
uint8_t nominal_length_flag; // Planner flag for nominal speed always reached
uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction
uint8_t nominal_length_flag; // Planner flag for nominal speed always reached
// Settings for the trapezoid generator
uint32_t initial_rate; // The step rate at start of block
uint32_t final_rate; // The step rate at end of block
int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
uint32_t accelerate_until; // The index of the step event on which to stop acceleration
uint32_t decelerate_after; // The index of the step event on which to start decelerating
uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
uint32_t d_next; // Scaled distance to next step
} block_t;
// Initialize the motion plan subsystem

2
settings.h
View File

@ -25,7 +25,7 @@
#include <math.h>
#include "nuts_bolts.h"
#define GRBL_VERSION "0.8c"
#define GRBL_VERSION "0.9a"
// Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl
// when firmware is upgraded. Always stored in byte 0 of eeprom

435
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 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.
// 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.
} 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,175 +124,166 @@ 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);
}
}
} 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);
// 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 {
@ -302,41 +291,27 @@ ISR(TIMER1_COMPA_vect)
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;
}
@ -349,74 +324,24 @@ void st_init()
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;