From 9ba117c1bb46bdcc4e702b4e3439e29d71c900d5 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sat, 8 Dec 2012 15:00:58 -0700 Subject: [PATCH] New stepper algorithm. Optimized planner. - 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. --- config.h | 66 ++++---- defaults.h | 10 +- nuts_bolts.h | 2 + planner.c | 377 +++++++++++++++++++----------------------- planner.h | 8 +- settings.h | 2 +- stepper.c | 449 +++++++++++++++++++++------------------------------ 7 files changed, 394 insertions(+), 520 deletions(-) diff --git a/config.h b/config.h index c0bffa4..9b028f3 100644 --- a/config.h +++ b/config.h @@ -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. diff --git a/defaults.h b/defaults.h index 279e4b9..ab91038 100644 --- a/defaults.h +++ b/defaults.h @@ -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) diff --git a/nuts_bolts.h b/nuts_bolts.h index 65bdbe2..f334713 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -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) diff --git a/planner.c b/planner.c index 846e403..a9a7317 100644 --- a/planner.c +++ b/planner.c @@ -65,168 +65,66 @@ 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 + + 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 (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. -} - - -// 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 --> -*/ -// 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) + 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. +*/ +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; - } - - block->accelerate_until = accelerate_steps; - block->decelerate_after = accelerate_steps+plateau_steps; + // 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; } + } } + /* PLANNER SPEED DEFINITION +--------+ <- current->nominal_speed @@ -234,19 +132,109 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_factor, fl current->entry_speed -> + \ | + <- 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; + time --> + + 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, 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 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 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,9 +375,9 @@ 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) { vmax_junction = min(pl.previous_nominal_speed,block->nominal_speed); diff --git a/planner.h b/planner.h index b0fffa6..2788bc6 100644 --- a/planner.h +++ b/planner.h @@ -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 diff --git a/settings.h b/settings.h index bf3bb16..273912b 100644 --- a/settings.h +++ b/settings.h @@ -25,7 +25,7 @@ #include #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 diff --git a/stepper.c b/stepper.c index 28e6f50..46c6318 100644 --- a/stepper.c +++ b/stepper.c @@ -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< 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<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<steps_y; - if (st.counter_y > 0) { + st.counter_y -= current_block->steps_y; + if (st.counter_y < 0) { out_bits |= (1<steps_z; - if (st.counter_z > 0) { + st.counter_z -= current_block->steps_z; + if (st.counter_z < 0) { out_bits |= (1<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<> 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<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;