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;