From aae1089051367091ed9551d4c5aa942d9298c855 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Tue, 12 Mar 2013 18:10:24 -0600 Subject: [PATCH 01/13] Pushed minor changes. Thanks @Protoneer! --- eeprom.h | 2 +- gcode.c | 16 ++++++++++------ 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/eeprom.h b/eeprom.h index 1769903..cffbcde 100644 --- a/eeprom.h +++ b/eeprom.h @@ -2,7 +2,7 @@ #define eeprom_h char eeprom_get_char(unsigned int addr); -void eeprom_put_char(unsigned int addr, char new_value); +void eeprom_put_char(unsigned int addr, unsigned char new_value); void memcpy_to_eeprom_with_checksum(unsigned int destination, char *source, unsigned int size); int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size); diff --git a/gcode.c b/gcode.c index d67ace0..88e6951 100644 --- a/gcode.c +++ b/gcode.c @@ -325,17 +325,21 @@ uint8_t gc_execute_line(char *line) } // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM float coord_data[N_AXIS]; - uint8_t home_select = SETTING_INDEX_G28; - if (non_modal_action == NON_MODAL_GO_HOME_1) { home_select = SETTING_INDEX_G30; } - if (!settings_read_coord_data(home_select,coord_data)) { return(STATUS_SETTING_READ_FAIL); } + if (non_modal_action == NON_MODAL_GO_HOME_0) { + if (!settings_read_coord_data(SETTING_INDEX_G28,coord_data)) { return(STATUS_SETTING_READ_FAIL); } + } else { + if (!settings_read_coord_data(SETTING_INDEX_G30,coord_data)) { return(STATUS_SETTING_READ_FAIL); } + } mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], -1.0, false); memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[]; axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags. break; case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1: - home_select = SETTING_INDEX_G28; - if (non_modal_action == NON_MODAL_SET_HOME_1) { home_select = SETTING_INDEX_G30; } - settings_write_coord_data(home_select,gc.position); + if (non_modal_action == NON_MODAL_SET_HOME_0) { + settings_write_coord_data(SETTING_INDEX_G28,gc.position); + } else { + settings_write_coord_data(SETTING_INDEX_G30,gc.position); + } break; case NON_MODAL_SET_COORDINATE_OFFSET: if (!axis_words) { // No axis words From e3cfa93d973f60b16faa3664c2dbb35e4976334a Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Tue, 19 Mar 2013 11:33:04 -0600 Subject: [PATCH 02/13] G-code updates for G10 L2 and L20. - LinuxCNC's g-code definitions changed for G10. Updated to their descriptions. --- gcode.c | 33 +++++++++++++++++---------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/gcode.c b/gcode.c index 88e6951..4445787 100644 --- a/gcode.c +++ b/gcode.c @@ -279,28 +279,29 @@ uint8_t gc_execute_line(char *line) break; case NON_MODAL_SET_COORDINATE_DATA: int_value = trunc(p); // Convert p value to int. - if ((l != 2 && l != 20) || (int_value < 1 || int_value > N_COORDINATE_SYSTEM)) { // L2 and L20. P1=G54, P2=G55, ... + if ((l != 2 && l != 20) || (int_value < 0 || int_value > N_COORDINATE_SYSTEM)) { // L2 and L20. P1=G54, P2=G55, ... FAIL(STATUS_UNSUPPORTED_STATEMENT); } else if (!axis_words && l==2) { // No axis words. FAIL(STATUS_INVALID_STATEMENT); } else { - int_value--; // Adjust P index to EEPROM coordinate data indexing. - if (l == 20) { - settings_write_coord_data(int_value,gc.position); - // Update system coordinate system if currently active. - if (gc.coord_select == int_value) { memcpy(gc.coord_system,gc.position,sizeof(gc.position)); } - } else { - float coord_data[N_AXIS]; - if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); } - // Update axes defined only in block. Always in machine coordinates. Can change non-active system. - uint8_t i; - for (i=0; i 0) { int_value--; } // Adjust P1-P6 index to EEPROM coordinate data indexing. + else { int_value = gc.coord_select; } // Index P0 as the active coordinate system + float coord_data[N_AXIS]; + if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); } + uint8_t i; + // Update axes defined only in block. Always in machine coordinates. Can change non-active system. + for (i=0; i Date: Thu, 21 Mar 2013 19:22:07 -0600 Subject: [PATCH 03/13] Untested! Soft limits, max travel, homing changes, new settings. - WARNING: Completely untested. Will later when there is time. Settings WILL be overwritten, as there are new settings. - Soft limits installed. Homing must be enabled for soft limits to work correctly. Errors out much like a hard limit, locking out everything and bringing up the alarm mode. Only difference is it forces a feed hold before doing so. Position is not lost. - IMPORTANT: Homing had to be updated so that soft limits work better with less CPU overhead. When homing completes, all axes are assumed to exist in negative space. If your limit switch is other side, the homing cycle with set this axis location to the max travel value, rather than zero. - Update mc_line() to accept an array, rather than individual variables. - Added an mc_auto_cycle_start() function handle this feature. Organization only. - --- config.h | 17 ++++---- defaults.h | 18 ++++++++- gcode.c | 11 +++-- limits.c | 28 ++++++++++++- limits.h | 8 +++- main.c | 4 +- motion_control.c | 102 ++++++++++++++++++++++++----------------------- motion_control.h | 7 +++- nuts_bolts.h | 5 ++- planner.c | 8 +--- planner.h | 4 +- protocol.c | 10 +++-- report.c | 48 ++++++++++++---------- report.h | 5 ++- settings.c | 49 ++++++++++++++--------- settings.h | 7 ++-- 16 files changed, 201 insertions(+), 130 deletions(-) diff --git a/config.h b/config.h index 93fd1ee..d1587fa 100644 --- a/config.h +++ b/config.h @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -107,13 +107,12 @@ // 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. +// the Ranade algorithm ticks at. Recommended step frequencies are limited by the Ranade frequency by +// approximately 0.75-0.9 * ISR_TICK_PER_SECOND. Meaning for 30kHz, the max step frequency is roughly +// 22.5-27kHz, but 30kHz is still possible, just not optimal. 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. #define ISR_TICKS_PER_SECOND 30000L // Integer (Hz) // The temporal resolution of the acceleration management subsystem. Higher number give smoother @@ -208,7 +207,7 @@ // The number of linear motions in the planner buffer to be planned at any give time. The vast // majority of RAM that Grbl uses is based on this buffer size. Only increase if there is extra -// available RAM, like when re-compiling for a Teensy or Sanguino. Or decrease if the Arduino +// available RAM, like when re-compiling for a Mega or Sanguino. Or decrease if the Arduino // begins to crash due to the lack of available RAM or if the CPU is having trouble keeping // up with planning new incoming motions as they are executed. // #define BLOCK_BUFFER_SIZE 18 // Uncomment to override default in planner.h. diff --git a/defaults.h b/defaults.h index 293e934..df3903c 100644 --- a/defaults.h +++ b/defaults.h @@ -2,7 +2,7 @@ defaults.h - defaults settings configuration file Part of Grbl - Copyright (c) 2012 Sungeun K. Jeon + Copyright (c) 2012-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -42,6 +42,7 @@ #define DEFAULT_REPORT_INCHES 0 // false #define DEFAULT_AUTO_START 1 // true #define DEFAULT_INVERT_ST_ENABLE 0 // false + #define DEFAULT_SOFT_LIMIT_ENABLE 0 // false #define DEFAULT_HARD_LIMIT_ENABLE 0 // false #define DEFAULT_HOMING_ENABLE 0 // false #define DEFAULT_HOMING_DIR_MASK 0 // move positive dir @@ -51,6 +52,9 @@ #define DEFAULT_HOMING_PULLOFF 1.0 // mm #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255) #define DEFAULT_DECIMAL_PLACES 3 + #define DEFAULT_X_MAX_TRAVEL 200 // mm + #define DEFAULT_Y_MAX_TRAVEL 200 // mm + #define DEFAULT_Z_MAX_TRAVEL 200 // mm #endif #ifdef DEFAULTS_SHERLINE_5400 @@ -72,6 +76,7 @@ #define DEFAULT_REPORT_INCHES 1 // false #define DEFAULT_AUTO_START 1 // true #define DEFAULT_INVERT_ST_ENABLE 0 // false + #define DEFAULT_SOFT_LIMIT_ENABLE 0 // false #define DEFAULT_HARD_LIMIT_ENABLE 0 // false #define DEFAULT_HOMING_ENABLE 0 // false #define DEFAULT_HOMING_DIR_MASK 0 // move positive dir @@ -81,6 +86,9 @@ #define DEFAULT_HOMING_PULLOFF 1.0 // mm #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255) #define DEFAULT_DECIMAL_PLACES 3 + #define DEFAULT_X_MAX_TRAVEL 200 // mm + #define DEFAULT_Y_MAX_TRAVEL 200 // mm + #define DEFAULT_Z_MAX_TRAVEL 200 // mm #endif #ifdef DEFAULTS_SHAPEOKO @@ -105,6 +113,7 @@ #define DEFAULT_REPORT_INCHES 0 // false #define DEFAULT_AUTO_START 1 // true #define DEFAULT_INVERT_ST_ENABLE 0 // false + #define DEFAULT_SOFT_LIMIT_ENABLE 0 // false #define DEFAULT_HARD_LIMIT_ENABLE 0 // false #define DEFAULT_HOMING_ENABLE 0 // false #define DEFAULT_HOMING_DIR_MASK 0 // move positive dir @@ -114,6 +123,9 @@ #define DEFAULT_HOMING_PULLOFF 1.0 // mm #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-255) #define DEFAULT_DECIMAL_PLACES 3 + #define DEFAULT_X_MAX_TRAVEL 200 // mm + #define DEFAULT_Y_MAX_TRAVEL 200 // mm + #define DEFAULT_Z_MAX_TRAVEL 200 // mm #endif #ifdef DEFAULTS_ZEN_TOOLWORKS_7x7 @@ -136,6 +148,7 @@ #define DEFAULT_REPORT_INCHES 0 // false #define DEFAULT_AUTO_START 1 // true #define DEFAULT_INVERT_ST_ENABLE 0 // false + #define DEFAULT_SOFT_LIMIT_ENABLE 0 // false #define DEFAULT_HARD_LIMIT_ENABLE 0 // false #define DEFAULT_HOMING_ENABLE 0 // false #define DEFAULT_HOMING_DIR_MASK 0 // move positive dir @@ -145,6 +158,9 @@ #define DEFAULT_HOMING_PULLOFF 1.0 // mm #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255) #define DEFAULT_DECIMAL_PLACES 3 + #define DEFAULT_X_MAX_TRAVEL 200 // mm + #define DEFAULT_Y_MAX_TRAVEL 200 // mm + #define DEFAULT_Z_MAX_TRAVEL 200 // mm #endif #endif diff --git a/gcode.c b/gcode.c index 4445787..b52de4b 100644 --- a/gcode.c +++ b/gcode.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -322,7 +322,7 @@ uint8_t gc_execute_line(char *line) target[i] = gc.position[i]; } } - mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], -1.0, false); + mc_line(target, -1.0, false); } // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM float coord_data[N_AXIS]; @@ -331,7 +331,7 @@ uint8_t gc_execute_line(char *line) } else { if (!settings_read_coord_data(SETTING_INDEX_G30,coord_data)) { return(STATUS_SETTING_READ_FAIL); } } - mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], -1.0, false); + mc_line(coord_data, -1.0, false); memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[]; axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags. break; @@ -404,7 +404,7 @@ uint8_t gc_execute_line(char *line) break; case MOTION_MODE_SEEK: if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} - else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], -1.0, false); } + else { mc_line(target, -1.0, false); } break; case MOTION_MODE_LINEAR: // TODO: Inverse time requires F-word with each statement. Need to do a check. Also need @@ -412,8 +412,7 @@ uint8_t gc_execute_line(char *line) // and after an inverse time move and then check for non-zero feed rate each time. This // should be efficient and effective. if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} - else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], - (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); } + else { mc_line(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); } break; case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: // Check if at least one of the axes of the selected plane has been specified. If in center diff --git a/limits.c b/limits.c index 1da9c54..a235057 100644 --- a/limits.c +++ b/limits.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2012 Sungeun K. Jeon + Copyright (c) 2012-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -244,3 +244,29 @@ void limits_go_home() st_go_idle(); // Call main stepper shutdown routine. } + + +// Performs a soft limit check. Called from mc_line() only. Assumes the machine has been homed, +// and the workspace volume is in all negative space. +void limits_soft_check(float *target) +{ + if ( target[X_AXIS] > 0 || target[X_AXIS] < -settings.max_travel[X_AXIS] || + target[Y_AXIS] > 0 || target[Y_AXIS] < -settings.max_travel[Y_AXIS] || + target[Z_AXIS] > 0 || target[Z_AXIS] < -settings.max_travel[Z_AXIS] ) { + + // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within + // workspace volume so just come to a controlled stop so position is not lost. When complete + // enter alarm mode. + if (sys.state == STATE_CYCLE) { + st_feed_hold(); + while (sys.state == STATE_HOLD) { + protocol_execute_runtime(); + if (sys.abort) { return; } + } + } + + mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. + sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event + protocol_execute_runtime(); // Execute to enter critical event loop and system abort + } +} diff --git a/limits.h b/limits.h index 847c667..ac94dd6 100644 --- a/limits.h +++ b/limits.h @@ -3,6 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -21,10 +22,13 @@ #ifndef limits_h #define limits_h -// initialize the limits module +// Initialize the limits module void limits_init(); -// perform the homing cycle +// Perform the homing cycle void limits_go_home(); +// Check for soft limit violations +void limits_soft_check(float *target); + #endif \ No newline at end of file diff --git a/main.c b/main.c index b1e7734..84b0313 100644 --- a/main.c +++ b/main.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -106,7 +106,7 @@ int main(void) // When the serial protocol returns, there are no more characters in the serial read buffer to // be processed and executed. This indicates that individual commands are being issued or // streaming is finished. In either case, auto-cycle start, if enabled, any queued moves. - if (sys.auto_start) { st_cycle_start(); } + mc_auto_cycle_start(); } return 0; /* never reached */ diff --git a/motion_control.c b/motion_control.c index ca23c87..15601e6 100644 --- a/motion_control.c +++ b/motion_control.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. Jeon Copyright (c) 2011 Jens Geisler Grbl is free software: you can redistribute it and/or modify @@ -41,19 +41,15 @@ // (1 minute)/feed_rate time. // NOTE: This is the primary gateway to the grbl planner. All line motions, including arc line // segments, must pass through this routine before being passed to the planner. The seperation of -// mc_line and plan_buffer_line is done primarily to make backlash compensation or canned cycle -// integration simple and direct. -// TODO: Check for a better way to avoid having to push the arguments twice for non-backlash cases. -// However, this keeps the memory requirements lower since it doesn't have to call and hold two -// plan_buffer_lines in memory. Grbl only has to retain the original line input variables during a -// backlash segment(s). -void mc_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) +// mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being +// in the planner and to let backlash compensation or canned cycle integration simple and direct. +void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) { - // TODO: Perform soft limit check here. Just check if the target x,y,z values are outside the - // work envelope. Should be straightforward and efficient. By placing it here, rather than in - // the g-code parser, it directly picks up motions from everywhere in Grbl. - - // If in check gcode mode, prevent motion by blocking planner. + // If enabled, check for soft limit violations. Placed here all line motions are picked up + // from everywhere in Grbl. + if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { limits_soft_check(target); } + + // If in check gcode mode, prevent motion by blocking planner. Soft limits still work. if (sys.state == STATE_CHECK_MODE) { return; } // TODO: Backlash compensation may be installed here. Only need direction info to track when @@ -68,22 +64,15 @@ void mc_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rat do { protocol_execute_runtime(); // Check for any run-time commands if (sys.abort) { return; } // Bail, if system abort. - } while ( plan_check_full_buffer() ); - plan_buffer_line(x, y, z, feed_rate, invert_feed_rate); + if ( plan_check_full_buffer() ) { mc_auto_cycle_start(); } // Auto-cycle start when buffer is full. + else { break; } + } while (1); + + plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); // If idle, indicate to the system there is now a planned block in the buffer ready to cycle // start. Otherwise ignore and continue on. if (!sys.state) { sys.state = STATE_QUEUED; } - - // Auto-cycle start immediately after planner finishes. Enabled/disabled by grbl settings. During - // a feed hold, auto-start is disabled momentarily until the cycle is resumed by the cycle-start - // runtime command. - // NOTE: This is allows the user to decide to exclusively use the cycle start runtime command to - // begin motion or let grbl auto-start it for them. This is useful when: manually cycle-starting - // when the buffer is completely full and primed; auto-starting, if there was only one g-code - // command sent during manual operation; or if a system is prone to buffer starvation, auto-start - // helps make sure it minimizes any dwelling/motion hiccups and keeps the cycle going. - // if (sys.auto_start) { st_cycle_start(); } } @@ -191,14 +180,14 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 arc_target[axis_0] = center_axis0 + r_axis0; arc_target[axis_1] = center_axis1 + r_axis1; arc_target[axis_linear] += linear_per_segment; - mc_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], feed_rate, invert_feed_rate); + mc_line(arc_target, feed_rate, invert_feed_rate); // Bail mid-circle on system abort. Runtime command check already performed by mc_line. if (sys.abort) { return; } } } // Ensure last segment arrives at target location. - mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); + mc_line(target, feed_rate, invert_feed_rate); } @@ -230,31 +219,35 @@ void mc_go_home() protocol_execute_runtime(); // Check for reset and set system abort. if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm. - // The machine should now be homed and machine zero has been located. Upon completion, - // reset system position and sync internal position vectors. - clear_vector_float(sys.position); // Set machine zero - sys_sync_current_position(); - sys.state = STATE_IDLE; // Set system state to IDLE to complete motion and indicate homed. - - // Pull-off axes (that have been homed) from limit switches before continuing motion. + // The machine should now be homed and machine limits have been located. By default, + // grbl defines machine space as all negative, as do most CNCs. Since limit switches + // can be on either side of an axes, check and set machine zero appropriately. + // At the same time, set up pull-off maneuver from axes limit switches that have been homed. // This provides some initial clearance off the switches and should also help prevent them // from falsely tripping when hard limits are enabled. - int8_t x_dir, y_dir, z_dir; - x_dir = y_dir = z_dir = 0; - if (HOMING_LOCATE_CYCLE & (1< (b)) ? (a) : (b)) #define min(a,b) (((a) < (b)) ? (a) : (b)) diff --git a/planner.c b/planner.c index baaba6c..c1a4d4f 100644 --- a/planner.c +++ b/planner.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. Jeon Copyright (c) 2011 Jens Geisler Grbl is free software: you can redistribute it and/or modify @@ -360,11 +360,7 @@ inline block_t *plan_get_current_block() // Returns the availability status of the block ring buffer. True, if full. uint8_t plan_check_full_buffer() { - if (block_buffer_tail == next_buffer_head) { - // TODO: Move this back into motion control. Shouldn't be here, but it's efficient. - if (sys.auto_start) { st_cycle_start(); } // Auto-cycle start when buffer is full. - return(true); - } + if (block_buffer_tail == next_buffer_head) { return(true); } return(false); } diff --git a/planner.h b/planner.h index a16c8c9..ce5126d 100644 --- a/planner.h +++ b/planner.h @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -52,7 +52,7 @@ typedef struct { uint32_t d_next; // Scaled distance to next step } block_t; -// Initialize the motion plan subsystem +// Initialize the motion plan subsystem void plan_init(); // Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in diff --git a/protocol.c b/protocol.c index c97770a..56aad3c 100644 --- a/protocol.c +++ b/protocol.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -105,15 +105,17 @@ void protocol_execute_runtime() if (rt_exec & (EXEC_ALARM | EXEC_CRIT_EVENT)) { sys.state = STATE_ALARM; // Set system alarm state - // Critical event. Only hard limit qualifies. Update this as new critical events surface. + // Critical event. Only hard/soft limit errors currently qualify. if (rt_exec & EXEC_CRIT_EVENT) { - report_alarm_message(ALARM_HARD_LIMIT); + report_alarm_message(ALARM_LIMIT_ERROR); report_feedback_message(MESSAGE_CRITICAL_EVENT); bit_false(sys.execute,EXEC_RESET); // Disable any existing reset do { // Nothing. Block EVERYTHING until user issues reset or power cycles. Hard limits // typically occur while unattended or not paying attention. Gives the user time - // to do what is needed before resetting, like killing the incoming stream. + // to do what is needed before resetting, like killing the incoming stream. The + // same could be said about soft limits. While the position is not lost, the incoming + // stream could be still engaged and cause a serious crash if it continues afterwards. } while (bit_isfalse(sys.execute,EXEC_RESET)); // Standard alarm event. Only abort during motion qualifies. diff --git a/report.c b/report.c index 6b810ad..c115dc7 100644 --- a/report.c +++ b/report.c @@ -2,7 +2,7 @@ report.c - reporting and messaging methods Part of Grbl - Copyright (c) 2012 Sungeun K. Jeon + Copyright (c) 2012-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -74,6 +74,8 @@ void report_status_message(uint8_t status_code) printPgmString(PSTR("Busy or queued")); break; case STATUS_ALARM_LOCK: printPgmString(PSTR("Alarm lock")); break; + case STATUS_SOFT_LIMIT_ERROR: + printPgmString(PSTR("Homing not enabled")); break; } printPgmString(PSTR("\r\n")); } @@ -84,8 +86,8 @@ void report_alarm_message(int8_t alarm_code) { printPgmString(PSTR("ALARM: ")); switch (alarm_code) { - case ALARM_HARD_LIMIT: - printPgmString(PSTR("Hard limit")); break; + case ALARM_LIMIT_ERROR: + printPgmString(PSTR("Hard/soft limit")); break; case ALARM_ABORT_CYCLE: printPgmString(PSTR("Abort during cycle")); break; } @@ -112,7 +114,7 @@ void report_feedback_message(uint8_t message_code) case MESSAGE_ENABLED: printPgmString(PSTR("Enabled")); break; case MESSAGE_DISABLED: - printPgmString(PSTR("Disabled")); break; + printPgmString(PSTR("Disabled")); break; } printPgmString(PSTR("]\r\n")); } @@ -153,25 +155,29 @@ void report_grbl_settings() { printPgmString(PSTR(" (z v_max, mm/min)\r\n$6=")); printFloat(settings.acceleration[X_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (x accel, mm/sec^2)\r\n$7=")); printFloat(settings.acceleration[Y_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (y accel, mm/sec^2)\r\n$8=")); printFloat(settings.acceleration[Z_AXIS]/(60*60)); // Convert from mm/min^2 for human readability - printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printInteger(settings.pulse_microseconds); - printPgmString(PSTR(" (step pulse, usec)\r\n$10=")); printFloat(settings.default_feed_rate); - printPgmString(PSTR(" (default feed, mm/min)\r\n$11=")); printInteger(settings.invert_mask); + printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(settings.max_travel[X_AXIS]); + printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(settings.max_travel[Y_AXIS]); + printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(settings.max_travel[Z_AXIS]); + printPgmString(PSTR(" (z max travel, mm)\r\n$12=")); printInteger(settings.pulse_microseconds); + printPgmString(PSTR(" (step pulse, usec)\r\n$13=")); printFloat(settings.default_feed_rate); + printPgmString(PSTR(" (default feed, mm/min)\r\n$14=")); printInteger(settings.invert_mask); printPgmString(PSTR(" (step port invert mask, int:")); print_uint8_base2(settings.invert_mask); - printPgmString(PSTR(")\r\n$12=")); printInteger(settings.stepper_idle_lock_time); - printPgmString(PSTR(" (step idle delay, msec)\r\n$13=")); printFloat(settings.junction_deviation); - printPgmString(PSTR(" (junction deviation, mm)\r\n$14=")); printFloat(settings.arc_tolerance); - printPgmString(PSTR(" (arc tolerance, mm)\r\n$15=")); printInteger(settings.decimal_places); - printPgmString(PSTR(" (n-decimals, int)\r\n$16=")); printInteger(bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)); - printPgmString(PSTR(" (report inches, bool)\r\n$17=")); printInteger(bit_istrue(settings.flags,BITFLAG_AUTO_START)); - printPgmString(PSTR(" (auto start, bool)\r\n$18=")); printInteger(bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)); - printPgmString(PSTR(" (invert step enable, bool)\r\n$19=")); printInteger(bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)); - printPgmString(PSTR(" (hard limits, bool)\r\n$20=")); printInteger(bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)); - printPgmString(PSTR(" (homing cycle, bool)\r\n$21=")); printInteger(settings.homing_dir_mask); + printPgmString(PSTR(")\r\n$15=")); printInteger(settings.stepper_idle_lock_time); + printPgmString(PSTR(" (step idle delay, msec)\r\n$16=")); printFloat(settings.junction_deviation); + printPgmString(PSTR(" (junction deviation, mm)\r\n$17=")); printFloat(settings.arc_tolerance); + printPgmString(PSTR(" (arc tolerance, mm)\r\n$18=")); printInteger(settings.decimal_places); + printPgmString(PSTR(" (n-decimals, int)\r\n$19=")); printInteger(bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)); + printPgmString(PSTR(" (report inches, bool)\r\n$20=")); printInteger(bit_istrue(settings.flags,BITFLAG_AUTO_START)); + printPgmString(PSTR(" (auto start, bool)\r\n$21=")); printInteger(bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)); + printPgmString(PSTR(" (invert step enable, bool)\r\n$22=")); printInteger(bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)); + printPgmString(PSTR(" (soft limits, bool)\r\n$23=")); printInteger(bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)); + printPgmString(PSTR(" (hard limits, bool)\r\n$24=")); printInteger(bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)); + printPgmString(PSTR(" (homing cycle, bool)\r\n$25=")); printInteger(settings.homing_dir_mask); printPgmString(PSTR(" (homing dir invert mask, int:")); print_uint8_base2(settings.homing_dir_mask); - printPgmString(PSTR(")\r\n$22=")); printFloat(settings.homing_feed_rate); - printPgmString(PSTR(" (homing feed, mm/min)\r\n$23=")); printFloat(settings.homing_seek_rate); - printPgmString(PSTR(" (homing seek, mm/min)\r\n$24=")); printInteger(settings.homing_debounce_delay); - printPgmString(PSTR(" (homing debounce, msec)\r\n$25=")); printFloat(settings.homing_pulloff); + printPgmString(PSTR(")\r\n$26=")); printFloat(settings.homing_feed_rate); + printPgmString(PSTR(" (homing feed, mm/min)\r\n$27=")); printFloat(settings.homing_seek_rate); + printPgmString(PSTR(" (homing seek, mm/min)\r\n$28=")); printInteger(settings.homing_debounce_delay); + printPgmString(PSTR(" (homing debounce, msec)\r\n$29=")); printFloat(settings.homing_pulloff); printPgmString(PSTR(" (homing pull-off, mm)\r\n")); } diff --git a/report.h b/report.h index 8f1555c..cd7f42d 100644 --- a/report.h +++ b/report.h @@ -2,7 +2,7 @@ report.h - reporting and messaging methods Part of Grbl - Copyright (c) 2012 Sungeun K. Jeon + Copyright (c) 2012-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -35,9 +35,10 @@ #define STATUS_SETTING_READ_FAIL 10 #define STATUS_IDLE_ERROR 11 #define STATUS_ALARM_LOCK 12 +#define STATUS_SOFT_LIMIT_ERROR 13 // Define Grbl alarm codes. Less than zero to distinguish alarm error from status error. -#define ALARM_HARD_LIMIT -1 +#define ALARM_LIMIT_ERROR -1 #define ALARM_ABORT_CYCLE -2 // Define Grbl feedback message codes. diff --git a/settings.c b/settings.c index 2f75e6e..9225a91 100644 --- a/settings.c +++ b/settings.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -88,6 +88,7 @@ void settings_reset(bool reset_all) { if (DEFAULT_REPORT_INCHES) { settings.flags |= BITFLAG_REPORT_INCHES; } if (DEFAULT_AUTO_START) { settings.flags |= BITFLAG_AUTO_START; } if (DEFAULT_INVERT_ST_ENABLE) { settings.flags |= BITFLAG_INVERT_ST_ENABLE; } + if (DEFAULT_SOFT_LIMIT_ENABLE) { settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; } if (DEFAULT_HARD_LIMIT_ENABLE) { settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; } if (DEFAULT_HOMING_ENABLE) { settings.flags |= BITFLAG_HOMING_ENABLE; } settings.homing_dir_mask = DEFAULT_HOMING_DIR_MASK; @@ -97,6 +98,9 @@ void settings_reset(bool reset_all) { settings.homing_pulloff = DEFAULT_HOMING_PULLOFF; settings.stepper_idle_lock_time = DEFAULT_STEPPER_IDLE_LOCK_TIME; settings.decimal_places = DEFAULT_DECIMAL_PLACES; + settings.max_travel[X_AXIS] = DEFAULT_X_MAX_TRAVEL; + settings.max_travel[Y_AXIS] = DEFAULT_Y_MAX_TRAVEL; + settings.max_travel[Z_AXIS] = DEFAULT_Z_MAX_TRAVEL; write_global_settings(); } @@ -165,41 +169,50 @@ uint8_t settings_store_global_setting(int parameter, float value) { case 6: settings.acceleration[X_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 7: settings.acceleration[Y_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 8: settings.acceleration[Z_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. - case 9: + case 9: settings.max_travel[X_AXIS] = value; break; + case 10: settings.max_travel[Y_AXIS] = value; break; + case 11: settings.max_travel[Z_AXIS] = value; break; + case 12: if (value < 3) { return(STATUS_SETTING_STEP_PULSE_MIN); } settings.pulse_microseconds = round(value); break; - case 10: settings.default_feed_rate = value; break; - case 11: settings.invert_mask = trunc(value); break; - case 12: settings.stepper_idle_lock_time = round(value); break; - case 13: settings.junction_deviation = fabs(value); break; - case 14: settings.arc_tolerance = value; break; - case 15: settings.decimal_places = round(value); break; - case 16: + case 13: settings.default_feed_rate = value; break; + case 14: settings.invert_mask = trunc(value); break; + case 15: settings.stepper_idle_lock_time = round(value); break; + case 16: settings.junction_deviation = fabs(value); break; + case 17: settings.arc_tolerance = value; break; + case 18: settings.decimal_places = round(value); break; + case 19: if (value) { settings.flags |= BITFLAG_REPORT_INCHES; } else { settings.flags &= ~BITFLAG_REPORT_INCHES; } break; - case 17: // Reset to ensure change. Immediate re-init may cause problems. + case 20: // Reset to ensure change. Immediate re-init may cause problems. if (value) { settings.flags |= BITFLAG_AUTO_START; } else { settings.flags &= ~BITFLAG_AUTO_START; } break; - case 18: // Reset to ensure change. Immediate re-init may cause problems. + case 21: // Reset to ensure change. Immediate re-init may cause problems. if (value) { settings.flags |= BITFLAG_INVERT_ST_ENABLE; } else { settings.flags &= ~BITFLAG_INVERT_ST_ENABLE; } break; - case 19: + case 22: + if (value) { + if (bit_isfalse(settings.flags, BITFLAG_HOMING_ENABLE)) { return(STATUS_SOFT_LIMIT_ERROR); } + settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; + } else { settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; } + break; + case 23: if (value) { settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; } else { settings.flags &= ~BITFLAG_HARD_LIMIT_ENABLE; } limits_init(); // Re-init to immediately change. NOTE: Nice to have but could be problematic later. break; - case 20: + case 24: if (value) { settings.flags |= BITFLAG_HOMING_ENABLE; } else { settings.flags &= ~BITFLAG_HOMING_ENABLE; } break; - case 21: settings.homing_dir_mask = trunc(value); break; - case 22: settings.homing_feed_rate = value; break; - case 23: settings.homing_seek_rate = value; break; - case 24: settings.homing_debounce_delay = round(value); break; - case 25: settings.homing_pulloff = value; break; + case 25: settings.homing_dir_mask = trunc(value); break; + case 26: settings.homing_feed_rate = value; break; + case 27: settings.homing_seek_rate = value; break; + case 28: settings.homing_debounce_delay = round(value); break; + case 29: settings.homing_pulloff = value; break; default: return(STATUS_INVALID_STATEMENT); } diff --git a/settings.h b/settings.h index 221c4fb..3369845 100644 --- a/settings.h +++ b/settings.h @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -29,7 +29,7 @@ // 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 -#define SETTINGS_VERSION 52 +#define SETTINGS_VERSION 53 // Define bit flag masks for the boolean settings in settings.flag. #define BITFLAG_REPORT_INCHES bit(0) @@ -37,6 +37,7 @@ #define BITFLAG_INVERT_ST_ENABLE bit(2) #define BITFLAG_HARD_LIMIT_ENABLE bit(3) #define BITFLAG_HOMING_ENABLE bit(4) +#define BITFLAG_SOFT_LIMIT_ENABLE bit(5) // Define EEPROM memory address location values for Grbl settings and parameters // NOTE: The Atmega328p has 1KB EEPROM. The upper half is reserved for parameters and @@ -74,7 +75,7 @@ typedef struct { uint8_t stepper_idle_lock_time; // If max value 255, steppers do not disable. uint8_t decimal_places; float max_velocity[N_AXIS]; -// float mm_soft_limit[N_AXIS]; + float max_travel[N_AXIS]; // uint8_t status_report_mask; // Mask to indicate desired report data. } settings_t; extern settings_t settings; From 49f703bb2c3ab8816ef7451b34cef078f31701fb Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Fri, 22 Mar 2013 08:45:46 -0600 Subject: [PATCH 04/13] Update README.md --- README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 88bd4fb..660a778 100644 --- a/README.md +++ b/README.md @@ -11,13 +11,15 @@ Grbl includes full acceleration management with look ahead. That means the contr ##Changelog for v0.9 from v0.8 - **ALPHA status: Under heavy development.** - - New stepper algorithm: Based on the Pramod Ranade inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. + - New stepper algorithm: Based on the Pramod Ranade inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. Overall, a much better, smoother stepper algorithm with the capability of very high speeds. - Planner optimizations: Multiple changes to increase planner execution speed and removed redundant variables. - Acceleration independence: Each axes may be defined with different acceleration parameters and Grbl will automagically calculate the maximum acceleration through a path depending on the direction traveled. This is very useful for machine that have very different axes properties, like the ShapeOko z-axis. - Maximum velocity independence: As with acceleration, the maximum velocity of individual axes may be defined. All seek/rapids motions will move at these maximum rates, but never exceed any one axes. So, when two or more axes move, the limiting axis will move at its maximum rate, while the other axes are scaled down. - Significantly improved arc performance: Arcs are now defined in terms of chordal tolerance, rather than segment length. Chordal tolerance will automatically scale all arc line segments depending on arc radius, such that the error does not exceed the tolerance value (default: 0.005 mm.) So, for larger radii arcs, Grbl can move faster through them, because the segments are always longer and the planner has more distance to plan with. + - Soft limits: Checks if any motion command exceeds workspace limits. Alarms out when found. Another safety feature, but, unlike hard limits, position does not get lost, as it forces a feed hold before erroring out. - New Grbl SIMULATOR by @jgeisler: A completely independent wrapper of the Grbl main source code that may be compiled as an executable on a computer. No Arduino required. Simply simulates the responses of Grbl as if it was on an Arduino. May be used for many things: checking out how Grbl works, pre-process moves for GUI graphics, debugging of new features, etc. Much left to do, but potentially very powerful, as the dummy AVR variables can be written to output anything you need. + - Homing routine updated: Sets workspace volume in all negative space regardless of limit switch position. Common on pro CNCs. Also reduces soft limits CPU overhead. - Feedrate overrides: In the works, but planner has begun to be re-factored for this feature. - - Jogging controls: Methodology needs to be to figured out first. Last item on the agenda. + - Jogging controls: Methodology needs to be to figured out first. Could be dropped due to flash space concerns. Last item on the agenda. _The project was initially inspired by the Arduino GCode Interpreter by Mike Ellery_ From 08baabc63cd74ba18ca633a58c482934c4b305cc Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Thu, 28 Mar 2013 10:11:34 -0600 Subject: [PATCH 05/13] Minor updates to code and commenting. --- gcode.c | 16 +++++++++------- limits.c | 22 ++++++++-------------- motion_control.c | 13 ++++++++++--- stepper.c | 7 +++---- 4 files changed, 30 insertions(+), 28 deletions(-) diff --git a/gcode.c b/gcode.c index b52de4b..b17a34a 100644 --- a/gcode.c +++ b/gcode.c @@ -77,8 +77,9 @@ static float to_millimeters(float value) // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase // characters and signed floating point values (no whitespace). Comments and block delete -// characters have been removed. All units and positions are converted and exported to grbl's -// internal functions in terms of (mm, mm/min) and absolute machine coordinates, respectively. +// characters have been removed. In this function, all units and positions are converted and +// exported to grbl's internal functions in terms of (mm, mm/min) and absolute machine +// coordinates, respectively. uint8_t gc_execute_line(char *line) { @@ -203,7 +204,10 @@ uint8_t gc_execute_line(char *line) /* Pass 2: Parameters. All units converted according to current block commands. Position parameters are converted and flagged to indicate a change. These can have multiple connotations - for different commands. Each will be converted to their proper value upon execution. */ + for different commands. Each will be converted to their proper value upon execution. + NOTE: Grbl unconventionally pre-converts these parameter values based on the block G and M + commands. This is set out of the order of execution defined by NIST only for code efficiency/size + purposes, but should not affect proper g-code execution. */ float p = 0, r = 0; uint8_t l = 0; char_counter = 0; @@ -242,10 +246,8 @@ uint8_t gc_execute_line(char *line) if (gc.status_code) { return(gc.status_code); } - /* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41. - NOTE: Independent non-motion/settings parameters are set out of this order for code efficiency - and simplicity purposes, but this should not affect proper g-code execution. */ - + /* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41. */ + // ([F]: Set feed rate.) if (sys.state != STATE_CHECK_MODE) { diff --git a/limits.c b/limits.c index a235057..3507785 100644 --- a/limits.c +++ b/limits.c @@ -96,22 +96,16 @@ static void homing_cycle(uint8_t cycle_mask, int8_t pos_dir, bool invert_pin, fl // and speedy homing routine. // NOTE: For each axes enabled, the following calculations assume they physically move // an equal distance over each time step until they hit a limit switch, aka dogleg. - uint32_t steps[N_AXIS]; - uint8_t dist = 0; + uint32_t step_event_count, steps[N_AXIS]; + uint8_t i, dist = 0; clear_vector(steps); - if (cycle_mask & (1<rate_delta) { st_go_idle(); bit_true(sys.execute,EXEC_CYCLE_STOP); - busy = false; return; } } @@ -330,7 +329,7 @@ void st_init() TCNT2 = 0; // Clear Timer2 counter TCCR2A = (1< Date: Fri, 5 Apr 2013 09:21:52 -0600 Subject: [PATCH 06/13] Updates to edge/dev. Line buffer increased/planner buffer decreased. Line overflow feedback. - Increased g-code parser line buffer to 70 characters (from 50) to prevent some long arc commands from getting truncated. - Decreased planner buffer from 18 to 17 blocks to free up memory for line buffer. - Added a line buffer overflow feedback error (Thanks @BHSPitMonkey!) --- config.h | 4 +- gcode.c | 7 +- planner.h | 2 +- protocol.c | 21 ++- protocol.h | 2 +- report.c | 2 + report.h | 1 + stepper.c | 6 + stepper_new.c | 424 ++++++++++++++++++++++++++++++++++++++++++++++++++ 9 files changed, 454 insertions(+), 15 deletions(-) create mode 100644 stepper_new.c diff --git a/config.h b/config.h index d1587fa..3791b31 100644 --- a/config.h +++ b/config.h @@ -210,7 +210,7 @@ // available RAM, like when re-compiling for a Mega or Sanguino. Or decrease if the Arduino // begins to crash due to the lack of available RAM or if the CPU is having trouble keeping // up with planning new incoming motions as they are executed. -// #define BLOCK_BUFFER_SIZE 18 // Uncomment to override default in planner.h. +// #define BLOCK_BUFFER_SIZE 17 // Uncomment to override default in planner.h. // Line buffer size from the serial input stream to be executed. Also, governs the size of // each of the startup blocks, as they are each stored as a string of this size. Make sure @@ -220,7 +220,7 @@ // can be too small and g-code blocks can get truncated. Officially, the g-code standards // support up to 256 characters. In future versions, this default will be increased, when // we know how much extra memory space we can re-invest into this. -// #define LINE_BUFFER_SIZE 50 // Uncomment to override default in protocol.h +// #define LINE_BUFFER_SIZE 70 // Uncomment to override default in protocol.h // Serial send and receive buffer size. The receive buffer is often used as another streaming // buffer to store incoming blocks to be processed by Grbl when its ready. Most streaming diff --git a/gcode.c b/gcode.c index b17a34a..d87ce52 100644 --- a/gcode.c +++ b/gcode.c @@ -245,7 +245,8 @@ uint8_t gc_execute_line(char *line) // If there were any errors parsing this line, we will return right away with the bad news if (gc.status_code) { return(gc.status_code); } - + uint8_t i; + /* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41. */ // ([F]: Set feed rate.) @@ -290,7 +291,6 @@ uint8_t gc_execute_line(char *line) else { int_value = gc.coord_select; } // Index P0 as the active coordinate system float coord_data[N_AXIS]; if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); } - uint8_t i; // Update axes defined only in block. Always in machine coordinates. Can change non-active system. for (i=0; i= LINE_BUFFER_SIZE-1) { - // Throw away any characters beyond the end of the line buffer + // Detect line buffer overflow. Report error and reset line buffer. + report_status_message(STATUS_OVERFLOW); + protocol_reset_line_buffer(); } else if (c >= 'a' && c <= 'z') { // Upcase lowercase line[char_counter++] = c-'a'+'A'; } else { diff --git a/protocol.h b/protocol.h index 209d19d..4d90c1c 100644 --- a/protocol.h +++ b/protocol.h @@ -30,7 +30,7 @@ // memory space we can invest into here or we re-write the g-code parser not to have his // buffer. #ifndef LINE_BUFFER_SIZE - #define LINE_BUFFER_SIZE 50 + #define LINE_BUFFER_SIZE 70 #endif // Initialize the serial protocol diff --git a/report.c b/report.c index c115dc7..8c90a17 100644 --- a/report.c +++ b/report.c @@ -76,6 +76,8 @@ void report_status_message(uint8_t status_code) printPgmString(PSTR("Alarm lock")); break; case STATUS_SOFT_LIMIT_ERROR: printPgmString(PSTR("Homing not enabled")); break; + case STATUS_OVERFLOW: + printPgmString(PSTR("Line overflow")); break; } printPgmString(PSTR("\r\n")); } diff --git a/report.h b/report.h index cd7f42d..7dcfc47 100644 --- a/report.h +++ b/report.h @@ -36,6 +36,7 @@ #define STATUS_IDLE_ERROR 11 #define STATUS_ALARM_LOCK 12 #define STATUS_SOFT_LIMIT_ERROR 13 +#define STATUS_OVERFLOW 14 // Define Grbl alarm codes. Less than zero to distinguish alarm error from status error. #define ALARM_LIMIT_ERROR -1 diff --git a/stepper.c b/stepper.c index cac20f3..acb3282 100644 --- a/stepper.c +++ b/stepper.c @@ -78,6 +78,7 @@ static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being // 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. void st_wake_up() @@ -101,6 +102,7 @@ void st_wake_up() } } + // Stepper shutdown void st_go_idle() { @@ -297,6 +299,7 @@ ISR(TIMER2_COMPA_vect) // SPINDLE_ENABLE_PORT ^= 1<. +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define CRUISE_RAMP 0 +#define ACCEL_RAMP 1 +#define DECEL_RAMP 2 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + uint32_t event_count; // Total event count. Retained for feed holds. + uint32_t step_events_remaining; // Steps remaining in motion + + // Used by Pramod Ranade inverse time algorithm + int32_t delta_d; // Ranade distance traveled per interrupt tick + int32_t d_counter; // Ranade distance traveled since last step event + uint8_t ramp_count; // Acceleration interrupt tick counter. + uint8_t ramp_type; // Ramp type variable. + uint8_t execute_step; // Flags step execution for each interrupt. + +} 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 + +// 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. + +// __________________________ +// /| |\ _________________ ^ +// / | | \ /| |\ | +// / | | \ / | | \ s +// / | | | | | \ p +// / | | | | | \ e +// +-----+------------------------+---+--+---------------+----+ e +// | BLOCK 1 | BLOCK 2 | d +// +// time -----> +// +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta +// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after +// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as +// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. + + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT0 = 0; // Clear Timer2 + TIMSK0 |= (1<d_next; + + // Load next step + 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) { + out_bits |= (1<steps_z; + if (st.counter_z < 0) { + out_bits |= (1<direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + + // Initialize Bresenham variables + st.counter_x = (current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + st.event_count = current_block->step_event_count; + st.step_events_remaining = st.event_count; + + // During feed hold, do not update inverse time counter, rate, or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { + // Initialize Ranade variables + st.d_counter = current_block->d_next; + st.delta_d = current_block->initial_rate; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + + // Initialize ramp type. + if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } + else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } + else { st.ramp_type = ACCEL_RAMP; } + } + + } else { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + } + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { + // Tick acceleration ramp counter + st.ramp_count--; + if (st.ramp_count == 0) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration + st.delta_d += current_block->rate_delta; + if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. + st.ramp_type = CRUISE_RAMP; + st.delta_d = current_block->nominal_rate; // Set cruise velocity + } + } 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 >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + } + } + } + } + + + // Check for feed hold state and execute accordingly. + if (sys.state == STATE_HOLD) { + if (st.ramp_type != DECEL_RAMP) { + st.ramp_type = DECEL_RAMP; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + } + if (st.delta_d <= current_block->rate_delta) { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); + return; + } + } + + if (st.ramp_type != DECEL_RAMP) { + // Acceleration and cruise handled by ramping. Just check for deceleration. + if (st.step_events_remaining <= current_block->decelerate_after) { + st.ramp_type = DECEL_RAMP; + if (st.step_events_remaining == current_block->decelerate_after) { + if (st.delta_d == current_block->nominal_rate) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + } + } + } + } else { + + busy = false; +} + + +// The Stepper Port Reset Interrupt: Timer2 OVF interrupt handles the falling edge of the +// step pulse. This should always trigger before the next Timer0 COMPA interrupt and independently +// finish, if Timer0 is disabled after completing a move. +ISR(TIMER2_OVF_vect) +{ + STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); + TCCR2B = 0; // Disable timer until needed. +} + + +// Reset and clear stepper subsystem variables +void st_reset() +{ + memset(&st, 0, sizeof(st)); + current_block = NULL; + busy = false; +} + + +// Initialize and start the stepper motor subsystem +void st_init() +{ + // Configure directions of interface pins + STEPPING_DDR |= STEPPING_MASK; + STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask; + STEPPERS_DISABLE_DDR |= 1< Date: Mon, 19 Aug 2013 09:24:22 -0600 Subject: [PATCH 07/13] Push old dev_2 draft to work on other things. - **NON-FUNCTIONAL** - Contains an old draft of separating the stepper driver direct access to the planner buffer. This is designed to keep the stepper and planner modules independent and prevent overwriting or other complications. In this way, feedrate override should be able to be installed as well. - A number of planner optimizations are installed too. - Not sure where the bugs are. Either in the new planner optimizations, new stepper module updates, or in both. Or it just could be that the Arduino AVR is choking with the new things it has to do. --- config.h | 9 +- gcode.c | 270 +++++----- gcode.h | 8 +- limits.c | 42 +- main.c | 5 +- motion_control.c | 33 +- nuts_bolts.c | 15 +- nuts_bolts.h | 3 +- planner.c | 577 +++++++++----------- planner.h | 49 +- planner_old.c | 476 +++++++++++++++++ planner_old.h | 83 +++ print.c | 2 +- print.h | 2 + protocol.c | 1 + report.c | 6 +- settings.c | 11 +- stepper.c | 571 ++++++++++++++------ stepper.h | 2 + stepper_new2.c | 601 +++++++++++++++++++++ stepper_new.c => stepper_new_dual_ISR.c | 31 +- stepper_new_time.c | 680 ++++++++++++++++++++++++ stepper_old.c | 387 ++++++++++++++ 23 files changed, 3160 insertions(+), 704 deletions(-) create mode 100644 planner_old.c create mode 100644 planner_old.h create mode 100644 stepper_new2.c rename stepper_new.c => stepper_new_dual_ISR.c (96%) create mode 100644 stepper_new_time.c create mode 100644 stepper_old.c diff --git a/config.h b/config.h index 3791b31..c1c450e 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_ZEN_TOOLWORKS_7x7 // Serial baud rate #define BAUD_RATE 9600 @@ -113,7 +113,7 @@ // 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. -#define ISR_TICKS_PER_SECOND 30000L // Integer (Hz) +#define ISR_TICKS_PER_SECOND 20000L // Integer (Hz) // The temporal resolution of the acceleration management subsystem. Higher number give smoother // acceleration but may impact performance. If you run at very high feedrates (>15kHz or so) and @@ -121,7 +121,8 @@ // 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 120L +// NOTE: Ramp count variable type in stepper module may need to be updated if changed. +#define ACCELERATION_TICKS_PER_SECOND 100L // NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. #define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) @@ -134,7 +135,7 @@ // 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 +#define INV_TIME_MULTIPLIER 10000000.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 diff --git a/gcode.c b/gcode.c index d87ce52..7623d1e 100644 --- a/gcode.c +++ b/gcode.c @@ -39,7 +39,9 @@ parser_state_t gc; #define FAIL(status) gc.status_code = status; -static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter); +static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter); +static void gc_convert_arc_radius_mode(float *target) __attribute__((noinline)); + static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) { @@ -48,6 +50,7 @@ static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) gc.plane_axis_2 = axis_2; } + void gc_init() { memset(&gc, 0, sizeof(gc)); @@ -61,20 +64,24 @@ void gc_init() } } + // Sets g-code parser position in mm. Input in steps. Called by the system abort and hard // limit pull-off routines. -void gc_set_current_position(int32_t x, int32_t y, int32_t z) +void gc_sync_position(int32_t x, int32_t y, int32_t z) { - gc.position[X_AXIS] = x/settings.steps_per_mm[X_AXIS]; - gc.position[Y_AXIS] = y/settings.steps_per_mm[Y_AXIS]; - gc.position[Z_AXIS] = z/settings.steps_per_mm[Z_AXIS]; + uint8_t i; + for (i=0; i C -----------------+--------------- T <- [x,y] - | <------ d/2 ---->| - - C - Current position - T - Target position - O - center of circle that pass through both C and T - d - distance from C to T - r - designated radius - h - distance from center of CT to O - - Expanding the equations: - - d -> sqrt(x^2 + y^2) - h -> sqrt(4 * r^2 - x^2 - y^2)/2 - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - - Which can be written: - - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - - Which we for size and speed reasons optimize to: - - h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) - i = (x - (y * h_x2_div_d))/2 - j = (y + (x * h_x2_div_d))/2 - - */ - - // Calculate the change in position along each selected axis - float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0]; - float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; - - clear_vector(offset); - // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller - // than d. If so, the sqrt of a negative number is complex and error out. - float h_x2_div_d = 4 * r*r - x*x - y*y; - if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); } - // Finish computing h_x2_div_d. - h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d) - // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) - if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } - - /* The counter clockwise circle lies to the left of the target direction. When offset is positive, - the left hand circle will be generated - when it is negative the right hand circle is generated. - - - T <-- Target position - - ^ - Clockwise circles with this center | Clockwise circles with this center will have - will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! - \ | / - center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative - | - | - - C <-- Current position */ - - - // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), - // even though it is advised against ever generating such circles in a single line of g-code. By - // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of - // travel and thus we get the unadvisably long arcs as prescribed. - if (r < 0) { - h_x2_div_d = -h_x2_div_d; - r = -r; // Finished with r. Set to positive for mc_arc - } - // Complete the operation by calculating the actual center of the arc - offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d)); - offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d)); - + if (gc.arc_radius != 0) { // Arc Radius Mode + // Compute arc radius and offsets + gc_convert_arc_radius_mode(target); + if (gc.status_code) { return(gc.status_code); } } else { // Arc Center Format Offset Mode - r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc + gc.arc_radius = hypot(gc.arc_offset[gc.plane_axis_0], gc.arc_offset[gc.plane_axis_1]); // Compute arc radius for mc_arc } // Set clockwise/counter-clockwise sign for mc_arc computations @@ -523,9 +443,9 @@ uint8_t gc_execute_line(char *line) if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; } // Trace the arc - mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, + mc_arc(gc.position, target, gc.arc_offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, - r, isclockwise); + gc.arc_radius, isclockwise); } break; } @@ -557,7 +477,7 @@ uint8_t gc_execute_line(char *line) // Parses the next statement and leaves the counter on the first character following // the statement. Returns 1 if there was a statements, 0 if end of string was reached // or there was an error (check state.status_code). -static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter) +static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter) { if (line[*char_counter] == 0) { return(0); // No more statements @@ -576,6 +496,100 @@ static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *c return(1); } + +static void gc_convert_arc_radius_mode(float *target) +{ +/* We need to calculate the center of the circle that has the designated radius and passes + through both the current position and the target position. This method calculates the following + set of equations where [x,y] is the vector from current to target position, d == magnitude of + that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to + the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the + length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point + [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc. + + d^2 == x^2 + y^2 + h^2 == r^2 - (d/2)^2 + i == x/2 - y/d*h + j == y/2 + x/d*h + + O <- [i,j] + - | + r - | + - | + - | h + - | + [0,0] -> C -----------------+--------------- T <- [x,y] + | <------ d/2 ---->| + + C - Current position + T - Target position + O - center of circle that pass through both C and T + d - distance from C to T + r - designated radius + h - distance from center of CT to O + + Expanding the equations: + + d -> sqrt(x^2 + y^2) + h -> sqrt(4 * r^2 - x^2 - y^2)/2 + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + + Which can be written: + + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + + Which we for size and speed reasons optimize to: + + h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) + i = (x - (y * h_x2_div_d))/2 + j = (y + (x * h_x2_div_d))/2 */ + + // Calculate the change in position along each selected axis + float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0]; + float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; + + clear_vector(gc.arc_offset); + // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller + // than d. If so, the sqrt of a negative number is complex and error out. + float h_x2_div_d = 4 * gc.arc_radius*gc.arc_radius - x*x - y*y; + if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return; } + // Finish computing h_x2_div_d. + h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d) + // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) + if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } + + /* The counter clockwise circle lies to the left of the target direction. When offset is positive, + the left hand circle will be generated - when it is negative the right hand circle is generated. + + + T <-- Target position + + ^ + Clockwise circles with this center | Clockwise circles with this center will have + will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! + \ | / + center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative + | + | + + C <-- Current position */ + + + // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), + // even though it is advised against ever generating such circles in a single line of g-code. By + // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of + // travel and thus we get the unadvisably long arcs as prescribed. + if (gc.arc_radius < 0) { + h_x2_div_d = -h_x2_div_d; + gc.arc_radius = -gc.arc_radius; // Finished with r. Set to positive for mc_arc + } + // Complete the operation by calculating the actual center of the arc + gc.arc_offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d)); + gc.arc_offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d)); +} + /* Not supported: diff --git a/gcode.h b/gcode.h index 50b4228..6b8949a 100644 --- a/gcode.h +++ b/gcode.h @@ -82,7 +82,11 @@ typedef struct { float coord_system[N_AXIS]; // Current work coordinate system (G54+). Stores offset from absolute machine // position in mm. Loaded from EEPROM when called. float coord_offset[N_AXIS]; // Retains the G92 coordinate offset (work coordinates) relative to - // machine zero in mm. Non-persistent. Cleared upon reset and boot. + // machine zero in mm. Non-persistent. Cleared upon reset and boot. + + float arc_radius; + float arc_offset[N_AXIS]; + } parser_state_t; extern parser_state_t gc; @@ -93,6 +97,6 @@ void gc_init(); uint8_t gc_execute_line(char *line); // Set g-code parser position. Input in steps. -void gc_set_current_position(int32_t x, int32_t y, int32_t z); +void gc_sync_position(); #endif diff --git a/limits.c b/limits.c index 3507785..a03d32e 100644 --- a/limits.c +++ b/limits.c @@ -96,16 +96,17 @@ static void homing_cycle(uint8_t cycle_mask, int8_t pos_dir, bool invert_pin, fl // and speedy homing routine. // NOTE: For each axes enabled, the following calculations assume they physically move // an equal distance over each time step until they hit a limit switch, aka dogleg. - uint32_t step_event_count, steps[N_AXIS]; + uint32_t step_event_count = 0; uint8_t i, dist = 0; + uint32_t steps[N_AXIS]; clear_vector(steps); for (i=0; i 0 || target[X_AXIS] < -settings.max_travel[X_AXIS] || - target[Y_AXIS] > 0 || target[Y_AXIS] < -settings.max_travel[Y_AXIS] || - target[Z_AXIS] > 0 || target[Z_AXIS] < -settings.max_travel[Z_AXIS] ) { - - // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within - // workspace volume so just come to a controlled stop so position is not lost. When complete - // enter alarm mode. - if (sys.state == STATE_CYCLE) { - st_feed_hold(); - while (sys.state == STATE_HOLD) { - protocol_execute_runtime(); - if (sys.abort) { return; } - } - } + uint8_t idx; + for (idx=0; idx 0 || target[idx] < settings.max_travel[idx]) { // NOTE: max_travel is stored as negative - mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. - sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event - protocol_execute_runtime(); // Execute to enter critical event loop and system abort + // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within + // workspace volume so just come to a controlled stop so position is not lost. When complete + // enter alarm mode. + if (sys.state == STATE_CYCLE) { + st_feed_hold(); + while (sys.state == STATE_HOLD) { + protocol_execute_runtime(); + if (sys.abort) { return; } + } + } + + mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. + sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event + protocol_execute_runtime(); // Execute to enter critical event loop and system abort + return; + + } } } diff --git a/main.c b/main.c index 84b0313..9decae8 100644 --- a/main.c +++ b/main.c @@ -72,7 +72,8 @@ int main(void) // Sync cleared gcode and planner positions to current system position, which is only // cleared upon startup, not a reset/abort. - sys_sync_current_position(); + plan_sync_position(); + gc_sync_position(); // Reset system variables. sys.abort = false; @@ -101,12 +102,12 @@ int main(void) } protocol_execute_runtime(); - protocol_process(); // ... process the serial protocol // When the serial protocol returns, there are no more characters in the serial read buffer to // be processed and executed. This indicates that individual commands are being issued or // streaming is finished. In either case, auto-cycle start, if enabled, any queued moves. mc_auto_cycle_start(); + protocol_process(); // ... process the serial protocol } return 0; /* never reached */ diff --git a/motion_control.c b/motion_control.c index 2a2f508..e420c98 100644 --- a/motion_control.c +++ b/motion_control.c @@ -72,7 +72,7 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) else { break; } } while (1); - plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); + plan_buffer_line(target, feed_rate, invert_feed_rate); // If idle, indicate to the system there is now a planned block in the buffer ready to cycle // start. Otherwise ignore and continue on. @@ -148,8 +148,8 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 This is important when there are successive arc motions. */ // Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec - float cos_T = 2 - theta_per_segment*theta_per_segment; - float sin_T = theta_per_segment*0.16666667*(cos_T + 4); + float cos_T = 2.0 - theta_per_segment*theta_per_segment; + float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0); cos_T *= 0.5; float arc_target[N_AXIS]; @@ -229,37 +229,34 @@ void mc_go_home() // At the same time, set up pull-off maneuver from axes limit switches that have been homed. // This provides some initial clearance off the switches and should also help prevent them // from falsely tripping when hard limits are enabled. - // TODO: Need to improve dir_mask[] to be more axes independent. float pulloff_target[N_AXIS]; clear_vector_float(pulloff_target); // Zero pulloff target. clear_vector_long(sys.position); // Zero current position for now. - uint8_t dir_mask[N_AXIS]; - dir_mask[X_AXIS] = (1< + - +--------+ <- 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. - - 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 n_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_sqr, float exit_speed_sqr) -{ - // Compute new initial rate for stepper algorithm - block->initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // TODO: Compute new nominal rate if a feedrate override occurs. - // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // Compute efficiency variable for following calculations. Removes a float divide and multiply. - // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds. - float steps_per_mm_div_2_acc = block->step_event_count/(2*block->acceleration*block->millimeters); - - // First determine intersection distance (in steps) from the exit point for a triangular profile. - // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) - int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) ); - - // 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 (in steps) 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. - // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) - block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr)); - - // 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 @@ -185,178 +126,130 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, */ static void planner_recalculate() { - -// float entry_speed_sqr; -// uint8_t block_index = block_buffer_head; -// block_t *previous = NULL; -// block_t *current = NULL; -// block_t *next; -// while (block_index != block_buffer_tail) { -// block_index = prev_block_index( block_index ); -// next = current; -// current = previous; -// previous = &block_buffer[block_index]; -// -// if (next && current) { -// if (next != block_buffer_planned) { -// if (previous == block_buffer_tail) { block_buffer_planned = next; } -// else { -// -// if (current->entry_speed_sqr != current->max_entry_speed_sqr) { -// current->recalculate_flag = true; // Almost always changes. So force recalculate. -// entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; -// if (entry_speed_sqr < current->max_entry_speed_sqr) { -// current->entry_speed_sqr = entry_speed_sqr; -// } else { -// current->entry_speed_sqr = current->max_entry_speed_sqr; -// } -// } else { -// block_buffer_planned = current; -// } -// } -// } else { -// break; -// } -// } -// } -// -// block_index = block_buffer_planned; -// next = &block_buffer[block_index]; -// current = prev_block_index(block_index); -// while (block_index != block_buffer_head) { -// -// // If the current 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 exit speed accordingly. Entry -// // speeds have already been reset, maximized, and reverse planned by reverse planner. -// if (current->entry_speed_sqr < next->entry_speed_sqr) { -// // Compute block exit speed based on the current block speed and distance -// // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance -// entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; -// -// // If it's less than the stored value, update the exit speed and set recalculate flag. -// if (entry_speed_sqr < next->entry_speed_sqr) { -// next->entry_speed_sqr = entry_speed_sqr; -// next->recalculate_flag = true; -// } -// } -// -// // 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_sqr, next->entry_speed_sqr); -// current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed -// } -// -// current = next; -// next = &block_buffer[block_index]; -// 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_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); -// next->recalculate_flag = false; + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + uint8_t block_index = block_buffer_head; + plan_block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer - // 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. + // Ping the stepper algorithm to check if we can alter the parameters of the currently executing + // block. If not, skip it and work on the next block. + // TODO: Need to look into if there are conditions where this fails. + uint8_t block_buffer_safe = next_block_index( block_buffer_tail ); + + // TODO: Need to recompute buffer tail millimeters based on how much is completed. + + if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on -// if (block_buffer_head != block_buffer_tail) { - float entry_speed_sqr; + block_buffer_planned = block_buffer_safe; +// calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the - // tail(first) block to prevent over-writing of the initial entry speed. - uint8_t block_index = prev_block_index( block_buffer_head ); // Assume buffer is not empty. - block_t *current = &block_buffer[block_index]; // Head block-1 = Newly appended block - block_t *next; - if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); } - while (block_index != block_buffer_tail) { - next = current; - current = &block_buffer[block_index]; - - // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter - // the planner nominal speeds at any time. This calc could be done in the override handler, but - // this could require an additional variable to be stored to differentiate the programmed nominal - // speeds, max junction speed, and override speeds/scalar. - - // 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_sqr != current->max_entry_speed_sqr) { + } else { + + // TODO: need to account for the two block condition better. If the currently executing block + // is not safe, do we wait until its done? Can we treat the buffer head differently? + + // Calculate trapezoid for the last/newest block. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, + MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters); +// calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - current->entry_speed_sqr = current->max_entry_speed_sqr; - current->recalculate_flag = true; // Almost always changes. So force recalculate. - - if (next->entry_speed_sqr < current->max_entry_speed_sqr) { - // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance + + // Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease + // planning when: (1) the last optimal planned pointer is reached. (2) the safe block + // pointer is reached, whereby the planned pointer is updated. + float entry_speed_sqr; + plan_block_t *next; + block_index = prev_block_index(block_index); + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + + // Exit loop and update planned pointer when the tail/safe block is reached. + if (block_index == block_buffer_safe) { + block_buffer_planned = block_buffer_safe; + break; + } + + // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to + // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; if (entry_speed_sqr < current->max_entry_speed_sqr) { current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; } - } - } - block_index = prev_block_index( block_index ); - } + } + block_index = prev_block_index(block_index); + } - // Perform forward planner pass. Begins junction speed adjustments after tail(first) block. - // Also recalculate trapezoids, block by block, as the forward pass completes the plan. - block_index = next_block_index(block_buffer_tail); - next = &block_buffer[block_buffer_tail]; // Places tail(first) block into current - while (block_index != block_buffer_head) { - current = next; - next = &block_buffer[block_index]; - - // If the current 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 exit speed accordingly. Entry - // speeds have already been reset, maximized, and reverse planned by reverse planner. + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + block_index = block_buffer_planned; // Begin at buffer planned pointer + next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop + while (block_index != next_buffer_head) { + current = next; + next = &block_buffer[block_index]; + + // Any acceleration detected in the forward pass automatically moves the optimal planned + // pointer forward, since everything before this is all optimal. In other words, nothing + // can improve the plan from the buffer tail to the planned pointer by logic. if (current->entry_speed_sqr < next->entry_speed_sqr) { - // Compute block exit speed based on the current block speed and distance - // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance + block_buffer_planned = block_index; entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; - - // If it's less than the stored value, update the exit speed and set recalculate flag. if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; - next->recalculate_flag = true; + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this. } } - - // 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_sqr, next->entry_speed_sqr); - current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. The optimally planned pointer is updated. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; } - - block_index = next_block_index( block_index ); - } + + // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned + // pointer to the end of the buffer, except the last block. +// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); + 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_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - next->recalculate_flag = false; -// } } + void plan_init() { + block_buffer_head = 0; block_buffer_tail = block_buffer_head; next_buffer_head = next_block_index(block_buffer_head); -// block_buffer_planned = block_buffer_head; + block_buffer_planned = block_buffer_head; memset(&pl, 0, sizeof(pl)); // Clear planner struct } -inline void plan_discard_current_block() + +void plan_discard_current_block() { if (block_buffer_head != block_buffer_tail) { block_buffer_tail = next_block_index( block_buffer_tail ); } } -inline block_t *plan_get_current_block() + +plan_block_t *plan_get_current_block() { if (block_buffer_head == block_buffer_tail) { return(NULL); } return(&block_buffer[block_buffer_tail]); } + +plan_block_t *plan_get_block_by_index(uint8_t block_index) +{ + if (block_buffer_head == block_index) { return(NULL); } + return(&block_buffer[block_index]); +} + + // Returns the availability status of the block ring buffer. True, if full. uint8_t plan_check_full_buffer() { @@ -364,6 +257,7 @@ uint8_t plan_check_full_buffer() return(false); } + // Block until all buffered steps are executed or in a cycle state. Works with feed hold // during a synchronize call, if it should happen. Also, waits for clean cycle end. void plan_synchronize() @@ -374,43 +268,54 @@ void plan_synchronize() } } -// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in -// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed + +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. // All position data passed to the planner must be in terms of machine position to keep the planner // independent of any coordinate system changes and offsets, which are handled by the g-code parser. // NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. -// Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate -// is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate -// value is negative (and invert_feed_rate always false). -void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) +// In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value +// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if +// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and +// invert_feed_rate always false). +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) { - // Prepare to set up new block - block_t *block = &block_buffer[block_buffer_head]; + // Prepare and initialize new block + plan_block_t *block = &block_buffer[block_buffer_head]; + block->step_event_count = 0; + block->millimeters = 0; + block->direction_bits = 0; + block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later - // Calculate target position in absolute steps - int32_t target[N_AXIS]; - target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); - target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); - target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); + // Compute and store initial move distance data. + int32_t target_steps[N_AXIS]; + float unit_vec[N_AXIS], delta_mm; + uint8_t idx; + for (idx=0; idxsteps[idx] = labs(target_steps[idx]-pl.position[idx]); + block->step_event_count = max(block->step_event_count, block->steps[idx]); + + // Compute individual axes distance for move and prep unit vector calculations. + // NOTE: Computes true distance from converted step values. + delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx]; + unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. + + // Set direction bits. Bit enabled always means direction is negative. + if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } + + // Incrementally compute total move distance by Euclidean norm. First add square of each term. + block->millimeters += delta_mm*delta_mm; + } + block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() - // Number of steps for each axis - block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]); - block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]); - block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]); - block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); - // Bail if this is a zero-length block - if (block->step_event_count == 0) { return; }; + if (block->step_event_count == 0) { return; } - // Compute path vector in terms of absolute step target and current positions - float delta_mm[N_AXIS]; - delta_mm[X_AXIS] = x-pl.last_x; - delta_mm[Y_AXIS] = y-pl.last_y; - delta_mm[Z_AXIS] = z-pl.last_z; - block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + - delta_mm[Z_AXIS]*delta_mm[Z_AXIS]); - // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later @@ -422,117 +327,151 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert // and axes properties as well. // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. - uint8_t i; - float unit_vec[N_AXIS], inverse_unit_vec_value; + float inverse_unit_vec_value; float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides - block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration in loop - for (i=0; iacceleration = min(block->acceleration,settings.acceleration[i]*inverse_unit_vec_value); + float junction_cos_theta = 0; + for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); + + // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction + // between the current move and the previous move is simply the dot product of the two unit vectors, + // where prev_unit_vec is negative. Used later to compute maximum junction speed. + junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; } } - // Compute nominal speed and rates - block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 - block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + /* Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + Let a circle be tangent to both previous and current path line segments, where the junction + deviation is defined as the distance from the junction to the closest edge of the circle, + colinear with the circle center. The circular segment joining the two paths represents the + path of centripetal acceleration. Solve for max velocity based on max acceleration about the + radius of the circle, defined indirectly by junction deviation. This may be also viewed as + path width or max_jerk in the previous grbl version. This approach does not actually deviate + from path, but used as a robust way to compute cornering speeds, as it takes into account the + nonlinearities of both the junction angle and junction velocity. + NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + is exactly the same. Instead of motioning all the way to junction point, the machine will + just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + a continuous mode path, but ARM-based microcontrollers most certainly do. + */ + // TODO: Acceleration need to be limited by the minimum of the two junctions. + // TODO: Need to setup a method to handle zero junction speeds when starting from rest. + if (block_buffer_head == block_buffer_tail) { + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + } else { + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. + block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2); + } - // Compute the acceleration and distance traveled per step event for the stepper algorithm. - block->rate_delta = ceil(block->acceleration* - ((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step) - - // Compute direction bits. Bit enabled always means direction is negative. - block->direction_bits = 0; - if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; - if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed_sqr > 0.0)) { - // 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] ; - - // Skip and use default max junction speed for 0 degree acute junction. - if (cos_theta < 0.95) { - block->max_entry_speed_sqr = min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr); - // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. - if (cos_theta > -0.95) { - // Compute maximum junction velocity based on maximum acceleration and junction deviation - float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. - block->max_entry_speed_sqr = min(block->max_entry_speed_sqr, - block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)); - } - } - } - - // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED. - // TODO: This could be moved to the planner recalculate function. - block->entry_speed_sqr = min( block->max_entry_speed_sqr, - MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters); - - // Set new block to be recalculated for conversion to stepper data. - block->recalculate_flag = true; + // Store block nominal speed and rate + block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 +// block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) +// +// // Compute and store acceleration and distance traveled per step event. +// block->rate_delta = ceil(block->acceleration* +// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) +// block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step) // Update previous path unit_vector and nominal speed (squared) memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; // Update planner position - memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[] - pl.last_x = x; - pl.last_y = y; - pl.last_z = z; - - // Update buffer head and next buffer head indices - block_buffer_head = next_buffer_head; - next_buffer_head = next_block_index(block_buffer_head); + memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] planner_recalculate(); + + // Update buffer head and next buffer head indices. + // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan + // calculations to help prevent overwriting scenarios with adding a new block to a low buffer. + block_buffer_head = next_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); } + // Reset the planner position vectors. Called by the system abort/initialization routine. -void plan_set_current_position(int32_t x, int32_t y, int32_t z) +void plan_sync_position() { - pl.position[X_AXIS] = x; - pl.position[Y_AXIS] = y; - pl.position[Z_AXIS] = z; - pl.last_x = x/settings.steps_per_mm[X_AXIS]; - pl.last_y = y/settings.steps_per_mm[Y_AXIS]; - pl.last_z = z/settings.steps_per_mm[Z_AXIS]; + uint8_t idx; + for (idx=0; idx + + +--------+ <- 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. + + 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 n_steps until deceleration are computed, since the stepper algorithm + already handles acceleration and cruising and just needs to know when to start decelerating. +*/ +int32_t calculate_trapezoid_for_block(uint8_t block_index) +{ + plan_block_t *current_block = &block_buffer[block_index]; + + // Determine current block exit speed + float exit_speed_sqr; + uint8_t next_index = next_block_index(block_index); + plan_block_t *next_block = plan_get_block_by_index(next_index); + if (next_block == NULL) { exit_speed_sqr = 0; } // End of planner buffer. Zero speed. + else { exit_speed_sqr = next_block->entry_speed_sqr; } // Entry speed of next block + + // First determine intersection distance (in steps) from the exit point for a triangular profile. + // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) + float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) ); + + // 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 ) { + float decelerate_distance; + // Determine deceleration distance (in steps) 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. + // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) + decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration); + + // The lesser of the two triangle and trapezoid distances always defines the velocity profile. + if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; } + + // Finally, check if this is a pure deceleration block. + if (decelerate_distance > current_block->millimeters) { decelerate_distance = current_block->millimeters; } + + return(ceil(((current_block->millimeters-decelerate_distance)*current_block->step_event_count)/ current_block->millimeters)); + } + return(0); +} + + // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. // Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped. void plan_cycle_reinitialize(int32_t step_events_remaining) { - block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block + plan_block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block // Only remaining millimeters and step_event_count need to be updated for planner recalculate. // Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to @@ -540,9 +479,9 @@ void plan_cycle_reinitialize(int32_t step_events_remaining) block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count; block->step_event_count = step_events_remaining; - // Re-plan from a complete stop. Reset planner entry speeds and flags. + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. block->entry_speed_sqr = 0.0; - block->max_entry_speed_sqr = 0.0; - block->recalculate_flag = true; + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + block_buffer_planned = block_buffer_tail; planner_recalculate(); } diff --git a/planner.h b/planner.h index 830735d..e371bb0 100644 --- a/planner.h +++ b/planner.h @@ -2,8 +2,8 @@ planner.h - buffers movement commands and manages the acceleration profile plan Part of Grbl + Copyright (c) 2011-2013 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -21,10 +21,11 @@ #ifndef planner_h #define planner_h - +#include "nuts_bolts.h" + // The number of linear motions that can be in the plan at any give time #ifndef BLOCK_BUFFER_SIZE - #define BLOCK_BUFFER_SIZE 17 + #define BLOCK_BUFFER_SIZE 18 #endif // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in @@ -32,43 +33,43 @@ typedef struct { // Fields used by the bresenham algorithm for tracing the line - uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - uint32_t steps_x, steps_y, steps_z; // Step count along each axis - int32_t step_event_count; // The number of step events required to complete this block + uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + int32_t steps[N_AXIS]; // Step count along each axis + int32_t step_event_count; // The number of step events required to complete this block // Fields used by the motion planner to manage acceleration - float nominal_speed_sqr; // The nominal speed for this block in mm/min - float entry_speed_sqr; // Entry speed at previous-current block junction in mm/min - float max_entry_speed_sqr; // Maximum allowable junction entry speed in mm/min - float millimeters; // The total travel of this block in mm - float acceleration; - uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction - + float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 + float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2 + float acceleration; // Axes-limit adjusted line acceleration in mm/min^2 + float millimeters; // The total travel for this block to be executed in mm + // Settings for the trapezoid generator - uint32_t initial_rate; // The step rate at start of block - int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - 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; +// int32_t decelerate_after; // The index of the step event on which to start decelerating + +} plan_block_t; // Initialize the motion plan subsystem void plan_init(); -// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in -// millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. -void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate); +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate); // Called when the current block is no longer needed. Discards the block and makes the memory // availible for new blocks. void plan_discard_current_block(); // Gets the current block. Returns NULL if buffer empty -block_t *plan_get_current_block(); +plan_block_t *plan_get_current_block(); + +plan_block_t *plan_get_block_by_index(uint8_t block_index); + +int32_t calculate_trapezoid_for_block(uint8_t block_index); // Reset the planner position vector (in steps) -void plan_set_current_position(int32_t x, int32_t y, int32_t z); +void plan_sync_position(); // Reinitialize plan with a partially completed block void plan_cycle_reinitialize(int32_t step_events_remaining); diff --git a/planner_old.c b/planner_old.c new file mode 100644 index 0000000..1cf6fb4 --- /dev/null +++ b/planner_old.c @@ -0,0 +1,476 @@ +/* + planner.c - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 Jens Geisler + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ + +#include +#include +#include "planner.h" +#include "nuts_bolts.h" +#include "stepper.h" +#include "settings.h" +#include "config.h" +#include "protocol.h" + +#define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs + // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. + +static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions +static volatile uint8_t block_buffer_tail; // Index of the block to process now +static uint8_t block_buffer_head; // Index of the next block to be pushed +static uint8_t next_buffer_head; // Index of the next buffer head +static uint8_t block_buffer_planned; // Index of the optimally planned block + +// Define planner variables +typedef struct { + int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate + // from g-code position for movements requiring multiple line motions, + // i.e. arcs, canned cycles, and backlash compensation. + float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment + float previous_nominal_speed_sqr; // Nominal speed of previous path line segment + float last_target[N_AXIS]; // Target position of previous path line segment +} planner_t; +static planner_t pl; + + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static uint8_t next_block_index(uint8_t block_index) +{ + block_index++; + if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + + +// Returns the index of the previous block in the ring buffer +static uint8_t prev_block_index(uint8_t block_index) +{ + if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } + block_index--; + return(block_index); +} + + +/* 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. + + 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 n_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_sqr, float exit_speed_sqr) +{ + // Compute new initial rate for stepper algorithm + block->initial_rate = ceil(sqrt(entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Compute new nominal rate if a feedrate override occurs. + // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // Compute efficiency variable for following calculations. Removes a float divide and multiply. + // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds. + float steps_per_mm_div_2_acc = block->step_event_count/(2*block->acceleration*block->millimeters); + + // First determine intersection distance (in steps) from the exit point for a triangular profile. + // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) + int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) ); + + // 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 (in steps) 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. + // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) + block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr)); + + // 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 + / \ + current->entry_speed -> + \ + | + <- next->entry_speed + +-------------+ + 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. + + Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's + constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and + nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile + through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting + conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile. + + Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line + segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled + in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the + buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be + able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. + The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. + Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost + steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the + bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory + is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering + up to a hundred or more. + + NOTE: Since this function is constantly re-calculating for every new incoming block, it 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 then starve and empty, leading + to weird hiccup-like jerky motions. +*/ +static void planner_recalculate() +{ + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + uint8_t block_index = block_buffer_head; + block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer + + // Determine safe point for which to plan to. + uint8_t block_buffer_safe = next_block_index( block_buffer_tail ); + + if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on + + block_buffer_planned = block_buffer_safe; + calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + + } else { + + // TODO: need to account for the two block condition better. If the currently executing block + // is not safe, do we wait until its done? Can we treat the buffer head differently? + + // Calculate trapezoid for the last/newest block. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, + MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters); + calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + + + // Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease + // planning when: (1) the last optimal planned pointer is reached. (2) the safe block + // pointer is reached, whereby the planned pointer is updated. + float entry_speed_sqr; + block_t *next; + block_index = prev_block_index(block_index); + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + + // Exit loop and update planned pointer when the tail/safe block is reached. + if (block_index == block_buffer_safe) { + block_buffer_planned = block_buffer_safe; + break; + } + + // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to + // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; + } + } + block_index = prev_block_index(block_index); + } + + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + block_index = block_buffer_planned; // Begin at buffer planned pointer + next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop + while (block_index != next_buffer_head) { + current = next; + next = &block_buffer[block_index]; + + // Any acceleration detected in the forward pass automatically moves the optimal planned + // pointer forward, since everything before this is all optimal. In other words, nothing + // can improve the plan from the buffer tail to the planned pointer by logic. + if (current->entry_speed_sqr < next->entry_speed_sqr) { + block_buffer_planned = block_index; + entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this. + } + } + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. The optimally planned pointer is updated. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; + } + + // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned + // pointer to the end of the buffer, except the last block. + calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); + block_index = next_block_index( block_index ); + } + + } + +} + + +void plan_init() +{ + block_buffer_tail = block_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); + block_buffer_planned = block_buffer_head; + memset(&pl, 0, sizeof(pl)); // Clear planner struct +} + + +inline void plan_discard_current_block() +{ + if (block_buffer_head != block_buffer_tail) { + block_buffer_tail = next_block_index( block_buffer_tail ); + } +} + + +inline block_t *plan_get_current_block() +{ + if (block_buffer_head == block_buffer_tail) { return(NULL); } + return(&block_buffer[block_buffer_tail]); +} + + +// Returns the availability status of the block ring buffer. True, if full. +uint8_t plan_check_full_buffer() +{ + if (block_buffer_tail == next_buffer_head) { return(true); } + return(false); +} + + +// Block until all buffered steps are executed or in a cycle state. Works with feed hold +// during a synchronize call, if it should happen. Also, waits for clean cycle end. +void plan_synchronize() +{ + while (plan_get_current_block() || sys.state == STATE_CYCLE) { + protocol_execute_runtime(); // Check and execute run-time commands + if (sys.abort) { return; } // Check for system abort + } +} + + +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed +// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. +// All position data passed to the planner must be in terms of machine position to keep the planner +// independent of any coordinate system changes and offsets, which are handled by the g-code parser. +// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. +// In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value +// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if +// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and +// invert_feed_rate always false). +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) +{ + // Prepare and initialize new block + block_t *block = &block_buffer[block_buffer_head]; + block->step_event_count = 0; + block->millimeters = 0; + block->direction_bits = 0; + block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later + + // Compute and store initial move distance data. + int32_t target_steps[N_AXIS]; + float unit_vec[N_AXIS], delta_mm; + uint8_t idx; + for (idx=0; idxsteps[idx] = labs(target_steps[idx]-pl.position[idx]); + block->step_event_count = max(block->step_event_count, block->steps[idx]); + + // Compute individual axes distance for move and prep unit vector calculations. + delta_mm = target[idx] - pl.last_target[idx]; + unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. + + // Incrementally compute total move distance by Euclidean norm + block->millimeters += delta_mm*delta_mm; + + // Set direction bits. Bit enabled always means direction is negative. + if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } + } + block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation + + // Bail if this is a zero-length block + if (block->step_event_count == 0) { return; } + + // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) + // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. + if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later + else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } + + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited + // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so + // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction + // and axes properties as well. + // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, + // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. + float inverse_unit_vec_value; + float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides + float junction_cos_theta = 0; + for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); + + // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction + // between the current move and the previous move is simply the dot product of the two unit vectors, + // where prev_unit_vec is negative. Used later to compute maximum junction speed. + junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; + } + } + + /* Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + Let a circle be tangent to both previous and current path line segments, where the junction + deviation is defined as the distance from the junction to the closest edge of the circle, + colinear with the circle center. The circular segment joining the two paths represents the + path of centripetal acceleration. Solve for max velocity based on max acceleration about the + radius of the circle, defined indirectly by junction deviation. This may be also viewed as + path width or max_jerk in the previous grbl version. This approach does not actually deviate + from path, but used as a robust way to compute cornering speeds, as it takes into account the + nonlinearities of both the junction angle and junction velocity. + NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + is exactly the same. Instead of motioning all the way to junction point, the machine will + just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + a continuous mode path, but ARM-based microcontrollers most certainly do. + */ + // TODO: Acceleration need to be limited by the minimum of the two junctions. + // TODO: Need to setup a method to handle zero junction speeds when starting from rest. + if (block_buffer_head == block_buffer_tail) { + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + } else { + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. + block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2); + } + + // Store block nominal speed and rate + block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 + block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // Compute and store acceleration and distance traveled per step event. + block->rate_delta = ceil(block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step) + + // Update previous path unit_vector and nominal speed (squared) + memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + + // Update planner position + memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] + memcpy(pl.last_target, target, sizeof(target)); // pl.last_target[] = target[] + + planner_recalculate(); + + // Update buffer head and next buffer head indices. + // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan + // calculations to help prevent overwriting scenarios with adding a new block to a low buffer. + block_buffer_head = next_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); +} + + +// Reset the planner position vectors. Called by the system abort/initialization routine. +void plan_sync_position() +{ + uint8_t idx; + for (idx=0; idxmillimeters = (block->millimeters*step_events_remaining)/block->step_event_count; + block->step_event_count = step_events_remaining; + + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. + block->entry_speed_sqr = 0.0; + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + block_buffer_planned = block_buffer_tail; + planner_recalculate(); +} diff --git a/planner_old.h b/planner_old.h new file mode 100644 index 0000000..84ecc4b --- /dev/null +++ b/planner_old.h @@ -0,0 +1,83 @@ +/* + planner.h - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2011-2013 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 + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef planner_h +#define planner_h +#include "nuts_bolts.h" + +// The number of linear motions that can be in the plan at any give time +#ifndef BLOCK_BUFFER_SIZE + #define BLOCK_BUFFER_SIZE 17 +#endif + +// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in +// the source g-code and may never actually be reached if acceleration management is active. +typedef struct { + + // Fields used by the bresenham algorithm for tracing the line + uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + uint32_t steps[N_AXIS]; // Step count along each axis + int32_t step_event_count; // The number of step events required to complete this block + + // Fields used by the motion planner to manage acceleration + float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 + float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2 + float millimeters; // The total travel of this block in mm + float acceleration; // Axes-limit adjusted line acceleration in mm/min^2 + + // Settings for the trapezoid generator + uint32_t initial_rate; // The step rate at start of block + int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + 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 +void plan_init(); + +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed +// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate); + +// Called when the current block is no longer needed. Discards the block and makes the memory +// availible for new blocks. +void plan_discard_current_block(); + +// Gets the current block. Returns NULL if buffer empty +block_t *plan_get_current_block(); + +// Reset the planner position vector (in steps) +void plan_sync_position(); + +// Reinitialize plan with a partially completed block +void plan_cycle_reinitialize(int32_t step_events_remaining); + +// Returns the status of the block ring buffer. True, if buffer is full. +uint8_t plan_check_full_buffer(); + +// Block until all buffered steps are executed +void plan_synchronize(); + +#endif diff --git a/print.c b/print.c index 2f820d5..7a7686b 100644 --- a/print.c +++ b/print.c @@ -77,7 +77,7 @@ void print_uint8_base2(uint8_t n) serial_write('0' + buf[i - 1]); } -static void print_uint32_base10(unsigned long n) +void print_uint32_base10(unsigned long n) { unsigned char buf[10]; uint8_t i = 0; diff --git a/print.h b/print.h index 9983aee..8a161c1 100644 --- a/print.h +++ b/print.h @@ -31,6 +31,8 @@ void printPgmString(const char *s); void printInteger(long n); +void print_uint32_base10(uint32_t n); + void print_uint8_base2(uint8_t n); void printFloat(float n); diff --git a/protocol.c b/protocol.c index a75a141..253ec1f 100644 --- a/protocol.c +++ b/protocol.c @@ -104,6 +104,7 @@ ISR(PINOUT_INT_vect) // limit switches, or the main program. void protocol_execute_runtime() { + st_prep_buffer(); if (sys.execute) { // Enter only if any bit flag is true uint8_t rt_exec = sys.execute; // Avoid calling volatile multiple times diff --git a/report.c b/report.c index 8c90a17..e71ac54 100644 --- a/report.c +++ b/report.c @@ -157,9 +157,9 @@ void report_grbl_settings() { printPgmString(PSTR(" (z v_max, mm/min)\r\n$6=")); printFloat(settings.acceleration[X_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (x accel, mm/sec^2)\r\n$7=")); printFloat(settings.acceleration[Y_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (y accel, mm/sec^2)\r\n$8=")); printFloat(settings.acceleration[Z_AXIS]/(60*60)); // Convert from mm/min^2 for human readability - printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(settings.max_travel[X_AXIS]); - printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(settings.max_travel[Y_AXIS]); - printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(settings.max_travel[Z_AXIS]); + printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(-settings.max_travel[X_AXIS]); // Grbl internally store this as negative. + printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(-settings.max_travel[Y_AXIS]); // Grbl internally store this as negative. + printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(-settings.max_travel[Z_AXIS]); // Grbl internally store this as negative. printPgmString(PSTR(" (z max travel, mm)\r\n$12=")); printInteger(settings.pulse_microseconds); printPgmString(PSTR(" (step pulse, usec)\r\n$13=")); printFloat(settings.default_feed_rate); printPgmString(PSTR(" (default feed, mm/min)\r\n$14=")); printInteger(settings.invert_mask); diff --git a/settings.c b/settings.c index 9225a91..8f628a5 100644 --- a/settings.c +++ b/settings.c @@ -169,9 +169,9 @@ uint8_t settings_store_global_setting(int parameter, float value) { case 6: settings.acceleration[X_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 7: settings.acceleration[Y_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 8: settings.acceleration[Z_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. - case 9: settings.max_travel[X_AXIS] = value; break; - case 10: settings.max_travel[Y_AXIS] = value; break; - case 11: settings.max_travel[Z_AXIS] = value; break; + case 9: settings.max_travel[X_AXIS] = -value; break; // Store as negative for grbl internal use. + case 10: settings.max_travel[Y_AXIS] = -value; break; // Store as negative for grbl internal use. + case 11: settings.max_travel[Z_AXIS] = -value; break; // Store as negative for grbl internal use. case 12: if (value < 3) { return(STATUS_SETTING_STEP_PULSE_MIN); } settings.pulse_microseconds = round(value); break; @@ -206,7 +206,10 @@ uint8_t settings_store_global_setting(int parameter, float value) { break; case 24: if (value) { settings.flags |= BITFLAG_HOMING_ENABLE; } - else { settings.flags &= ~BITFLAG_HOMING_ENABLE; } + else { + settings.flags &= ~BITFLAG_HOMING_ENABLE; + settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; + } break; case 25: settings.homing_dir_mask = trunc(value); break; case 26: settings.homing_feed_rate = value; break; diff --git a/stepper.c b/stepper.c index acb3282..05ddd9e 100644 --- a/stepper.c +++ b/stepper.c @@ -19,20 +19,30 @@ along with Grbl. If not, see . */ -/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith - and Philipp Tiefenbacher. */ - #include #include "stepper.h" #include "config.h" #include "settings.h" #include "planner.h" +#include "nuts_bolts.h" // Some useful constants #define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define CRUISE_RAMP 0 -#define ACCEL_RAMP 1 -#define DECEL_RAMP 2 + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_LINE 1 +#define LOAD_BLOCK 2 + +#define ST_NOOP 0 +#define ST_END_OF_BLOCK 1 +#define ST_DECEL 2 +#define ST_DECEL_EOB 3 + +#define SEGMENT_BUFFER_SIZE 10 // Stepper state variable. Contains running data and trapezoid variables. typedef struct { @@ -40,44 +50,92 @@ typedef struct { int32_t counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z; - uint32_t event_count; // Total event count. Retained for feed holds. - uint32_t step_events_remaining; // Steps remaining in motion + uint8_t segment_steps_remaining; // Steps remaining in line motion - // Used by Pramod Ranade inverse time algorithm - int32_t delta_d; // Ranade distance traveled per interrupt tick - int32_t d_counter; // Ranade distance traveled since last step event - uint8_t ramp_count; // Acceleration interrupt tick counter. - uint8_t ramp_type; // Ramp type variable. - uint8_t execute_step; // Flags step execution for each interrupt. + // Used by inverse time algorithm to track step rate + int32_t counter_d; // Inverse time distance traveled since last step event + uint32_t delta_d; // Inverse time distance traveled per interrupt tick + uint32_t d_per_tick; + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; + + uint8_t ramp_count; + uint8_t ramp_type; } stepper_t; static stepper_t st; -static block_t *current_block; // A pointer to the block currently being traced -// Used by the stepper driver interrupt -static uint8_t step_pulse_time; // Step pulse reset time after step rise -static uint8_t out_bits; // The next stepping-bits to be output +// Stores stepper buffer common data. Can change planner mid-block in special conditions. +typedef struct { + int32_t step_events_remaining; // Tracks step event count for the executing planner block + uint32_t d_next; // Scaled distance to next step + uint32_t initial_rate; // Initialized step rate at re/start of a planner block + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + int32_t decelerate_after; + float mm_per_step; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; -// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then -// this blocking variable is no longer needed. Only here for safety reasons. -static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. +// Primary stepper motion buffer +typedef struct { + uint8_t n_step; + uint8_t st_data_index; + uint8_t flag; +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; -// __________________________ -// /| |\ _________________ ^ -// / | | \ /| |\ | -// / | | \ / | | \ s -// / | | | | | \ p -// / | | | | | \ e -// +-----+------------------------+---+--+---------------+----+ e -// | BLOCK 1 | BLOCK 2 | d -// -// time -----> -// -// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta -// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after -// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as -// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +static volatile uint8_t segment_buffer_tail; +static uint8_t segment_buffer_head; +static uint8_t segment_next_head; +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer +static uint8_t pl_prep_index; +static st_data_t *st_prep_data; +static uint8_t st_data_prep_index; + + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static uint8_t next_block_index(uint8_t block_index) +{ + block_index++; + if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + +static uint8_t next_block_pl_index(uint8_t block_index) +{ + block_index++; + if (block_index == 18) { block_index = 0; } + return(block_index); +} + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ // Stepper state initialization. Cycle should only start if the st.cycle_start flag is // enabled. Startup init and limits call this function but shouldn't start the cycle. @@ -91,14 +149,17 @@ void st_wake_up() } if (sys.state == STATE_CYCLE) { // Initialize stepper output bits - out_bits = settings.invert_mask; + st.out_bits = settings.invert_mask; // Initialize step pulse timing from settings. - step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); + st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Enable stepper driver interrupt st.execute_step = false; + st.load_flag = LOAD_BLOCK; + TCNT2 = 0; // Clear Timer2 TIMSK2 |= (1<direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits. + // NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is + // a small chance that this will load at the same time as a step event. Hopefully, + // the overhead for this loading event isn't too much.. possibly 2-5 usec. + + // NOTE: The stepper algorithm must control the planner buffer tail as it completes + // the block moves. Otherwise, a feed hold can leave a few step buffer line moves + // without the correct planner block information. - // Initialize Bresenham variables - st.counter_x = (current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - st.event_count = current_block->step_event_count; - st.step_events_remaining = st.event_count; + st_current_segment = &segment_buffer[segment_buffer_tail]; - // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { - // Initialize Ranade variables - st.d_counter = current_block->d_next; - st.delta_d = current_block->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + // Load number of steps to execute from stepper buffer + st.segment_steps_remaining = st_current_segment->n_step; + + // Check if the counters need to be reset for a new planner block + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index]; - // Initialize ramp type. - if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } - else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } - else { st.ramp_type = ACCEL_RAMP; } + // Initialize direction bits for block + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits upon next ISR tick. + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time and step rate counter data + st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + + // During feed hold, do not update rate or ramp type. Keep decelerating. +// if (sys.state == STATE_CYCLE) { + st.delta_d = st_current_data->initial_rate; +// if (st.delta_d == st_current_data->nominal_rate) { +// st.ramp_type = RAMP_NOOP_CRUISE; + st.ramp_type = RAMP_ACCEL; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid +// } +// } + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } + + } + + // Acceleration and cruise handled by ramping. Just check for deceleration. + if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) { + if (st.ramp_type == RAMP_NOOP_CRUISE) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + st.ramp_type = RAMP_DECEL; } + st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete. + } else { + // Can't discard planner block here if a feed hold stops in middle of block. st_go_idle(); bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end return; // Nothing to do but exit. } + } // Adjust inverse time counter for ac/de-celerations if (st.ramp_type) { - // Tick acceleration ramp counter - st.ramp_count--; - if (st.ramp_count == 0) { + st.ramp_count--; // Tick acceleration ramp counter + if (st.ramp_count == 0) { // Adjust step rate when its time st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration - st.delta_d += current_block->rate_delta; - if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. - st.ramp_type = CRUISE_RAMP; - st.delta_d = current_block->nominal_rate; // Set cruise velocity + if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.delta_d += st_current_data->rate_delta; + if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. + st.delta_d = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore } - } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration - if (st.delta_d > current_block->rate_delta) { - st.delta_d -= current_block->rate_delta; + } else { // Adjust velocity for deceleration + if (st.delta_d > st_current_data->rate_delta) { + st.delta_d -= st_current_data->rate_delta; } else { + + // Moving near zero feed rate. Gracefully slow down. st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + + // Check for and handle feed hold exit? At this point, machine is stopped. + } } + // Finalize adjusted step rate. Ensure minimum. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } } } - - // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; } - else { st.d_counter -= st.delta_d; } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.d_per_tick; // Execute Bresenham step event, when it's time to do so. - if (st.d_counter < 0) { - st.d_counter += current_block->d_next; - - // Check for feed hold state and execute accordingly. - if (sys.state == STATE_HOLD) { - if (st.ramp_type != DECEL_RAMP) { - st.ramp_type = DECEL_RAMP; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - } - if (st.delta_d <= current_block->rate_delta) { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); - return; - } - } - - // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). - - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + if (st.counter_d < 0) { + st.counter_d += st_current_data->d_next; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= current_block->steps_x; + st.counter_x -= pl_current_block->steps[X_AXIS]; if (st.counter_x < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps_y; + st.counter_y -= pl_current_block->steps[Y_AXIS]; if (st.counter_y < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps_z; + st.counter_z -= pl_current_block->steps[Z_AXIS]; if (st.counter_z < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<decelerate_after) { - st.ramp_type = DECEL_RAMP; - if (st.step_events_remaining == current_block->decelerate_after) { - if (st.delta_d == current_block->nominal_rate) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - } - } + st.segment_steps_remaining--; // Decrement step events count + if (st.segment_steps_remaining == 0) { + + // Line move is complete, set load line flag to check for new move. + // Check if last line move in planner block. Discard if so. + if (st_current_segment->flag == ST_END_OF_BLOCK || st_current_segment->flag == ST_DECEL_EOB) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_LINE; } - } else { - // If current block is finished, reset pointer - current_block = NULL; - plan_discard_current_block(); + + // Discard current segment + segment_buffer_tail = next_block_index( segment_buffer_tail ); + + // NOTE: sys.position updates could be done here. The bresenham counters can have + // their own fast 8-bit addition-only counters. Here we would check the direction and + // apply it to sys.position accordingly. However, this could take too much time. + } - out_bits ^= settings.invert_mask; // Apply step port invert mask + st.out_bits ^= settings.invert_mask; // Apply step port invert mask } busy = false; // SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// st.ramp_type = RAMP_ACCEL; +// st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.delta_d = 0; +// sys.state = STATE_QUEUED; +// } else { +// sys.state = STATE_IDLE; +// } sys.state = STATE_IDLE; - } + } + + +/* Preps stepper buffer. Called from main program. + + NOTE: There doesn't seem to be a great way to figure out how many steps occur within + a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a + critical problem. So, either numerical round-off checks could be made to account for + them, while CPU overhead could be minimized in some way, or we can flip the algorithm + around to have the stepper algorithm track number of steps over an indeterminant amount + of time instead. + In other words, we use the planner velocity floating point data to get an estimate of + the number of steps we want to execute. We then back out the approximate velocity for + the planner to use, which should be much more robust to round-off error. The main problem + now is that we are loading the stepper algorithm to handle acceleration now, rather than + pre-calculating with the main program. This approach does make sense in the way that + planner velocities and stepper profiles can be traced more accurately. + Which is better? Very hard to tell. The time-based algorithm would be able to handle + Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would + require some additional math in the stepper algorithm to adjust on the fly, plus adaptation + would occur in a non-deterministic manner. + I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. + + TODO: Need to describe the importance of continuations of step pulses between ramp states + and planner blocks. This has to do with Alden's problem with step "phase". The things I've + been doing here limit this phase issue by truncating some of the ramp timing for certain + events like deceleration initialization and end of block. +*/ +void st_prep_buffer() +{ + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Determine if we need to load a new planner block. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); + if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out. + + // Prepare commonly shared planner block data for the ensuing step buffer moves + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert new block to stepper variables. + // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must + // be maintained as these execute. + // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, + // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. + st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // This data doesn't change. Could be performed in the planner, but fits nicely here. + // Although, acceleration can change for S-curves. So keep it here. + st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + // This definitely doesn't change, but could be precalculated in a way to help some of the + // math in this handler, i.e. millimeters per step event data. + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + + // Calculate trapezoid data from planner. + st_prep_data->decelerate_after = calculate_trapezoid_for_block(pl_prep_index); + + } + + + /* + TODO: Need to check for a planner flag to indicate a change to this planner block. + If so, need to check for a change in acceleration state, from deceleration to acceleration, + to reset the stepper ramp counters and the initial_rate data to trace the new + ac/de-celeration profile correctly. + No change conditions: + - From nominal speed to acceleration from feedrate override + - From nominal speed to new deceleration. + - From acceleration to new deceleration point later or cruising point. + - From acceleration to immediate deceleration? Can happen during feedrate override + and slowing down, but likely ok by enforcing the normal ramp counter protocol. + Change conditions: + - From deceleration to acceleration, i.e. common with jogging when new blocks are added. + */ + + st_segment_t *st_prep_segment = &segment_buffer[segment_buffer_head]; + st_prep_segment->st_data_index = st_data_prep_index; + + // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. + st_prep_segment->n_step = 250; //floor( (exit_speed*approx_time)/mm_per_step ); +// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? +// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + + // Check if n_step exceeds steps remaining in planner block. If so, truncate. + if (st_prep_segment->n_step > st_prep_data->step_events_remaining) { + st_prep_segment->n_step = st_prep_data->step_events_remaining; + } + + // Check if n_step exceeds decelerate point in block. Need to perform this so that the + // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should + // be OK since it is likely moving at a fast rate already. + if (st_prep_data->decelerate_after > 0) { + if (st_prep_segment->n_step > st_prep_data->decelerate_after) { + st_prep_segment->n_step = st_prep_data->decelerate_after; + } + } + +// float distance, exit_speed_sqr; +// distance = st_prep_segment->n_step*st_prep_data->mm_per_step; // Always greater than zero +// if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) { +// exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; +// } else { // Acceleration or cruising ramp +// if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) { +// exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; +// if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; } +// } else { +// exit_speed_sqr = pl_prep_block->nominal_speed_sqr; +// } +// } + + // Update planner block variables. +// pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr); +// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable. +// pl_prep_block->millimeters -= distance; // Potential round-off error near end of block. +// pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter. + + // Update stepper block variables. + st_prep_data->step_events_remaining -= st_prep_segment->n_step; + if ( st_prep_data->step_events_remaining == 0 ) { + // Move planner pointer to next block + if (st_prep_data->decelerate_after == 0) { + st_prep_segment->flag = ST_DECEL_EOB; + } else { + st_prep_segment->flag = ST_END_OF_BLOCK; + } + pl_prep_index = next_block_pl_index(pl_prep_index); + pl_prep_block = NULL; +printString("EOB"); + } else { + if (st_prep_data->decelerate_after == 0) { + st_prep_segment->flag = ST_DECEL; + } else { + st_prep_segment->flag = ST_NOOP; + } +printString("x"); + } + st_prep_data->decelerate_after -= st_prep_segment->n_step; + + // New step block completed. Increment step buffer indices. + segment_buffer_head = segment_next_head; + segment_next_head = next_block_index(segment_buffer_head); + +printInteger((long)st_prep_segment->n_step); +printString(" "); +printInteger((long)st_prep_data->decelerate_after); +printString(" "); +printInteger((long)st_prep_data->step_events_remaining); + } +} diff --git a/stepper.h b/stepper.h index 0cb4189..74ab717 100644 --- a/stepper.h +++ b/stepper.h @@ -45,4 +45,6 @@ void st_cycle_reinitialize(); // Initiates a feed hold of the running program void st_feed_hold(); +void st_prep_buffer(); + #endif diff --git a/stepper_new2.c b/stepper_new2.c new file mode 100644 index 0000000..c248d4d --- /dev/null +++ b/stepper_new2.c @@ -0,0 +1,601 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 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 + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" +#include "nuts_bolts.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define CRUISE_RAMP 0 +#define ACCEL_RAMP 1 +#define DECEL_RAMP 2 + +#define LOAD_NOOP 0 +#define LOAD_LINE 1 +#define LOAD_BLOCK 2 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int32_t step_events_remaining; // Steps remaining in line motion + + // Used by inverse time algorithm + int32_t delta_d; // Inverse time distance traveled per interrupt tick + int32_t d_counter; // Inverse time distance traveled since last step event + int32_t d_per_tick; + + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; +} stepper_t; +static stepper_t st; + +#define STEPPER_BUFFER_SIZE 5 +typedef struct { + int32_t event_count; + int32_t rate; + uint8_t end_of_block; + uint8_t tick_count; + + int32_t initial_rate; // The step rate at start of block + int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + int32_t decelerate_after; // The index of the step event on which to start decelerating + int32_t nominal_rate; // The nominal step rate for this block in step_events/minute + int32_t d_next; // Scaled distance to next step + +} stepper_buffer_t; +static stepper_buffer_t step_buffer[STEPPER_BUFFER_SIZE]; + +static volatile uint8_t step_buffer_tail; +static uint8_t step_buffer_head; +static uint8_t step_next_head; + +// 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. +static plan_block_t *plan_current_block; // A pointer to the planner block currently being traced + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + + st.counter_x = (plan_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // This is correct. Sets the total time before the next step occurs. + st.counter_d = plan_current_block->d_next; // d_next always greater than delta_d. + + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + + } + + st.load_flag = LOAD_NOOP; // Line motion loaded. Set no-operation flag until complete. + + } else { + // Can't discard planner block here if a feed hold stops in middle of block. + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + + } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.delta_d; + + // Execute Bresenham step event, when it's time to do so. + if (st.counter_d < 0) { + st.counter_d += plan_current_block->d_next; // Reload inverse time counter + + st.out_bits = plan_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 -= plan_current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<step_event_count; + //st.step_events_remaining = st.event_count; + + // Convert new block to stepper variables. + // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must + // be maintained as these execute. + // TODO: The initial rate needs to be sent back to the planner to update the entry speed + block->initial_rate = ceil(sqrt(plan_current_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + block->nominal_rate = ceil(plan_current_block->nominal_speed*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // This data doesn't change. Could be performed in the planner, but fits nicely here. + // Although, acceleration can change for S-curves. So keep it here. + block->rate_delta = ceil(plan_current_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + // This definitely doesn't change, but could be precalculated in a way to help some of the + // math in this handler, i.e. millimeters per step event data. + block->d_next = ceil((plan_current_block->millimeters*INV_TIME_MULTIPLIER)/plan_current_block->step_event_count); // (mult*mm/step) + + // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { } + } + + +// Track instead the trapezoid line and use the average of the entry and exit velocities +// to determine step rate. This should take care of the deceleration issue automatically... +// i think. +// First need to figure out what type of profile is segment is, i.e. acceleration only, accel +// to decel triangle, cruise to decel, or all three. May need more profile data to compute this +// from the planner itself, like accelerate until. +// Another issue. This is only tracking the velocity profile, not the distance covered over that +// time period. This can lead to an unsynchronized velocity profile and steps executed. But, +// how much drift is there really? Enough to be a problem? Not sure. I would think typical +// drift would be on the order of a few steps, more depending on step resolution. +entry_rate = last_exit_rate; +time = 250 ISR ticks per acceleration tick. +distance = 0; +if (distance_traveled < accelerate_until) + exit_rate = entry_rate + acceleration*time; + if (exit_rate > nominal_rate) { + exit_rate = nominal_rate; + time = 2*(accelerate_until-distance_travel)/(entry_rate+nominal_rate); + // distance = accelerate_until; // Enforce distance? + // Truncate this segment. + } +} else if (distance_traveled >= decelerate_after) { + if (accelerate_until == decelerate_after) { + time = last time; + exit_rate = entry_rate; + } else { + exit_rate = entry_rate - acceleration*time; + } +} else { + exit_rate = nominal_rate; // Just cruise + distance = nominal_rate*time; + if (distance > decelerate_after) { // Truncate segment at nominal rate. + time = (decelerate_after-distance_traveled)/(nominal_rate); + distance = decelerate_after; + } +} +mean_rate = 0.5*(entry_rate+exit_rate); +distance = mean_rate*time; + + +if (entry_rate < nominal_rate) { + if (entry_distance < decelerate_after) { // Acceleration case + exit_rate = entry_rate + acceleration*time + exit_rate = min(exit_rate,nominal_rate); +mean_rate = 0.5*(entry_rate + exit_rate); +distance = mean_rate*time; +if (distance > decelerate_after) { + exit_rate = + + + // If the MINIMUM_STEP_RATE is less than ACCELERATION_TICKS_PER_SECOND then there can be + // rate adjustements that have less than one step per tick. + // How do you deal with the remainer? +time = 250 ISR ticks per acceleration tick. (30000/120) +delta_d*time // mm per acceleration tick +delta_d*time/d_next // number of steps/acceleration_tick. Chance of integer overflow. +delta_d*time/d_next + last_remainder. // steps/acceleration_tick. +n_step*d_next/delta_d // number of ISR ticks for enforced n_steps. + +// In floating point? Then convert? +// Requires exact millimeters. Roundoff might be a problem. But could be corrected by just +// checking if the total step event counts are performed. +// Could be limited by float conversion and about 1e7 steps per block. +line_mm = feed_rate / acc_tick // mm per acc_tick +n_steps = floor(line_mm * step_event_remaining/millimeters_remaining) // steps. float 7.2 digits|int32 10 digits +millimeters_remaining -= line_mm; +step_events_remaining -= n_steps; + +// There doesn't seem to be a way to avoid this divide here. +line_mm = feed_rate / acc_tick // mm per acc_tick +n_steps = floor( (line_mm+line_remainder) * step_event_count/millimeters) // steps. float 7.2 digits|int32 10 digits +line_remainder = line_mm - n_steps*(millimeters/step_event_count); + +// Need to handle when rate is very very low, i.e. less than one step per accel tick. +// Could be bounded by MINIMUM_STEP_RATE. + +// 1. Figure out how many steps occur exactly within n ISR ticks. +// 2. Account for step-time remainder for next line motion exactly. +// 3. At the end of block, determine exact number of ISR ticks to finish the steps. Or,\ + have the ISR track steps to exit on time. It would require an extra counter. + +// NOTE: There doesn't seem to be a great way to figure out how many steps occur within +// a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a +// critical problem. So, either numerical round-off checks could be made to account for +// them, while CPU overhead could be minimized in some way, or we can flip the algorithm +// around to have the stepper algorithm track number of steps over an indeterminant amount +// of time instead. +// In other words, we use the planner velocity floating point data to get an estimate of +// the number of steps we want to execute. We then back out the approximate velocity for +// the planner to use, which should be much more robust to round-off error. The main problem +// now is that we are loading the stepper algorithm to handle acceleration now, rather than +// pre-calculating with the main program. This approach does make sense in the way that +// planner velocities and stepper profiles can be traced more accurately. +// Which is better? Very hard to tell. The time-based algorithm would be able to handle +// Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would +// require some additional math in the stepper algorithm to adjust on the fly, plus adaptation +// would occur in a non-deterministic manner. +// I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. + +feed_rate/120 = millimeters per acceleration tick + + + steps? + d_next // (mult*mm/step) + rate // (mult*mm/isr_tic) + rate/d_next // step/isr_tic + + if (plan_current_block->step_events_remaining <= plan_current_block->decelerate_after) { + // Determine line segment velocity and associated inverse time counter. + if (step_block.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration + step_block.delta_d += plan_current_block->rate_delta; + if (step_block.delta_d >= plan_current_block->nominal_rate) { // Reached cruise state. + step_block.ramp_type = CRUISE_RAMP; + step_block.delta_d = plan_current_block->nominal_rate; // Set cruise velocity + } + } + } else { // Adjust velocity for deceleration + if (step_block.delta_d > plan_current_block->rate_delta) { + step_block.delta_d -= plan_current_block->rate_delta; + } else { + step_block.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + } + } + + // Incorrect. Can't overwrite delta_d. Needs to override instead. + if (step_block.delta_d < MINIMUM_STEP_RATE) { step_block.delta_d = MINIMUM_STEP_RATE; } + + /* - Compute the number of steps needed to complete this move over the move time, i.e. + ISR_TICKS_PER_ACCELERATION_TICK. + - The first block in the buffer is half of the move time due to midpoint rule. + - Check if this reaches the deceleration after location. If so, truncate move. Also, + if this is a triangle move, double the truncated move to stay with midpoint rule. + NOTE: This can create a stepper buffer move down to just one step in length. + - Update the planner block entry speed for the planner to compute from end of the + stepper buffer location. + - If a feed hold occurs, begin to enforce deceleration, while enforcing the above rules. + When the deceleration is complete, all we need to do is update the planner block + entry speed and force a replan. + */ + + // Planner block move completed. + // TODO: planner buffer tail no longer needs to be volatile. only accessed by main program. + if (st.step_events_remaining == 0) { + plan_current_block = NULL; // Set flag that we are done with this planner block. + plan_discard_current_block(); + } + + + + + + step_buffer_head = step_next_head; + step_next_head = next_block_index(step_buffer_head); + + } +} diff --git a/stepper_new.c b/stepper_new_dual_ISR.c similarity index 96% rename from stepper_new.c rename to stepper_new_dual_ISR.c index ae6bd8b..87f87bc 100644 --- a/stepper_new.c +++ b/stepper_new_dual_ISR.c @@ -34,9 +34,7 @@ // Stepper state variable. Contains running data and trapezoid variables. typedef struct { // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; + int32_t counter[N_AXIS]; // Counter variables for the bresenham line tracer uint32_t event_count; // Total event count. Retained for feed holds. uint32_t step_events_remaining; // Steps remaining in motion @@ -182,26 +180,29 @@ won't take too much time in the interrupt. // Prepare Bresenham step event, when it's time to do so. if (st.d_counter < 0) { st.d_counter += current_block->d_next; - - // Load next step - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; + + // Configure next step + out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= current_block->steps_x; - if (st.counter_x < 0) { + st.counter[X_AXIS] -= current_block->steps_x; // Doesn't change when set up. + if (st.counter[X_AXIS] < 0) { out_bits |= (1<steps_y; - if (st.counter_y < 0) { + st.counter[Y_AXIS] -= current_block->steps_y; + if (st.counter[Y_AXIS] < 0) { out_bits |= (1<steps_z; - if (st.counter_z < 0) { + st.counter[Z_AXIS] -= current_block->steps_z; + if (st.counter[Z_AXIS] < 0) { out_bits |= (1<. +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" +#include "nuts_bolts.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_LINE 1 +#define LOAD_BLOCK 2 + +#define ST_NOOP 0 +#define ST_END_OF_BLOCK 1 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int8_t segment_steps_remaining; // Steps remaining in line motion + + // Used by inverse time algorithm to track step rate + int32_t counter_d; // Inverse time distance traveled since last step event + int32_t delta_d; // Inverse time distance traveled per interrupt tick + int32_t d_per_tick; + + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; + + uint8_t ramp_count; + uint8_t ramp_type; +} stepper_t; +static stepper_t st; + +#define SEGMENT_BUFFER_SIZE 5 +// Stores stepper buffer common data. Can change planner mid-block in special conditions. +typedef struct { + int32_t step_events_remaining; // Tracks step event count for the executing planner block + int32_t d_next; // Scaled distance to next step + float mm_per_step; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; + +// Primary stepper motion buffer +typedef struct { + uint8_t n_step; + int32_t rate; + uint8_t st_data_index; + uint8_t flag; +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; + +static volatile uint8_t segment_buffer_tail; +static uint8_t segment_buffer_head; +static uint8_t segment_next_head; + +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer +static uint8_t pl_prep_index; +static st_data_t *st_prep_data; +static uint8_t st_data_prep_index; + + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static uint8_t next_block_index(uint8_t block_index) +{ + block_index++; + if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<n_step; + st.delta_d = st_current_segment->rate; + + // Check if the counters need to be reset for a new planner block + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[st_current_segment->st_data_index]; + + // Initialize direction bits for block + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits upon next ISR tick. + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time and step rate counter data + st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + } + st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete. + + } else { + // Can't discard planner block here if a feed hold stops in middle of block. + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + + } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.delta_d; + + // Execute Bresenham step event, when it's time to do so. + if (st.counter_d < 0) { + st.counter_d += st_current_data->d_next; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= pl_current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<flag == ST_END_OF_BLOCK) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_LINE; + } + + // Discard current block + if (segment_buffer_head != segment_buffer_tail) { + segment_buffer_tail = next_block_index( segment_buffer_tail ); + } + + // NOTE: sys.position updates could be done here. The bresenham counters can have + // their own fast 8-bit addition-only counters. Here we would check the direction and + // apply it to sys.position accordingly. However, this could take too much time. + + } + + st.out_bits ^= settings.invert_mask; // Apply step port invert mask + } + busy = false; +// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); + st.ramp_type = RAMP_ACCEL; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + st.delta_d = 0; + sys.state = STATE_QUEUED; + } else { + sys.state = STATE_IDLE; + } +} + + +/* Preps stepper buffer. Called from main program. + + NOTE: There doesn't seem to be a great way to figure out how many steps occur within + a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a + critical problem. So, either numerical round-off checks could be made to account for + them, while CPU overhead could be minimized in some way, or we can flip the algorithm + around to have the stepper algorithm track number of steps over an indeterminant amount + of time instead. + In other words, we use the planner velocity floating point data to get an estimate of + the number of steps we want to execute. We then back out the approximate velocity for + the planner to use, which should be much more robust to round-off error. The main problem + now is that we are loading the stepper algorithm to handle acceleration now, rather than + pre-calculating with the main program. This approach does make sense in the way that + planner velocities and stepper profiles can be traced more accurately. + Which is better? Very hard to tell. The time-based algorithm would be able to handle + Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would + require some additional math in the stepper algorithm to adjust on the fly, plus adaptation + would occur in a non-deterministic manner. + I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. + + TODO: Need to describe the importance of continuations of step pulses between ramp states + and planner blocks. This has to do with Alden's problem with step "phase". The things I've + been doing here limit this phase issue by truncating some of the ramp timing for certain + events like deceleration initialization and end of block. +*/ +void st_prep_buffer() +{ + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Determine if we need to load a new planner block. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); + if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out. + + // Prepare commonly shared planner block data for the ensuing step buffer moves + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert new block to stepper variables. + // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must + // be maintained as these execute. + // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, + // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. +// st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) +// st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // This data doesn't change. Could be performed in the planner, but fits nicely here. + // Although, acceleration can change for S-curves. So keep it here. +// st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* +// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + // This definitely doesn't change, but could be precalculated in a way to help some of the + // math in this handler, i.e. millimeters per step event data. + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + } + + + /* + // Check if planner has changed block exit parameters. If so, then we have to update the + // velocity profile for the remainder of this block. Otherwise, the block hasn't changed. + if (exit_speed_sqr != last_exit_speed_sqr) { + intersect_distance = 0.5*( millimeters + (entry_speed_sqr-exit_speed_sqr)/(2*acceleration) ); + if (intersect_distance <= 0) { // Deceleration only. + final_rate_sqr = initial_rate_sqr - 2*acceleration*segment_distance; + block->decelerate_after = 0; + } else { + decelerate_after = (block->nominal_speed_sqr - exit_speed_sqr)/(2*block->acceleration); + if (decelerate_after > intersect_distance) { decelerate_after = intersect_distance; } + if (decelerate_after > block->millimeters) { decelerate_after = block->millimeters; } + } + } + + + + + + n_step = 100; // Estimate distance we're going to travel in this segment + if (n_step > step_events_remaining) { n_step = step_events_remaining; }; + + segment_distance = n_step*mm_per_step; // True distance traveled + + // ISSUE: Either weighted average speed or truncated segments must be used. Otherwise + // accelerations, in particular high value, will not perform correctly. + if (distance_traveled < decelerate_after) { + if (segment_distance + distance_traveled > decelerate_after) { + n_step = ceil((decelerate_after-distance_traveled)/mm_per_step); + segment_distance = n_step*mm_per_step; + } + } + + + + + // based on time? + time = incremental time; + v_exit = v_entry + acceleration*time; + if (v_exit > v_nominal) { + time_accel = (v_nominal-v_entry)/acceleration; + distance = v_entry*time_accel + 0.5*acceleration*time_accel**2; + time_remaining = time - time_accel; + } + distance = v_entry*time + 0.5*acceleration*time*time; + + + t_accel = (v_nominal - v_entry) / acceleration; + t_decel = (v_exit - v_nominal) / -acceleration; + dt = (v_entry-v_exit)/acceleration; + if + + + + + // What's the speed of this segment? Computing per segment can get expensive, but this + // would allow S-curves to be installed pretty easily here. + if (initial_speed < nominal_speed) { + if (initial_distance < decelerate_after) { // Acceleration + exit_speed = sqrt(initial_speed*initial_speed + 2*acceleration*distance); + if (exit_speed > nominal_speed) { exit_speed = nominal_speed; } + } else { // Deceleration + exit_speed = sqrt(initial_speed*initial_speed - 2*acceleration*distance); + } + average_speed = 0.5*(initial_speed + exit_speed); + } else { // Cruise + average_speed = nominal_speed; + } + segment_rate = ceil(average_speed*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + if (segment_rate < MINIMUM_STEP_RATE) { segment_rate = MINIMUM_STEP_RATE; } + + + + + */ + + + + /* + TODO: Need to check for a planner flag to indicate a change to this planner block. + If so, need to check for a change in acceleration state, from deceleration to acceleration, + to reset the stepper ramp counters and the initial_rate data to trace the new + ac/de-celeration profile correctly. + No change conditions: + - From nominal speed to acceleration from feedrate override + - From nominal speed to new deceleration. + - From acceleration to new deceleration point later or cruising point. + - From acceleration to immediate deceleration? Can happen during feedrate override + and slowing down, but likely ok by enforcing the normal ramp counter protocol. + Change conditions: + - From deceleration to acceleration, i.e. common with jogging when new blocks are added. + */ + + st_segment_t *st_prep_block = &segment_buffer[segment_buffer_head]; + st_prep_block->st_data_index = st_data_prep_index; + + // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. + st_prep_block->n_step = 100; //floor( (exit_speed*approx_time)/mm_per_step ); +// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? +// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + + // Check if n_step exceeds steps remaining in planner block. If so, truncate. + if (st_prep_block->n_step > st_prep_data->step_events_remaining) { + st_prep_block->n_step = st_prep_data->step_events_remaining; + } + + // Check if n_step exceeds decelerate point in block. Need to perform this so that the + // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should + // be OK since it is likely moving at a fast rate already. + if (st_prep_block->n_step > pl_prep_block->decelerate_after) { + st_prep_block->n_step = pl_prep_block->decelerate_after; + } + + float distance, exit_speed_sqr; + distance = st_prep_block->n_step*st_prep_data->mm_per_step; // Always greater than zero + if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) { + exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; + // Set ISR tick reset flag for deceleration ramp. + } else { // Acceleration or cruising ramp + if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) { + exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; + if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; } + } else { + exit_speed_sqr = pl_prep_block->nominal_speed_sqr; + } + } + + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { + st.ramp_count--; // Tick acceleration ramp counter + if (st.ramp_count == 0) { // Adjust step rate when its time + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.delta_d += st_current_data->rate_delta; + if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. + st.delta_d = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore + } + } else { // Adjust velocity for deceleration + if (st.delta_d > st_current_data->rate_delta) { + st.delta_d -= st_current_data->rate_delta; + } else { + + // Moving near zero feed rate. Gracefully slow down. + st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + + // Check for and handle feed hold exit? At this point, machine is stopped. + + } + } + // Finalize adjusted step rate. Ensure minimum. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } + } + } + // During feed hold, do not update rate or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { + st.delta_d = st_current_data->initial_rate; + if (st.delta_d == st_current_data->nominal_rate) { + ramp_type = RAMP_NOOP_CRUISE; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } + // Acceleration and cruise handled by ramping. Just check for deceleration. + if (st_current_segment->flag == ST_NOOP) { + if (st.ramp_type == RAMP_NOOP_CRUISE) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + st.ramp_type = RAMP_DECEL; + } + + + + + + // Update planner block variables. + pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr); +// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable. + pl_prep_block->millimeters -= distance; // Potential round-off error near end of block. + pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter. + + // Update stepper block variables. + st_prep_data->step_events_remaining -= st_prep_block->n_step; + if ( st_prep_data->step_events_remaining == 0 ) { + // Move planner pointer to next block + st_prep_block->flag = ST_END_OF_BLOCK; + pl_prep_index = next_block_index(pl_prep_index); + pl_prep_block = NULL; + } else { + st_prep_block->flag = ST_NOOP; + } + + // New step block completed. Increment step buffer indices. + segment_buffer_head = segment_next_head; + segment_next_head = next_block_index(segment_buffer_head); + + } +} diff --git a/stepper_old.c b/stepper_old.c new file mode 100644 index 0000000..3191007 --- /dev/null +++ b/stepper_old.c @@ -0,0 +1,387 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 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 + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith + and Philipp Tiefenbacher. */ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define CRUISE_RAMP 0 +#define ACCEL_RAMP 1 +#define DECEL_RAMP 2 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int32_t event_count; // Total event count. Retained for feed holds. + int32_t step_events_remaining; // Steps remaining in motion + + // Used by Pramod Ranade inverse time algorithm + int32_t delta_d; // Ranade distance traveled per interrupt tick + int32_t d_counter; // Ranade distance traveled since last step event + uint8_t ramp_count; // Acceleration interrupt tick counter. + uint8_t ramp_type; // Ramp type variable. + uint8_t execute_step; // Flags step execution for each interrupt. + +} 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 + +// 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. + +// __________________________ +// /| |\ _________________ ^ +// / | | \ /| |\ | +// / | | \ / | | \ s +// / | | | | | \ p +// / | | | | | \ e +// +-----+------------------------+---+--+---------------+----+ e +// | BLOCK 1 | BLOCK 2 | d +// +// time -----> +// +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta +// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after +// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as +// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. + + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + + // Initialize Bresenham variables + st.counter_x = (current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + st.event_count = current_block->step_event_count; + st.step_events_remaining = st.event_count; + + // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { + // Initialize Ranade variables + st.d_counter = current_block->d_next; + st.delta_d = current_block->initial_rate; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + + // Initialize ramp type. + if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } + else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } + else { st.ramp_type = ACCEL_RAMP; } + } + + } else { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + } + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { + // Tick acceleration ramp counter + st.ramp_count--; + if (st.ramp_count == 0) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration + st.delta_d += current_block->rate_delta; + if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. + st.ramp_type = CRUISE_RAMP; + st.delta_d = current_block->nominal_rate; // Set cruise velocity + } + } 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 >>= 1; // Integer divide by 2 until complete. Also prevents 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 = ISR_TICKS_PER_ACCELERATION_TICK/2; + } + if (st.delta_d <= current_block->rate_delta) { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); + return; + } + } + + // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). + + out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + out_bits |= (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + out_bits |= (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + out_bits |= (1<decelerate_after) { + st.ramp_type = DECEL_RAMP; + if (st.step_events_remaining == current_block->decelerate_after) { + if (st.delta_d == current_block->nominal_rate) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + } + } + } + } 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 + } + busy = false; +// SPINDLE_ENABLE_PORT ^= 1< Date: Wed, 9 Oct 2013 09:33:22 -0600 Subject: [PATCH 08/13] Protected buffer works! Vast improvements to planner efficiency. Many things still broken with overhaul. Development push. Lots still broken. - Protected planner concept works! This is a critical precursor to enabling feedrate overrides in allowing the planner buffer and the stepper execution operate atomically. This is done through a intermediary segment buffer. - Still lots of work to be done, as this was a complete overhaul of the planner and stepper subsystems. The code can be cleaned up quite a bit, re-enabling some of the broken features like feed holds, and finishing up some of the concepts - Pushed some of the fixes from the master and edge branch to here, as this will likely replace the edge branch when done. --- Makefile | 2 +- config.h | 93 +++------- doc/pinmapping.txt | 97 +++++++++++ nuts_bolts.h | 1 + pin_map.h | 181 ++++++++++++++++++++ planner.c | 417 ++++++++++++++++++++++++++++++++------------- planner.h | 20 ++- serial.c | 12 +- stepper.c | 267 +++++++++++++++++++---------- stepper.h | 4 + 10 files changed, 793 insertions(+), 301 deletions(-) create mode 100644 doc/pinmapping.txt create mode 100644 pin_map.h diff --git a/Makefile b/Makefile index f0e7f41..78c1032 100644 --- a/Makefile +++ b/Makefile @@ -84,7 +84,7 @@ main.elf: $(OBJECTS) grbl.hex: main.elf rm -f grbl.hex avr-objcopy -j .text -j .data -O ihex main.elf grbl.hex - avr-size -C --mcu=$(DEVICE) main.elf + avr-size --format=berkeley main.elf # If you have an EEPROM section, you must also create a hex file for the # EEPROM and add it to the "flash" target. diff --git a/config.h b/config.h index c1c450e..41d6a38 100644 --- a/config.h +++ b/config.h @@ -2,8 +2,8 @@ config.h - compile time configuration Part of Grbl - Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2011-2013 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 @@ -19,80 +19,24 @@ along with Grbl. If not, see . */ -#ifndef config_h -#define config_h +// This file contains compile-time configurations for Grbl's internal system. For the most part, +// users will not need to directly modify these, but they are here for specific needs, i.e. +// performance tuning or adjusting to non-typical machines. // IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them. +#ifndef config_h +#define config_h + // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h #define DEFAULTS_ZEN_TOOLWORKS_7x7 // Serial baud rate -#define BAUD_RATE 9600 +#define BAUD_RATE 115200 -// Define pin-assignments -// NOTE: All step bit and direction pins must be on the same port. -#define STEPPING_DDR DDRD -#define STEPPING_PORT PORTD -#define X_STEP_BIT 2 // Uno Digital Pin 2 -#define Y_STEP_BIT 3 // Uno Digital Pin 3 -#define Z_STEP_BIT 4 // Uno Digital Pin 4 -#define X_DIRECTION_BIT 5 // Uno Digital Pin 5 -#define Y_DIRECTION_BIT 6 // Uno Digital Pin 6 -#define Z_DIRECTION_BIT 7 // Uno Digital Pin 7 -#define STEP_MASK ((1< #include "config.h" #include "defaults.h" +#include "pin_map.h" #define false 0 #define true 1 diff --git a/pin_map.h b/pin_map.h new file mode 100644 index 0000000..0167cc9 --- /dev/null +++ b/pin_map.h @@ -0,0 +1,181 @@ +/* + pin_map.h - Pin mapping configuration file + Part of Grbl + + Copyright (c) 2013 Sungeun K. Jeon + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The pin_map.h file serves as a central pin mapping settings file for different processor + types, i.e. AVR 328p or AVR Mega 2560. Grbl officially supports the Arduino Uno, but the + other supplied pin mappings are supplied by users, so your results may vary. */ + +#ifndef pin_map_h +#define pin_map_h + +#ifdef PIN_MAP_ARDUINO_UNO // AVR 328p, Officially supported by Grbl. + + // Serial port pins + #define SERIAL_RX USART_RX_vect + #define SERIAL_UDRE USART_UDRE_vect + + // NOTE: All step bit and direction pins must be on the same port. + #define STEPPING_DDR DDRD + #define STEPPING_PORT PORTD + #define X_STEP_BIT 2 // Uno Digital Pin 2 + #define Y_STEP_BIT 3 // Uno Digital Pin 3 + #define Z_STEP_BIT 4 // Uno Digital Pin 4 + #define X_DIRECTION_BIT 5 // Uno Digital Pin 5 + #define Y_DIRECTION_BIT 6 // Uno Digital Pin 6 + #define Z_DIRECTION_BIT 7 // Uno Digital Pin 7 + #define STEP_MASK ((1<entry_speed_sqr = exit_speed_sqr - 2*partial_block->acceleration*millimeters_remaining; + } else { // Block is accelerating or cruising + partial_block->entry_speed_sqr += 2*partial_block->acceleration*(partial_block->millimeters-millimeters_remaining); + partial_block->entry_speed_sqr = min(partial_block->entry_speed_sqr, partial_block->nominal_speed_sqr); + } + + // Update only the relevant planner block information so the planner can plan correctly. + partial_block->millimeters = millimeters_remaining; + partial_block->max_entry_speed_sqr = partial_block->entry_speed_sqr; // Not sure if this needs to be updated. + } +} + + + + /* PLANNER SPEED DEFINITION +--------+ <- current->nominal_speed / \ current->entry_speed -> + \ - | + <- next->entry_speed + | + <- next->entry_speed (aka exit speed) +-------------+ time --> @@ -112,7 +153,7 @@ static uint8_t prev_block_index(uint8_t block_index) in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. - The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. + The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_JUNCTION_SPEED. Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory @@ -123,69 +164,178 @@ static uint8_t prev_block_index(uint8_t block_index) 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 then starve and empty, leading to weird hiccup-like jerky motions. + + Index mapping: + - block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The planner + never touches the exit speed of this block, which always defaults to MINIMUM_JUNCTION_SPEED. + - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. + Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible + as long as the segment buffer is not empty. + - next_buffer_head: Points to next planner buffer block after the last block. Should always be empty. + - block_buffer_safe: Points to the first planner block in the buffer for which it is safe to change. Since + the stepper can be executing the first block and if the planner changes its conditions, this will cause + a discontinuity and error in the stepper profile with lost steps likely. With the new stepper algorithm, + the block_buffer_safe is always where the stepper segment buffer ends and can never be overwritten, but + this can change the state of the block profile from a pure trapezoid assumption. Meaning, if that block + is decelerating, the planner conditions can change such that the block can new accelerate mid-block. + + !!! I need to make sure that the stepper algorithm can modify the acceleration mid-block. Needed for feedrate overrides too. + + !!! planner_recalculate() may not work correctly with re-planning.... may need to artificially set both the + block_buffer_head and next_buffer_head back one index so that this works correctly, or allow the operation + of this function to accept two different conditions to operate on. + + - block_buffer_planned: Points to the first buffer block after the last optimally fixed block, which can no longer be + improved. This block and the trailing buffer blocks that can still be altered when new blocks are added. This planned + block points to the transition point between the fixed and non-fixed states and is handled slightly different. The entry + speed is fixed, indicating the reverse pass cannot maximize the speed further, but the velocity profile within it + can still be changed, meaning the forward pass calculations must start from here and influence the following block + entry speed. + + !!! Need to check if this is the start of the non-optimal or the end of the optimal block. */ static void planner_recalculate() -{ - // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. - uint8_t block_index = block_buffer_head; - plan_block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer +{ + // Query stepper module for safe planner block index to recalculate to, which corresponds to the end + // of the step segment buffer. + uint8_t block_buffer_safe = st_get_prep_block_index(); + // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module + // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although, + // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the + // planner at all times. - // Ping the stepper algorithm to check if we can alter the parameters of the currently executing - // block. If not, skip it and work on the next block. - // TODO: Need to look into if there are conditions where this fails. - uint8_t block_buffer_safe = next_block_index( block_buffer_tail ); - - // TODO: Need to recompute buffer tail millimeters based on how much is completed. - - if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on + /* - In theory, the state of the segment buffer can exist anywhere within the planner buffer tail and head-1 + or is empty, when there is nothing in the segment queue. The safe pointer can be the buffer head only + when the planner queue has been entirely queued into the segment buffer and there are no more blocks + in the planner buffer. The segment buffer will to continue to execute the remainder of it, but the + planner should be able to treat a newly added block during this time as an empty planner buffer since + we can't touch the segment buffer. + + - The segment buffer is atomic to the planner buffer, because the main program computes these seperately. + Even if we move the planner head pointer early at the end of plan_buffer_line(), this shouldn't + effect the safe pointer. - block_buffer_planned = block_buffer_safe; -// calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + - If the safe pointer is at head-1, this means that the stepper algorithm has segments queued and may + be executing. This is the last block in the planner queue, so it has been planned to decelerate to + zero at its end. When adding a new block, there will be at least two blocks to work with. When resuming, + from a feed hold, we only have this block and will be computing nothing. The planner doesn't have to + do anything, since the trapezoid calculations called by the stepper module should complete the block plan. + + - In most cases, the safe pointer is at the plan tail or the block after, and rarely on the block two + beyond the tail. Since the safe pointer points to the block used at the end of the segment buffer, it + can be in any one of these states. As the stepper module executes the planner block, the buffer tail, + and hence the safe pointer, can push forward through the planner blocks and overcome the planned + pointer at any time. + - Does the reverse pass not touch either the safe or the plan pointer blocks? The plan pointer only + allows the velocity profile within it to be altered, but not the entry speed, so the reverse pass + ignores this block. The safe pointer is the same way, where the entry speed does not change, but + the velocity profile within it does. + + - The planned pointer can exist anywhere in a given plan, except for the planner buffer head, if everything + operates as anticipated. Since the planner buffer can be executed by the stepper algorithm as any + rate and could empty the planner buffer quickly, the planner tail can overtake the planned pointer + at any time, but will never go around the ring buffer and re-encounter itself, the plan itself is not + changed by adding a new block or something else. + + - The planner recalculate function should always reset the planned pointer at the proper break points + or when it encounters the safe block pointer, but will only do so when there are more than one block + in the buffer. In the case of single blocks, the planned pointer should always be set to the first + write-able block in the buffer, aka safe block. + + - When does this not work? There might be an issue when the planned pointer moves from the tail to the + next head as a new block is being added and planned. Otherwise, the planned pointer should remain + static within the ring buffer no matter what the buffer is doing: being executed, adding new blocks, + or both simultaneously. Need to make sure that this case is covered. + */ + + + // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. + // NOTE: block_buffer_safe can be equal to block_buffer_head if the segment buffer has completely queued up + // the remainder of the planner buffer. In this case, a new planner block will be treated as a single block. + if (block_buffer_head == block_buffer_safe) { // Also catches head = tail + + // Just set block_buffer_planned pointer. + block_buffer_planned = block_buffer_head; + printString("z"); + + // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For + // a single added block and recalculate after a feed hold, we don't need to compute this, since we already + // know that the velocity starts and ends at zero. With an override, we can be traveling at some midblock + // rate, and we have to calculate the new velocity profile from it. + // plan_update_partial_block(block_index,0.0); + } else { - - // TODO: need to account for the two block condition better. If the currently executing block - // is not safe, do we wait until its done? Can we treat the buffer head differently? - - // Calculate trapezoid for the last/newest block. - current->entry_speed_sqr = min( current->max_entry_speed_sqr, - MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters); -// calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + + // TODO: If the nominal speeds change during a feedrate override, we need to recompute the max entry speeds for + // all junctions before proceeding. + // Initialize planner buffer pointers and indexing. + uint8_t block_index = block_buffer_head; + plan_block_t *current = &block_buffer[block_index]; + + // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); - // Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease - // planning when: (1) the last optimal planned pointer is reached. (2) the safe block - // pointer is reached, whereby the planned pointer is updated. + // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last + // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached. + // (2) the safe block pointer is reached, whereby the planned pointer is updated. + // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. + // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block + // will be recomputed within the plan. So, we need to update it if it is partially completed. float entry_speed_sqr; plan_block_t *next; block_index = prev_block_index(block_index); - while (block_index != block_buffer_planned) { - next = current; - current = &block_buffer[block_index]; + + if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. - // Exit loop and update planned pointer when the tail/safe block is reached. - if (block_index == block_buffer_safe) { - block_buffer_planned = block_buffer_safe; - break; - } + // Only two plannable blocks in buffer. Compute previous block based on + // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero. + // !!! Need to make the current entry speed calculation after this. + plan_update_partial_block(block_index, 0.0); + block_buffer_planned = block_index; +printString("y"); + + } else { - // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to - // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan. - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { - entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < current->max_entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - } else { - current->entry_speed_sqr = current->max_entry_speed_sqr; + // Three or more plan-able + while (block_index != block_buffer_planned) { + + next = current; + current = &block_buffer[block_index]; + + // Increment block index early to check if the safe block is before the current block. If encountered, + // this is an exit condition as we can't go further than this block in the reverse pass. + block_index = prev_block_index(block_index); + if (block_index == block_buffer_safe) { + // Check if the safe block is partially completed. If so, update it before its exit speed + // (=current->entry speed) is over-written. + // TODO: The update breaks with feedrate overrides, because the replanning process no longer has + // the previous nominal speed to update this block with. There will need to be something along the + // lines of a nominal speed change check and send the correct value to this function. + plan_update_partial_block(block_index,current->entry_speed_sqr); +printString("x"); + // Set planned pointer at safe block and for loop exit after following computation is done. + block_buffer_planned = block_index; + } + + // Compute maximum entry speed decelerating over the current block from its exit speed. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; + } } } - block_index = prev_block_index(block_index); - } + + } // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. - block_index = block_buffer_planned; // Begin at buffer planned pointer - next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop + next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer + block_index = next_block_index(block_buffer_planned); while (block_index != next_buffer_head) { current = next; next = &block_buffer[block_index]; @@ -194,22 +344,22 @@ static void planner_recalculate() // pointer forward, since everything before this is all optimal. In other words, nothing // can improve the plan from the buffer tail to the planned pointer by logic. if (current->entry_speed_sqr < next->entry_speed_sqr) { - block_buffer_planned = block_index; entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + // If true, current block is full-acceleration and we can move the planned pointer forward. if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this. + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + block_buffer_planned = block_index; // Set optimal plan pointer. } } // Any block set at its maximum entry speed also creates an optimal plan up to this - // point in the buffer. The optimally planned pointer is updated. + // point in the buffer. When the plan is bracketed by either the beginning of the + // buffer and a maximum entry speed or two maximum entry speeds, every block in between + // cannot logically be further improved. Hence, we don't have to recompute them anymore. if (next->entry_speed_sqr == next->max_entry_speed_sqr) { - block_buffer_planned = block_index; + block_buffer_planned = block_index; // Set optimal plan pointer } - // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned - // pointer to the end of the buffer, except the last block. -// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); block_index = next_block_index( block_index ); } @@ -218,19 +368,24 @@ static void planner_recalculate() } +void plan_reset_buffer() +{ + block_buffer_planned = block_buffer_tail; +} + void plan_init() { - block_buffer_head = 0; - block_buffer_tail = block_buffer_head; - next_buffer_head = next_block_index(block_buffer_head); - block_buffer_planned = block_buffer_head; + block_buffer_tail = 0; + block_buffer_head = 0; // Empty = tail + next_buffer_head = 1; // next_block_index(block_buffer_head) + plan_reset_buffer(); memset(&pl, 0, sizeof(pl)); // Clear planner struct } void plan_discard_current_block() { - if (block_buffer_head != block_buffer_tail) { + if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. block_buffer_tail = next_block_index( block_buffer_tail ); } } @@ -238,7 +393,10 @@ void plan_discard_current_block() plan_block_t *plan_get_current_block() { - if (block_buffer_head == block_buffer_tail) { return(NULL); } + if (block_buffer_head == block_buffer_tail) { // Buffer empty + plan_reset_buffer(); + return(NULL); + } return(&block_buffer[block_buffer_tail]); } @@ -289,6 +447,8 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later // Compute and store initial move distance data. + // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea + // to try to keep these types of things completely separate from the planner for portability. int32_t target_steps[N_AXIS]; float unit_vec[N_AXIS], delta_mm; uint8_t idx; @@ -313,7 +473,7 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) } block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() - // Bail if this is a zero-length block + // Bail if this is a zero-length block. Highly unlikely to occur. if (block->step_event_count == 0) { return; } // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) @@ -346,41 +506,59 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) } } - /* Compute maximum allowable entry speed at junction by centripetal acceleration approximation. - Let a circle be tangent to both previous and current path line segments, where the junction - deviation is defined as the distance from the junction to the closest edge of the circle, - colinear with the circle center. The circular segment joining the two paths represents the - path of centripetal acceleration. Solve for max velocity based on max acceleration about the - radius of the circle, defined indirectly by junction deviation. This may be also viewed as - path width or max_jerk in the previous grbl version. This approach does not actually deviate - from path, but used as a robust way to compute cornering speeds, as it takes into account the - nonlinearities of both the junction angle and junction velocity. - NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path - mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact - stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here - is exactly the same. Instead of motioning all the way to junction point, the machine will - just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform - a continuous mode path, but ARM-based microcontrollers most certainly do. - */ - // TODO: Acceleration need to be limited by the minimum of the two junctions. - // TODO: Need to setup a method to handle zero junction speeds when starting from rest. + + // TODO: Need to check this method handling zero junction speeds when starting from rest. if (block_buffer_head == block_buffer_tail) { - block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + + // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. + // !!! Ensures when the first block starts from zero speed. If we do this in the planner, this will break + // feedrate overrides later, as you can override this single block and it maybe moving already at a given rate. + // Better to do it here and make it clean. + // !!! Shouldn't need this for anything other than a single block. + block->entry_speed_sqr = 0.0; + block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. + } else { + /* + Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + Let a circle be tangent to both previous and current path line segments, where the junction + deviation is defined as the distance from the junction to the closest edge of the circle, + colinear with the circle center. The circular segment joining the two paths represents the + path of centripetal acceleration. Solve for max velocity based on max acceleration about the + radius of the circle, defined indirectly by junction deviation. This may be also viewed as + path width or max_jerk in the previous grbl version. This approach does not actually deviate + from path, but used as a robust way to compute cornering speeds, as it takes into account the + nonlinearities of both the junction angle and junction velocity. + + NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + is exactly the same. Instead of motioning all the way to junction point, the machine will + just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + a continuous mode path, but ARM-based microcontrollers most certainly do. + + NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be + changed dynamically during operation nor can the line segment geometry. This must be kept in + memory in the event of a feedrate override changing the nominal speeds of blocks, which can + change the overall maximum entry speed conditions of all blocks. + + */ // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. - block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2); + + // TODO: Acceleration used in calculation needs to be limited by the minimum of the two junctions. + block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, + (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); } - // Store block nominal speed and rate + // Store block nominal speed block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 -// block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) -// -// // Compute and store acceleration and distance traveled per step event. -// block->rate_delta = ceil(block->acceleration* -// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) -// block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step) - + + // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. + // TODO: Should call a function to determine this. The function can be used elsewhere for feedrate overrides later. + block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, + min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); + // Update previous path unit_vector and nominal speed (squared) memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; @@ -390,11 +568,17 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) planner_recalculate(); - // Update buffer head and next buffer head indices. - // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan - // calculations to help prevent overwriting scenarios with adding a new block to a low buffer. + // Update buffer head and next buffer head indices. Advance only after new plan has been computed. block_buffer_head = next_buffer_head; next_buffer_head = next_block_index(block_buffer_head); + + + +int32_t blength = block_buffer_head - block_buffer_tail; +if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } +printInteger(blength); + + } @@ -408,6 +592,8 @@ void plan_sync_position() } + + /* STEPPER VELOCITY PROFILE DEFINITION less than nominal rate-> + +--------+ <- nominal_rate /|\ @@ -419,31 +605,35 @@ void plan_sync_position() | | | | 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. + Calculates the "trapezoid" velocity profile parameters of a planner block for the stepper + algorithm. The planner computes the entry and exit speeds of each block, but does not bother to + determine the details of the velocity profiles within them, as they aren't needed for computing + an optimal plan. When the stepper algorithm begins to execute a block, the block velocity profiles + are computed ad hoc. + + 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. Both of these + velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, + as they can be influenced by the conditions of neighboring blocks. 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 n_steps until deceleration are computed, since the stepper algorithm - already handles acceleration and cruising and just needs to know when to start decelerating. + information for the stepper algorithm to execute the calculated profiles. Since the stepper + algorithm always assumes to begin accelerating from the initial_rate and cruise if the nominal_rate + is reached, we only need to know when to begin deceleration to the end of the block. Hence, only + the distance from the end of the block to begin a deceleration ramp are computed. */ -int32_t calculate_trapezoid_for_block(uint8_t block_index) +float plan_calculate_velocity_profile(uint8_t block_index) { plan_block_t *current_block = &block_buffer[block_index]; // Determine current block exit speed - float exit_speed_sqr; - uint8_t next_index = next_block_index(block_index); - plan_block_t *next_block = plan_get_block_by_index(next_index); - if (next_block == NULL) { exit_speed_sqr = 0; } // End of planner buffer. Zero speed. - else { exit_speed_sqr = next_block->entry_speed_sqr; } // Entry speed of next block + float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed. + plan_block_t *next_block = plan_get_block_by_index(next_block_index(block_index)); + if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block // First determine intersection distance (in steps) from the exit point for a triangular profile. - // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) + // Computes: d_intersect = distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) ); // Check if this is a pure acceleration block by a intersection distance less than zero. Also @@ -452,18 +642,17 @@ int32_t calculate_trapezoid_for_block(uint8_t block_index) float decelerate_distance; // Determine deceleration distance (in steps) 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. - // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) + // Computes: d_decelerate = (v_nominal^2 - v_exit^2)/(2*acceleration) decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration); // The lesser of the two triangle and trapezoid distances always defines the velocity profile. if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; } // Finally, check if this is a pure deceleration block. - if (decelerate_distance > current_block->millimeters) { decelerate_distance = current_block->millimeters; } - - return(ceil(((current_block->millimeters-decelerate_distance)*current_block->step_event_count)/ current_block->millimeters)); + if (decelerate_distance > current_block->millimeters) { return(0.0); } + else { return( (current_block->millimeters-decelerate_distance) ); } } - return(0); + return( current_block->millimeters ); // No deceleration in velocity profile. } @@ -481,7 +670,7 @@ void plan_cycle_reinitialize(int32_t step_events_remaining) // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. block->entry_speed_sqr = 0.0; - block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + block->max_entry_speed_sqr = 0.0; block_buffer_planned = block_buffer_tail; planner_recalculate(); } diff --git a/planner.h b/planner.h index e371bb0..083d182 100644 --- a/planner.h +++ b/planner.h @@ -33,20 +33,20 @@ typedef struct { // Fields used by the bresenham algorithm for tracing the line + // NOTE: Do not change any of these values once set. The stepper algorithm uses them to execute the block correctly. uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) int32_t steps[N_AXIS]; // Step count along each axis - int32_t step_event_count; // The number of step events required to complete this block + int32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. // Fields used by the motion planner to manage acceleration + float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable entry speed based on the minimum of junction limit and + // neighboring nominal speeds with overrides in (mm/min)^2 + float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2 float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 - float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2 - float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2 - float acceleration; // Axes-limit adjusted line acceleration in mm/min^2 - float millimeters; // The total travel for this block to be executed in mm + float acceleration; // Axis-limit adjusted line acceleration in mm/min^2 + float millimeters; // The remaining distance for this block to be executed in mm - // Settings for the trapezoid generator -// int32_t decelerate_after; // The index of the step event on which to start decelerating - } plan_block_t; // Initialize the motion plan subsystem @@ -66,7 +66,9 @@ plan_block_t *plan_get_current_block(); plan_block_t *plan_get_block_by_index(uint8_t block_index); -int32_t calculate_trapezoid_for_block(uint8_t block_index); +float plan_calculate_velocity_profile(uint8_t block_index); + +// void plan_update_partial_block(uint8_t block_index, float millimeters_remaining, uint8_t is_decelerating); // Reset the planner position vector (in steps) void plan_sync_position(); diff --git a/serial.c b/serial.c index 69fa717..d3325c7 100644 --- a/serial.c +++ b/serial.c @@ -91,11 +91,7 @@ void serial_write(uint8_t data) { } // Data Register Empty Interrupt handler -#ifdef __AVR_ATmega644P__ -ISR(USART0_UDRE_vect) -#else -ISR(USART_UDRE_vect) -#endif +ISR(SERIAL_UDRE) { // Temporary tx_buffer_tail (to optimize for volatile) uint8_t tail = tx_buffer_tail; @@ -144,11 +140,7 @@ uint8_t serial_read() } } -#ifdef __AVR_ATmega644P__ -ISR(USART0_RX_vect) -#else -ISR(USART_RX_vect) -#endif +ISR(SERIAL_RX) { uint8_t data = UDR0; uint8_t next_head; diff --git a/stepper.c b/stepper.c index 05ddd9e..6509bed 100644 --- a/stepper.c +++ b/stepper.c @@ -42,7 +42,7 @@ #define ST_DECEL 2 #define ST_DECEL_EOB 3 -#define SEGMENT_BUFFER_SIZE 10 +#define SEGMENT_BUFFER_SIZE 6 // Stepper state variable. Contains running data and trapezoid variables. typedef struct { @@ -50,11 +50,11 @@ typedef struct { int32_t counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z; - uint8_t segment_steps_remaining; // Steps remaining in line motion + uint8_t segment_steps_remaining; // Steps remaining in line segment motion // Used by inverse time algorithm to track step rate - int32_t counter_d; // Inverse time distance traveled since last step event - uint32_t delta_d; // Inverse time distance traveled per interrupt tick + int32_t counter_d; // Inverse time distance traveled since last step event + uint32_t delta_d; // Inverse time distance traveled per interrupt tick uint32_t d_per_tick; // Used by the stepper driver interrupt @@ -68,9 +68,11 @@ typedef struct { } stepper_t; static stepper_t st; -// Stores stepper buffer common data. Can change planner mid-block in special conditions. +// Stores stepper buffer common data for a planner block. Data can change mid-block when the planner +// updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is only partially used, but can fill up completely in certain conditions. typedef struct { - int32_t step_events_remaining; // Tracks step event count for the executing planner block + int32_t step_events_remaining; // Tracks step event count for the executing planner block uint32_t d_next; // Scaled distance to next step uint32_t initial_rate; // Initialized step rate at re/start of a planner block uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute @@ -80,16 +82,17 @@ typedef struct { } st_data_t; static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; -// Primary stepper motion buffer +// Primary stepper buffer. Contains small, short line segments for the stepper algorithm to execute checked +// out incrementally from the first block in the planner buffer. These step segments typedef struct { - uint8_t n_step; - uint8_t st_data_index; - uint8_t flag; + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. + uint8_t flag; // Stepper algorithm execution flag to notify special conditions. } st_segment_t; static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; static volatile uint8_t segment_buffer_tail; -static uint8_t segment_buffer_head; +static volatile uint8_t segment_buffer_head; static uint8_t segment_next_head; static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. @@ -97,11 +100,16 @@ static plan_block_t *pl_current_block; // A pointer to the planner block curren static st_segment_t *st_current_segment; static st_data_t *st_current_data; +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer static uint8_t pl_prep_index; static st_data_t *st_prep_data; static uint8_t st_data_prep_index; +static uint8_t pl_partial_block_flag; + + // Returns the index of the next block in the ring buffer // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. @@ -115,7 +123,7 @@ static uint8_t next_block_index(uint8_t block_index) static uint8_t next_block_pl_index(uint8_t block_index) { block_index++; - if (block_index == 18) { block_index = 0; } + if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } return(block_index); } @@ -255,22 +263,20 @@ ISR(TIMER2_COMPA_vect) // Initialize inverse time and step rate counter data st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } // During feed hold, do not update rate or ramp type. Keep decelerating. // if (sys.state == STATE_CYCLE) { st.delta_d = st_current_data->initial_rate; -// if (st.delta_d == st_current_data->nominal_rate) { -// st.ramp_type = RAMP_NOOP_CRUISE; - st.ramp_type = RAMP_ACCEL; st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid -// } + if (st.delta_d == st_current_data->nominal_rate) { st.ramp_type = RAMP_NOOP_CRUISE; } + else { st.ramp_type = RAMP_ACCEL; } // } - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } - else { st.d_per_tick = st.delta_d; } } - // Acceleration and cruise handled by ramping. Just check for deceleration. + // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) { if (st.ramp_type == RAMP_NOOP_CRUISE) { st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid @@ -292,6 +298,8 @@ ISR(TIMER2_COMPA_vect) } // Adjust inverse time counter for ac/de-celerations + // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally + // efficient on the Arduino AVR. This could change eventually, but it definitely will with ARM development. if (st.ramp_type) { st.ramp_count--; // Tick acceleration ramp counter if (st.ramp_count == 0) { // Adjust step rate when its time @@ -396,14 +404,20 @@ ISR(TIMER0_OVF_vect) void st_reset() { memset(&st, 0, sizeof(st)); - pl_current_block = NULL; - pl_prep_block = NULL; - pl_prep_index = 0; - st_data_prep_index = 0; + st.load_flag = LOAD_BLOCK; busy = false; + + pl_current_block = NULL; // Planner block pointer used by stepper algorithm + pl_prep_block = NULL; // Planner block pointer used by segment buffer + pl_prep_index = 0; // Planner buffer indices are also reset to zero. + st_data_prep_index = 0; + segment_buffer_tail = 0; + segment_buffer_head = 0; // empty = tail segment_next_head = 1; + + pl_partial_block_flag = false; } @@ -485,7 +499,7 @@ void st_cycle_reinitialize() } -/* Preps stepper buffer. Called from main program. +/* Prepares step segment buffer. Continuously called from main program. NOTE: There doesn't seem to be a great way to figure out how many steps occur within a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a @@ -510,41 +524,75 @@ void st_cycle_reinitialize() been doing here limit this phase issue by truncating some of the ramp timing for certain events like deceleration initialization and end of block. */ + +// !!! Need to make sure when a single partially completed block can be re-computed here with +// new deceleration point and the segment manager begins accelerating again immediately. void st_prep_buffer() { while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. // Determine if we need to load a new planner block. if (pl_prep_block == NULL) { - pl_prep_block = plan_get_block_by_index(pl_prep_index); - if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out. + pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block + if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - // Prepare commonly shared planner block data for the ensuing step buffer moves - st_data_prep_index = next_block_index(st_data_prep_index); - st_prep_data = &segment_data[st_data_prep_index]; - - // Initialize Bresenham variables - st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + // Check if the planner has re-computed this block mid-execution. If so, push the old segment block + // data Otherwise, prepare a new segment block data. + if (pl_partial_block_flag) { + + // Prepare new shared segment block data and copy the relevant last segment block data. + st_data_t *last_st_prep_data; + last_st_prep_data = &segment_data[st_data_prep_index]; + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; + st_prep_data->rate_delta = last_st_prep_data->rate_delta; + st_prep_data->d_next = last_st_prep_data->d_next; + st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Recompute with feedrate overrides. + + st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; - // Convert new block to stepper variables. - // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must - // be maintained as these execute. - // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, - // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. - st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // This data doesn't change. Could be performed in the planner, but fits nicely here. - // Although, acceleration can change for S-curves. So keep it here. - st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* - ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - // This definitely doesn't change, but could be precalculated in a way to help some of the - // math in this handler, i.e. millimeters per step event data. - st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) - st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; - - // Calculate trapezoid data from planner. - st_prep_data->decelerate_after = calculate_trapezoid_for_block(pl_prep_index); + pl_partial_block_flag = false; // Reset flag + + // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, + // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. + // The stepper algorithm must be flagged to adjust the acceleration counters. + + } else { + + // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since + // the planner buffer can dynamically change the velocity profile data as blocks are added. + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert planner block velocity profile data to stepper rate and step distance data. + st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + + // TODO: Check if we really need to store this. + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + + } + + // Convert planner entry speed to stepper initial rate. + st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Nominal rate changes with feedrate override. + // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // Calculate the planner block velocity profile type and determine deceleration point. + float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); + if (mm_decelerate_after == pl_prep_block->millimeters) { + st_prep_data->decelerate_after = st_prep_data->step_events_remaining; + } else { + st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); + } } @@ -564,77 +612,108 @@ void st_prep_buffer() - From deceleration to acceleration, i.e. common with jogging when new blocks are added. */ - st_segment_t *st_prep_segment = &segment_buffer[segment_buffer_head]; - st_prep_segment->st_data_index = st_data_prep_index; + st_segment_t *new_segment = &segment_buffer[segment_buffer_head]; + new_segment->st_data_index = st_data_prep_index; // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. - st_prep_segment->n_step = 250; //floor( (exit_speed*approx_time)/mm_per_step ); -// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? -// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + // The basic equation is: s = u*t + 0.5*a*t^2 + // For the most part, we can store the acceleration portion in the st_data buffer and all + // we would need to do is track the current approximate speed per loop with: v = u + a*t + // Each loop would require 3 multiplication and 2 additions, since most of the variables + // are constants and would get compiled out. + +//!!! Doesn't work as is. Requires last_velocity and acceleration in terms of steps, not mm. +// new_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step); +// if (st_prep_data->decelerate_after > 0) { +// new_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); +// } else { +// new_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); +// } + + new_segment->n_step = 7; //floor( (exit_speed*approx_time)/mm_per_step ); +// new_segment->n_step = max(new_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? +// new_segment->n_step = min(new_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + // Check if n_step exceeds steps remaining in planner block. If so, truncate. - if (st_prep_segment->n_step > st_prep_data->step_events_remaining) { - st_prep_segment->n_step = st_prep_data->step_events_remaining; + if (new_segment->n_step > st_prep_data->step_events_remaining) { + new_segment->n_step = st_prep_data->step_events_remaining; + + // Don't need to compute last velocity, since it will be refreshed with a new block. } // Check if n_step exceeds decelerate point in block. Need to perform this so that the // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should // be OK since it is likely moving at a fast rate already. if (st_prep_data->decelerate_after > 0) { - if (st_prep_segment->n_step > st_prep_data->decelerate_after) { - st_prep_segment->n_step = st_prep_data->decelerate_after; - } - } - -// float distance, exit_speed_sqr; -// distance = st_prep_segment->n_step*st_prep_data->mm_per_step; // Always greater than zero -// if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) { -// exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; -// } else { // Acceleration or cruising ramp -// if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) { -// exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; -// if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; } -// } else { -// exit_speed_sqr = pl_prep_block->nominal_speed_sqr; + if (new_segment->n_step > st_prep_data->decelerate_after) { + new_segment->n_step = st_prep_data->decelerate_after; + } +// !!! Doesn't work. Remove if not using. +// if (last_velocity < last_nominal_v) { +// // !!! Doesn't work since distance changes and gets truncated. +// last_velocity += pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); // In acceleration ramp. +// if {last_velocity > last_nominal_v) { last_velocity = last_nominal_v; } // Set to cruising. // } -// } - - // Update planner block variables. -// pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr); -// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable. -// pl_prep_block->millimeters -= distance; // Potential round-off error near end of block. -// pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter. +// } else { // In deceleration ramp +// last_velocity -= pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); + } // Update stepper block variables. - st_prep_data->step_events_remaining -= st_prep_segment->n_step; + st_prep_data->step_events_remaining -= new_segment->n_step; if ( st_prep_data->step_events_remaining == 0 ) { // Move planner pointer to next block if (st_prep_data->decelerate_after == 0) { - st_prep_segment->flag = ST_DECEL_EOB; + new_segment->flag = ST_DECEL_EOB; // Flag when deceleration begins and ends at EOB. Could rewrite to use bit flags too. } else { - st_prep_segment->flag = ST_END_OF_BLOCK; + new_segment->flag = ST_END_OF_BLOCK; } pl_prep_index = next_block_pl_index(pl_prep_index); pl_prep_block = NULL; -printString("EOB"); } else { + // Current segment is mid-planner block. Just set the DECEL/NOOP acceleration flags. if (st_prep_data->decelerate_after == 0) { - st_prep_segment->flag = ST_DECEL; + new_segment->flag = ST_DECEL; } else { - st_prep_segment->flag = ST_NOOP; + new_segment->flag = ST_NOOP; } -printString("x"); + st_prep_data->decelerate_after -= new_segment->n_step; } - st_prep_data->decelerate_after -= st_prep_segment->n_step; + - // New step block completed. Increment step buffer indices. + // New step segment completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; segment_next_head = next_block_index(segment_buffer_head); -printInteger((long)st_prep_segment->n_step); -printString(" "); -printInteger((long)st_prep_data->decelerate_after); -printString(" "); -printInteger((long)st_prep_data->step_events_remaining); } } + +uint8_t st_get_prep_block_index() +{ +// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this? + return(pl_prep_index); +} + +void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating) +{ + // if called, can we assume that this always changes and needs to be updated? if so, then + // we can perform all of the segment buffer setup tasks here to make sure the next time + // the segments are loaded, the st_data buffer is updated correctly. + // !!! Make sure that this is always pointing to the correct st_prep_data block. + + // When a mid-block acceleration occurs, we have to make sure the ramp counters are updated + // correctly, much in the same fashion as the deceleration counters. Need to think about this + // make sure this is right, but i'm pretty sure it is. + + // TODO: NULL means that the segment buffer has completed the block. Need to clean this up a bit. + if (pl_prep_block != NULL) { + *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step; + if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } + else { *is_decelerating = true; } + + // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. + pl_partial_block_flag = true; + pl_prep_block = NULL; + } + return; +} diff --git a/stepper.h b/stepper.h index 74ab717..8e2a6e0 100644 --- a/stepper.h +++ b/stepper.h @@ -47,4 +47,8 @@ void st_feed_hold(); void st_prep_buffer(); +uint8_t st_get_prep_block_index(); + +void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating); + #endif From 8a10654b1cb694f4b24f7fd247acc345f3275d28 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sat, 12 Oct 2013 10:35:26 -0600 Subject: [PATCH 09/13] New stepper subsystem bug fixes. - New stepper algorithm with the new optimized planner seems to be working nearly twice as fast as the previous algorithm. - For one, the planner computation overhead is probably a fraction of what it used to be with the worst case being about half still. - Secondly, anytime the planner plans back to the first executing block, it no longer overwrites the block conditions and allows it to complete without lost steps. So no matter if the streams slows, the protected planner should keep the steppers moving without risk of lost steps (although this still needs to be tested thoroughly and may audibly sound weird when this happens.) - It now seems that the bottleneck is the serial baudrate (which is good!) --- stepper.c | 173 ++++++++++++++++++++++++++++++++---------------------- 1 file changed, 104 insertions(+), 69 deletions(-) diff --git a/stepper.c b/stepper.c index 6509bed..aab0be4 100644 --- a/stepper.c +++ b/stepper.c @@ -34,13 +34,12 @@ #define RAMP_DECEL 2 #define LOAD_NOOP 0 -#define LOAD_LINE 1 +#define LOAD_SEGMENT 1 #define LOAD_BLOCK 2 -#define ST_NOOP 0 -#define ST_END_OF_BLOCK 1 -#define ST_DECEL 2 -#define ST_DECEL_EOB 3 +#define ST_END_OF_BLOCK bit(0) +#define ST_ACCEL bit(1) +#define ST_DECEL bit(2) #define SEGMENT_BUFFER_SIZE 6 @@ -87,7 +86,7 @@ static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; typedef struct { uint8_t n_step; // Number of step events to be executed for this segment uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. - uint8_t flag; // Stepper algorithm execution flag to notify special conditions. + uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. } st_segment_t; static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; @@ -208,8 +207,17 @@ void st_go_idle() 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. +/* TODO: + - Measure time in ISR. Typical and worst-case. + - Write how the acceleration counters work and why they are set at half via mid-point rule. + - Determine if placing the position counters elsewhere (or change them to 8-bit variables that + are added to the system position counters at the end of a segment) frees up cycles. + - Write a blurb about how the acceleration should be handled within the ISR. All of the + time/step/ramp counters accurately keep track of the remainders and phasing of the variables + with time. This means we do not have to compute them via expensive floating point beforehand. + - Need to do an analysis to determine if these counters are really that much cheaper. At least + find out when it isn't anymore. Particularly when the ISR is at a very high frequency. +*/ ISR(TIMER2_COMPA_vect) { // SPINDLE_ENABLE_PORT ^= 1<initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule if (st.delta_d == st_current_data->nominal_rate) { st.ramp_type = RAMP_NOOP_CRUISE; } else { st.ramp_type = RAMP_ACCEL; } // } @@ -277,16 +285,20 @@ ISR(TIMER2_COMPA_vect) } // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. - if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) { - if (st.ramp_type == RAMP_NOOP_CRUISE) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - st.ramp_type = RAMP_DECEL; + if ( st_current_segment->flag & (ST_DECEL | ST_ACCEL) ) { + /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, + or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to + the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the + ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known + rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 + as mandated by the mid-point rule. For these conditions, the ramp count has been pre-initialized + such that the following computation is still correct. */ + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; + if ( st_current_segment->flag & ST_DECEL ) { st.ramp_type = RAMP_DECEL; } + else { st.ramp_type = RAMP_ACCEL; } } - st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete. + st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. } else { // Can't discard planner block here if a feed hold stops in middle of block. @@ -299,18 +311,20 @@ ISR(TIMER2_COMPA_vect) // Adjust inverse time counter for ac/de-celerations // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally - // efficient on the Arduino AVR. This could change eventually, but it definitely will with ARM development. - if (st.ramp_type) { + // efficient on the Arduino AVR. This could may not be true with higher ISR frequencies or faster CPUs. + if (st.ramp_type) { // Ignored when ramp type is NOOP_CRUISE st.ramp_count--; // Tick acceleration ramp counter if (st.ramp_count == 0) { // Adjust step rate when its time - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter st.delta_d += st_current_data->rate_delta; if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. st.delta_d = st_current_data->nominal_rate; // Set cruising velocity - st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp. } - } else { // Adjust velocity for deceleration + } else { // Adjust velocity for deceleration. + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter if (st.delta_d > st_current_data->rate_delta) { st.delta_d -= st_current_data->rate_delta; } else { @@ -318,7 +332,11 @@ ISR(TIMER2_COMPA_vect) // Moving near zero feed rate. Gracefully slow down. st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - // Check for and handle feed hold exit? At this point, machine is stopped. + // TODO: Check for and handle feed hold exit? At this point, machine is stopped. + // - Set system flag to recompute plan and reset segment buffer. + // - Segment steps in buffer needs to be returned to planner correctly. + // busy = false; + // return; } } @@ -343,6 +361,7 @@ ISR(TIMER2_COMPA_vect) if (st.counter_x < 0) { st.out_bits |= (1<step_event_count; + // st.steps_x++; if (st.out_bits & (1<step_event_count; + // st.steps_y++; if (st.out_bits & (1<step_event_count; + // st.steps_z++; if (st.out_bits & (1< 0) { + if (st.out_bits & (1< 0) { + if (st.out_bits & (1< 0) { + if (st.out_bits & (1<flag == ST_END_OF_BLOCK || st_current_segment->flag == ST_DECEL_EOB) { + if (st_current_segment->flag & ST_END_OF_BLOCK) { plan_discard_current_block(); st.load_flag = LOAD_BLOCK; } else { - st.load_flag = LOAD_LINE; + st.load_flag = LOAD_SEGMENT; } // Discard current segment segment_buffer_tail = next_block_index( segment_buffer_tail ); - - // NOTE: sys.position updates could be done here. The bresenham counters can have - // their own fast 8-bit addition-only counters. Here we would check the direction and - // apply it to sys.position accordingly. However, this could take too much time. - + } st.out_bits ^= settings.invert_mask; // Apply step port invert mask @@ -531,13 +575,17 @@ void st_prep_buffer() { while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + prep_segment->flag = 0; + // Determine if we need to load a new planner block. if (pl_prep_block == NULL) { pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - + // Check if the planner has re-computed this block mid-execution. If so, push the old segment block - // data Otherwise, prepare a new segment block data. + // data. Otherwise, prepare a new segment block data for the new planner block. if (pl_partial_block_flag) { // Prepare new shared segment block data and copy the relevant last segment block data. @@ -553,6 +601,8 @@ void st_prep_buffer() st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; + prep_segment->flag |= ST_ACCEL; + pl_partial_block_flag = false; // Reset flag // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, @@ -588,11 +638,7 @@ void st_prep_buffer() // Calculate the planner block velocity profile type and determine deceleration point. float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); - if (mm_decelerate_after == pl_prep_block->millimeters) { - st_prep_data->decelerate_after = st_prep_data->step_events_remaining; - } else { - st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); - } + st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); } @@ -612,8 +658,7 @@ void st_prep_buffer() - From deceleration to acceleration, i.e. common with jogging when new blocks are added. */ - st_segment_t *new_segment = &segment_buffer[segment_buffer_head]; - new_segment->st_data_index = st_data_prep_index; + prep_segment->st_data_index = st_data_prep_index; // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. // The basic equation is: s = u*t + 0.5*a*t^2 @@ -623,21 +668,21 @@ void st_prep_buffer() // are constants and would get compiled out. //!!! Doesn't work as is. Requires last_velocity and acceleration in terms of steps, not mm. -// new_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step); +// prep_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step); // if (st_prep_data->decelerate_after > 0) { -// new_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); +// prep_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); // } else { -// new_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); +// prep_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); // } - new_segment->n_step = 7; //floor( (exit_speed*approx_time)/mm_per_step ); -// new_segment->n_step = max(new_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? -// new_segment->n_step = min(new_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + prep_segment->n_step = 15; //floor( (exit_speed*approx_time)/mm_per_step ); +// prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? +// prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. // Check if n_step exceeds steps remaining in planner block. If so, truncate. - if (new_segment->n_step > st_prep_data->step_events_remaining) { - new_segment->n_step = st_prep_data->step_events_remaining; + if (prep_segment->n_step > st_prep_data->step_events_remaining) { + prep_segment->n_step = st_prep_data->step_events_remaining; // Don't need to compute last velocity, since it will be refreshed with a new block. } @@ -646,8 +691,8 @@ void st_prep_buffer() // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should // be OK since it is likely moving at a fast rate already. if (st_prep_data->decelerate_after > 0) { - if (new_segment->n_step > st_prep_data->decelerate_after) { - new_segment->n_step = st_prep_data->decelerate_after; + if (prep_segment->n_step > st_prep_data->decelerate_after) { + prep_segment->n_step = st_prep_data->decelerate_after; } // !!! Doesn't work. Remove if not using. // if (last_velocity < last_nominal_v) { @@ -657,29 +702,19 @@ void st_prep_buffer() // } // } else { // In deceleration ramp // last_velocity -= pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); + } else { + if (st_prep_data->decelerate_after == 0) { prep_segment->flag |= ST_DECEL; } } - + st_prep_data->decelerate_after -= prep_segment->n_step; + // Update stepper block variables. - st_prep_data->step_events_remaining -= new_segment->n_step; + st_prep_data->step_events_remaining -= prep_segment->n_step; if ( st_prep_data->step_events_remaining == 0 ) { - // Move planner pointer to next block - if (st_prep_data->decelerate_after == 0) { - new_segment->flag = ST_DECEL_EOB; // Flag when deceleration begins and ends at EOB. Could rewrite to use bit flags too. - } else { - new_segment->flag = ST_END_OF_BLOCK; - } + prep_segment->flag |= ST_END_OF_BLOCK; + // Move planner pointer to next block and flag to load a new block for the next segment. pl_prep_index = next_block_pl_index(pl_prep_index); pl_prep_block = NULL; - } else { - // Current segment is mid-planner block. Just set the DECEL/NOOP acceleration flags. - if (st_prep_data->decelerate_after == 0) { - new_segment->flag = ST_DECEL; - } else { - new_segment->flag = ST_NOOP; - } - st_prep_data->decelerate_after -= new_segment->n_step; - } - + } // New step segment completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; @@ -705,7 +740,7 @@ void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_r // correctly, much in the same fashion as the deceleration counters. Need to think about this // make sure this is right, but i'm pretty sure it is. - // TODO: NULL means that the segment buffer has completed the block. Need to clean this up a bit. + // TODO: NULL means that the segment buffer has just completed a planner block. Clean up! if (pl_prep_block != NULL) { *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step; if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } From 0cb5756b5310b8bf0cbc554393d14e86a0186c05 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Mon, 14 Oct 2013 21:21:56 -0600 Subject: [PATCH 10/13] Fine tuning of new stepper algorithm with protected planner. Adaptive step prediction for segment buffer. - Cleaned up the new stepper algorithm code with more commenting and better logic flow. - The new segment buffer now predicts the number of steps each segment should have to execute over about 8 milliseconds each (based on the ACCELERATION_TICKS_PER_SECOND setting). So, for when the whole segment buffer is full, the stepper algorithm has roughly 40 milliseconds of steps queued before it needs to refilled by the main program. - Readjusted the max supported step rate back to 30kHz from the lower development 20kHz. Everything still works amazing great and the test CNC machine still runs twice as fast with the new stepper algorithm and planner. - Upped the standard serial baudrate to 115200 baud, as it is clear that the bottleneck is the serial interface. Will now support this, as well as the old 9600 baud, in new firmware builds. --- config.h | 4 +- defaults.h | 4 +- planner.c | 3 + stepper.c | 226 +++++++------- stepper_new2.c | 601 ------------------------------------ stepper_new_dual_ISR.c | 425 -------------------------- stepper_new_time.c | 680 ----------------------------------------- 7 files changed, 122 insertions(+), 1821 deletions(-) delete mode 100644 stepper_new2.c delete mode 100644 stepper_new_dual_ISR.c delete mode 100644 stepper_new_time.c diff --git a/config.h b/config.h index 41d6a38..f8f0810 100644 --- a/config.h +++ b/config.h @@ -57,7 +57,7 @@ // 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. -#define ISR_TICKS_PER_SECOND 20000L // Integer (Hz) +#define ISR_TICKS_PER_SECOND 30000L // Integer (Hz) // The temporal resolution of the acceleration management subsystem. Higher number give smoother // acceleration but may impact performance. If you run at very high feedrates (>15kHz or so) and @@ -66,7 +66,7 @@ // 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. // NOTE: Ramp count variable type in stepper module may need to be updated if changed. -#define ACCELERATION_TICKS_PER_SECOND 100L +#define ACCELERATION_TICKS_PER_SECOND 120L // NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. #define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) diff --git a/defaults.h b/defaults.h index df3903c..d3ddc1c 100644 --- a/defaults.h +++ b/defaults.h @@ -140,9 +140,9 @@ #define DEFAULT_Z_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV) #define DEFAULT_STEP_PULSE_MICROSECONDS 10 #define DEFAULT_ARC_TOLERANCE 0.005 // mm - #define DEFAULT_RAPID_FEEDRATE 2500.0 // mm/min + #define DEFAULT_RAPID_FEEDRATE 4000.0 // mm/min #define DEFAULT_FEEDRATE 1000.0 // mm/min - #define DEFAULT_ACCELERATION 150.0*60*60 // 150 mm/min^2 + #define DEFAULT_ACCELERATION 400.0*60*60 // 150 mm/min^2 #define DEFAULT_JUNCTION_DEVIATION 0.05 // mm #define DEFAULT_STEPPING_INVERT_MASK (1<entry_speed_sqr < next->entry_speed_sqr) { entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; // If true, current block is full-acceleration and we can move the planned pointer forward. diff --git a/stepper.c b/stepper.c index aab0be4..27cf7d6 100644 --- a/stepper.c +++ b/stepper.c @@ -37,9 +37,12 @@ #define LOAD_SEGMENT 1 #define LOAD_BLOCK 2 -#define ST_END_OF_BLOCK bit(0) -#define ST_ACCEL bit(1) -#define ST_DECEL bit(2) +#define SEGMENT_NOOP 0 +#define SEGMENT_END_OF_BLOCK bit(0) +#define SEGMENT_ACCEL bit(1) +#define SEGMENT_DECEL bit(2) + +#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change #define SEGMENT_BUFFER_SIZE 6 @@ -67,22 +70,25 @@ typedef struct { } stepper_t; static stepper_t st; -// Stores stepper buffer common data for a planner block. Data can change mid-block when the planner -// updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. -// NOTE: Normally, this buffer is only partially used, but can fill up completely in certain conditions. +// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the +// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed +// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). typedef struct { - int32_t step_events_remaining; // Tracks step event count for the executing planner block - uint32_t d_next; // Scaled distance to next step - uint32_t initial_rate; // Initialized step rate at re/start of a planner block - uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute - uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - int32_t decelerate_after; + int32_t step_events_remaining; // Tracks step event count for the executing planner block + uint32_t d_next; // Scaled distance to next step + uint32_t initial_rate; // Initialized step rate at re/start of a planner block + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + uint32_t current_approx_rate; // Tracks the approximate segment rate to predict steps per segment to execute + int32_t decelerate_after; // Tracks when to initiate deceleration according to the planner block float mm_per_step; } st_data_t; -static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; -// Primary stepper buffer. Contains small, short line segments for the stepper algorithm to execute checked -// out incrementally from the first block in the planner buffer. These step segments +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, +// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps +// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. typedef struct { uint8_t n_step; // Number of step events to be executed for this segment uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. @@ -90,6 +96,7 @@ typedef struct { } st_segment_t; static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; +// Step segment ring buffer indices static volatile uint8_t segment_buffer_tail; static volatile uint8_t segment_buffer_head; static uint8_t segment_next_head; @@ -101,13 +108,11 @@ static st_data_t *st_current_data; // Pointers for the step segment being prepped from the planner buffer. Accessed only by the // main program. Pointers may be planning segments or planner blocks ahead of what being executed. -static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer -static uint8_t pl_prep_index; -static st_data_t *st_prep_data; -static uint8_t st_data_prep_index; - -static uint8_t pl_partial_block_flag; - +static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped +static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped +static uint8_t pl_prep_index; // Index of planner block being prepped +static uint8_t st_data_prep_index; // Index of stepper common data block being prepped +static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block // Returns the index of the next block in the ring buffer @@ -194,21 +199,24 @@ void st_go_idle() /* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based - on the Pramod Ranade inverse time stepper algorithm, where a timer ticks at a constant - frequency and uses time-distance counters to track when its the approximate time for any - step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off, - meaning that some axes steps may not execute for a given multi-axis motion. - Grbl's algorithm slightly differs by using a single Ranade time-distance counter to manage - a Bresenham line algorithm for multi-axis step events which ensures the number of steps for - each axis are executed exactly. In other words, it uses a Bresenham within a Bresenham algorithm, - where one tracks time(Ranade) and the other steps. - This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins - appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the - stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper - outputs simultaneously with these two interrupts. + on an 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 a step event. For reference, + a similar inverse-time algorithm by Pramod Ranade is susceptible to numerical round-off, + meaning that some axes steps may not execute correctly for a given multi-axis motion. + Grbl's algorithm differs by using a single inverse time-distance counter to manage a + Bresenham line algorithm for multi-axis step events, which ensures the number of steps for + each axis are executed exactly. In other words, Grbl uses a Bresenham within a Bresenham + algorithm, where one tracks time for step events and the other steps for multi-axis moves. + Grbl specifically uses the Bresenham algorithm due to its innate mathematical exactness and + low computational overhead, requiring simple integer +,- counters only. + This interrupt pops blocks from the step segment 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. */ /* TODO: - - Measure time in ISR. Typical and worst-case. + - Measure time in ISR. Typical and worst-case. Should be virtually identical to last algorithm. + There are no major changes to the base operations of this ISR with the new segment buffer. - Write how the acceleration counters work and why they are set at half via mid-point rule. - Determine if placing the position counters elsewhere (or change them to 8-bit variables that are added to the system position counters at the end of a segment) frees up cycles. @@ -278,23 +286,22 @@ ISR(TIMER2_COMPA_vect) // if (sys.state == STATE_CYCLE) { st.delta_d = st_current_data->initial_rate; st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule - if (st.delta_d == st_current_data->nominal_rate) { st.ramp_type = RAMP_NOOP_CRUISE; } - else { st.ramp_type = RAMP_ACCEL; } + st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. // } } // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. - if ( st_current_segment->flag & (ST_DECEL | ST_ACCEL) ) { + if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) { /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 - as mandated by the mid-point rule. For these conditions, the ramp count has been pre-initialized + as mandated by the mid-point rule. For these conditions, the ramp count have been initialized such that the following computation is still correct. */ st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; - if ( st_current_segment->flag & ST_DECEL ) { st.ramp_type = RAMP_DECEL; } + if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; } else { st.ramp_type = RAMP_ACCEL; } } @@ -415,7 +422,7 @@ ISR(TIMER2_COMPA_vect) // Line move is complete, set load line flag to check for new move. // Check if last line move in planner block. Discard if so. - if (st_current_segment->flag & ST_END_OF_BLOCK) { + if (st_current_segment->flag & SEGMENT_END_OF_BLOCK) { plan_discard_current_block(); st.load_flag = LOAD_BLOCK; } else { @@ -434,8 +441,8 @@ ISR(TIMER2_COMPA_vect) } -// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the -// step pulse. This should always trigger before the next Timer2 COMPA interrupt and independently +// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the step +// pulse. This should always trigger before the next Timer2 COMPA interrupt and independently // finish, if Timer2 is disabled after completing a move. ISR(TIMER0_OVF_vect) { @@ -569,51 +576,61 @@ void st_cycle_reinitialize() events like deceleration initialization and end of block. */ -// !!! Need to make sure when a single partially completed block can be re-computed here with -// new deceleration point and the segment manager begins accelerating again immediately. +/* + TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. + The problem is that when an override is reduced, the planner may not plan back to + the current rate. Meaning that the velocity profiles for certain conditions no longer + are trapezoidal or triangular. For example, if the current block is cruising at a + nominal rate and the feedrate override is reduced, the new nominal rate will now be + lower. The velocity profile must first decelerate to the new nominal rate and then + follow on the new plan. So the remaining velocity profile will have a decelerate, + cruise, and another decelerate. + Another issue is whether or not a feedrate override reduction causes a deceleration + that acts over several planner blocks. For example, say that the plan is already + heavily decelerating throughout it, reducing the feedrate will not do much to it. So, + how do we determine when to resume the new plan? How many blocks do we have to wait + until the new plan intersects with the deceleration curve? One plus though, the + deceleration will never be more than the number of blocks in the entire planner buffer, + but it theoretically can be equal to it when all planner blocks are decelerating already. +*/ void st_prep_buffer() { while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - + // Initialize new segment st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; - prep_segment->flag = 0; + prep_segment->flag = SEGMENT_NOOP; // Determine if we need to load a new planner block. if (pl_prep_block == NULL) { pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - // Check if the planner has re-computed this block mid-execution. If so, push the old segment block - // data. Otherwise, prepare a new segment block data for the new planner block. + // Increment stepper common data buffer index + if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } + + // Check if the planner has re-computed this block mid-execution. If so, push the previous segment + // data. Otherwise, prepare a new segment data for the new planner block. if (pl_partial_block_flag) { // Prepare new shared segment block data and copy the relevant last segment block data. st_data_t *last_st_prep_data; - last_st_prep_data = &segment_data[st_data_prep_index]; - st_data_prep_index = next_block_index(st_data_prep_index); + last_st_prep_data = st_prep_data; st_prep_data = &segment_data[st_data_prep_index]; st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; st_prep_data->rate_delta = last_st_prep_data->rate_delta; st_prep_data->d_next = last_st_prep_data->d_next; - st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Recompute with feedrate overrides. + st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; - prep_segment->flag |= ST_ACCEL; - pl_partial_block_flag = false; // Reset flag - // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, - // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. - // The stepper algorithm must be flagged to adjust the acceleration counters. - } else { // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since // the planner buffer can dynamically change the velocity profile data as blocks are added. - st_data_prep_index = next_block_index(st_data_prep_index); st_prep_data = &segment_data[st_data_prep_index]; // Initialize Bresenham variables @@ -636,81 +653,64 @@ void st_prep_buffer() // TODO: Nominal rate changes with feedrate override. // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - // Calculate the planner block velocity profile type and determine deceleration point. + st_prep_data->current_approx_rate = st_prep_data->initial_rate; + + // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); - + if (st_prep_data->decelerate_after > 0) { // If 0, SEGMENT_DECEL flag is set later. + if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = SEGMENT_ACCEL; } + } } - - /* - TODO: Need to check for a planner flag to indicate a change to this planner block. - If so, need to check for a change in acceleration state, from deceleration to acceleration, - to reset the stepper ramp counters and the initial_rate data to trace the new - ac/de-celeration profile correctly. - No change conditions: - - From nominal speed to acceleration from feedrate override - - From nominal speed to new deceleration. - - From acceleration to new deceleration point later or cruising point. - - From acceleration to immediate deceleration? Can happen during feedrate override - and slowing down, but likely ok by enforcing the normal ramp counter protocol. - Change conditions: - - From deceleration to acceleration, i.e. common with jogging when new blocks are added. - */ - + // Set new segment to point to the current segment data block. prep_segment->st_data_index = st_data_prep_index; - // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. - // The basic equation is: s = u*t + 0.5*a*t^2 - // For the most part, we can store the acceleration portion in the st_data buffer and all - // we would need to do is track the current approximate speed per loop with: v = u + a*t - // Each loop would require 3 multiplication and 2 additions, since most of the variables - // are constants and would get compiled out. + // Approximate the velocity over the new segment + if (st_prep_data->decelerate_after <= 0) { + if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } + else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } + if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } + } else { + if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate += st_prep_data->rate_delta; + if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate = st_prep_data->nominal_rate; + } + } + } -//!!! Doesn't work as is. Requires last_velocity and acceleration in terms of steps, not mm. -// prep_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step); -// if (st_prep_data->decelerate_after > 0) { -// prep_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); -// } else { -// prep_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); -// } - - prep_segment->n_step = 15; //floor( (exit_speed*approx_time)/mm_per_step ); -// prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? -// prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + // Compute the number of steps in the prepped segment based on the approximate current rate. The execution + // time of each segment should be about every ACCELERATION_TICK. + // NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. + // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255. + prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* + (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next); + prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); // Ensure it moves for very slow motions? + // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. // Check if n_step exceeds steps remaining in planner block. If so, truncate. if (prep_segment->n_step > st_prep_data->step_events_remaining) { prep_segment->n_step = st_prep_data->step_events_remaining; - - // Don't need to compute last velocity, since it will be refreshed with a new block. } - // Check if n_step exceeds decelerate point in block. Need to perform this so that the - // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should - // be OK since it is likely moving at a fast rate already. + // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration + // ramp counters are set correctly during execution. if (st_prep_data->decelerate_after > 0) { if (prep_segment->n_step > st_prep_data->decelerate_after) { prep_segment->n_step = st_prep_data->decelerate_after; } -// !!! Doesn't work. Remove if not using. -// if (last_velocity < last_nominal_v) { -// // !!! Doesn't work since distance changes and gets truncated. -// last_velocity += pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); // In acceleration ramp. -// if {last_velocity > last_nominal_v) { last_velocity = last_nominal_v; } // Set to cruising. -// } -// } else { // In deceleration ramp -// last_velocity -= pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); - } else { - if (st_prep_data->decelerate_after == 0) { prep_segment->flag |= ST_DECEL; } } - st_prep_data->decelerate_after -= prep_segment->n_step; // Update stepper block variables. + st_prep_data->decelerate_after -= prep_segment->n_step; st_prep_data->step_events_remaining -= prep_segment->n_step; + + // Check for end of planner block if ( st_prep_data->step_events_remaining == 0 ) { - prep_segment->flag |= ST_END_OF_BLOCK; + // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. + prep_segment->flag |= SEGMENT_END_OF_BLOCK; // Move planner pointer to next block and flag to load a new block for the next segment. pl_prep_index = next_block_pl_index(pl_prep_index); pl_prep_block = NULL; @@ -719,6 +719,10 @@ void st_prep_buffer() // New step segment completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; segment_next_head = next_block_index(segment_buffer_head); + +// long a = prep_segment->n_step; +// printInteger(a); +// printString(" "); } } diff --git a/stepper_new2.c b/stepper_new2.c deleted file mode 100644 index c248d4d..0000000 --- a/stepper_new2.c +++ /dev/null @@ -1,601 +0,0 @@ -/* - stepper.c - stepper motor driver: executes motion plans using stepper motors - Part of Grbl - - Copyright (c) 2011-2013 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 - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" -#include "nuts_bolts.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define CRUISE_RAMP 0 -#define ACCEL_RAMP 1 -#define DECEL_RAMP 2 - -#define LOAD_NOOP 0 -#define LOAD_LINE 1 -#define LOAD_BLOCK 2 - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; - int32_t step_events_remaining; // Steps remaining in line motion - - // Used by inverse time algorithm - int32_t delta_d; // Inverse time distance traveled per interrupt tick - int32_t d_counter; // Inverse time distance traveled since last step event - int32_t d_per_tick; - - // Used by the stepper driver interrupt - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t out_bits; // The next stepping-bits to be output - uint8_t load_flag; -} stepper_t; -static stepper_t st; - -#define STEPPER_BUFFER_SIZE 5 -typedef struct { - int32_t event_count; - int32_t rate; - uint8_t end_of_block; - uint8_t tick_count; - - int32_t initial_rate; // The step rate at start of block - int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - int32_t decelerate_after; // The index of the step event on which to start decelerating - int32_t nominal_rate; // The nominal step rate for this block in step_events/minute - int32_t d_next; // Scaled distance to next step - -} stepper_buffer_t; -static stepper_buffer_t step_buffer[STEPPER_BUFFER_SIZE]; - -static volatile uint8_t step_buffer_tail; -static uint8_t step_buffer_head; -static uint8_t step_next_head; - -// 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. -static plan_block_t *plan_current_block; // A pointer to the planner block currently being traced - - -/* __________________________ - /| |\ _________________ ^ - / | | \ /| |\ | - / | | \ / | | \ s - / | | | | | \ p - / | | | | | \ e - +-----+------------------------+---+--+---------------+----+ e - | BLOCK 1 | BLOCK 2 | d - - time -----> - - The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta - until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after - after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as - +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. -*/ - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - // Enable stepper driver interrupt - st.execute_step = false; - TCNT2 = 0; // Clear Timer2 - TIMSK2 |= (1<direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits. - - st.counter_x = (plan_current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - - // This is correct. Sets the total time before the next step occurs. - st.counter_d = plan_current_block->d_next; // d_next always greater than delta_d. - - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - - } - - st.load_flag = LOAD_NOOP; // Line motion loaded. Set no-operation flag until complete. - - } else { - // Can't discard planner block here if a feed hold stops in middle of block. - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - - } - - // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_d -= st.delta_d; - - // Execute Bresenham step event, when it's time to do so. - if (st.counter_d < 0) { - st.counter_d += plan_current_block->d_next; // Reload inverse time counter - - st.out_bits = plan_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 -= plan_current_block->steps[X_AXIS]; - if (st.counter_x < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<step_event_count; - //st.step_events_remaining = st.event_count; - - // Convert new block to stepper variables. - // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must - // be maintained as these execute. - // TODO: The initial rate needs to be sent back to the planner to update the entry speed - block->initial_rate = ceil(sqrt(plan_current_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - block->nominal_rate = ceil(plan_current_block->nominal_speed*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // This data doesn't change. Could be performed in the planner, but fits nicely here. - // Although, acceleration can change for S-curves. So keep it here. - block->rate_delta = ceil(plan_current_block->acceleration* - ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - // This definitely doesn't change, but could be precalculated in a way to help some of the - // math in this handler, i.e. millimeters per step event data. - block->d_next = ceil((plan_current_block->millimeters*INV_TIME_MULTIPLIER)/plan_current_block->step_event_count); // (mult*mm/step) - - // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { } - } - - -// Track instead the trapezoid line and use the average of the entry and exit velocities -// to determine step rate. This should take care of the deceleration issue automatically... -// i think. -// First need to figure out what type of profile is segment is, i.e. acceleration only, accel -// to decel triangle, cruise to decel, or all three. May need more profile data to compute this -// from the planner itself, like accelerate until. -// Another issue. This is only tracking the velocity profile, not the distance covered over that -// time period. This can lead to an unsynchronized velocity profile and steps executed. But, -// how much drift is there really? Enough to be a problem? Not sure. I would think typical -// drift would be on the order of a few steps, more depending on step resolution. -entry_rate = last_exit_rate; -time = 250 ISR ticks per acceleration tick. -distance = 0; -if (distance_traveled < accelerate_until) - exit_rate = entry_rate + acceleration*time; - if (exit_rate > nominal_rate) { - exit_rate = nominal_rate; - time = 2*(accelerate_until-distance_travel)/(entry_rate+nominal_rate); - // distance = accelerate_until; // Enforce distance? - // Truncate this segment. - } -} else if (distance_traveled >= decelerate_after) { - if (accelerate_until == decelerate_after) { - time = last time; - exit_rate = entry_rate; - } else { - exit_rate = entry_rate - acceleration*time; - } -} else { - exit_rate = nominal_rate; // Just cruise - distance = nominal_rate*time; - if (distance > decelerate_after) { // Truncate segment at nominal rate. - time = (decelerate_after-distance_traveled)/(nominal_rate); - distance = decelerate_after; - } -} -mean_rate = 0.5*(entry_rate+exit_rate); -distance = mean_rate*time; - - -if (entry_rate < nominal_rate) { - if (entry_distance < decelerate_after) { // Acceleration case - exit_rate = entry_rate + acceleration*time - exit_rate = min(exit_rate,nominal_rate); -mean_rate = 0.5*(entry_rate + exit_rate); -distance = mean_rate*time; -if (distance > decelerate_after) { - exit_rate = - - - // If the MINIMUM_STEP_RATE is less than ACCELERATION_TICKS_PER_SECOND then there can be - // rate adjustements that have less than one step per tick. - // How do you deal with the remainer? -time = 250 ISR ticks per acceleration tick. (30000/120) -delta_d*time // mm per acceleration tick -delta_d*time/d_next // number of steps/acceleration_tick. Chance of integer overflow. -delta_d*time/d_next + last_remainder. // steps/acceleration_tick. -n_step*d_next/delta_d // number of ISR ticks for enforced n_steps. - -// In floating point? Then convert? -// Requires exact millimeters. Roundoff might be a problem. But could be corrected by just -// checking if the total step event counts are performed. -// Could be limited by float conversion and about 1e7 steps per block. -line_mm = feed_rate / acc_tick // mm per acc_tick -n_steps = floor(line_mm * step_event_remaining/millimeters_remaining) // steps. float 7.2 digits|int32 10 digits -millimeters_remaining -= line_mm; -step_events_remaining -= n_steps; - -// There doesn't seem to be a way to avoid this divide here. -line_mm = feed_rate / acc_tick // mm per acc_tick -n_steps = floor( (line_mm+line_remainder) * step_event_count/millimeters) // steps. float 7.2 digits|int32 10 digits -line_remainder = line_mm - n_steps*(millimeters/step_event_count); - -// Need to handle when rate is very very low, i.e. less than one step per accel tick. -// Could be bounded by MINIMUM_STEP_RATE. - -// 1. Figure out how many steps occur exactly within n ISR ticks. -// 2. Account for step-time remainder for next line motion exactly. -// 3. At the end of block, determine exact number of ISR ticks to finish the steps. Or,\ - have the ISR track steps to exit on time. It would require an extra counter. - -// NOTE: There doesn't seem to be a great way to figure out how many steps occur within -// a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a -// critical problem. So, either numerical round-off checks could be made to account for -// them, while CPU overhead could be minimized in some way, or we can flip the algorithm -// around to have the stepper algorithm track number of steps over an indeterminant amount -// of time instead. -// In other words, we use the planner velocity floating point data to get an estimate of -// the number of steps we want to execute. We then back out the approximate velocity for -// the planner to use, which should be much more robust to round-off error. The main problem -// now is that we are loading the stepper algorithm to handle acceleration now, rather than -// pre-calculating with the main program. This approach does make sense in the way that -// planner velocities and stepper profiles can be traced more accurately. -// Which is better? Very hard to tell. The time-based algorithm would be able to handle -// Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would -// require some additional math in the stepper algorithm to adjust on the fly, plus adaptation -// would occur in a non-deterministic manner. -// I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. - -feed_rate/120 = millimeters per acceleration tick - - - steps? - d_next // (mult*mm/step) - rate // (mult*mm/isr_tic) - rate/d_next // step/isr_tic - - if (plan_current_block->step_events_remaining <= plan_current_block->decelerate_after) { - // Determine line segment velocity and associated inverse time counter. - if (step_block.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration - step_block.delta_d += plan_current_block->rate_delta; - if (step_block.delta_d >= plan_current_block->nominal_rate) { // Reached cruise state. - step_block.ramp_type = CRUISE_RAMP; - step_block.delta_d = plan_current_block->nominal_rate; // Set cruise velocity - } - } - } else { // Adjust velocity for deceleration - if (step_block.delta_d > plan_current_block->rate_delta) { - step_block.delta_d -= plan_current_block->rate_delta; - } else { - step_block.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - } - } - - // Incorrect. Can't overwrite delta_d. Needs to override instead. - if (step_block.delta_d < MINIMUM_STEP_RATE) { step_block.delta_d = MINIMUM_STEP_RATE; } - - /* - Compute the number of steps needed to complete this move over the move time, i.e. - ISR_TICKS_PER_ACCELERATION_TICK. - - The first block in the buffer is half of the move time due to midpoint rule. - - Check if this reaches the deceleration after location. If so, truncate move. Also, - if this is a triangle move, double the truncated move to stay with midpoint rule. - NOTE: This can create a stepper buffer move down to just one step in length. - - Update the planner block entry speed for the planner to compute from end of the - stepper buffer location. - - If a feed hold occurs, begin to enforce deceleration, while enforcing the above rules. - When the deceleration is complete, all we need to do is update the planner block - entry speed and force a replan. - */ - - // Planner block move completed. - // TODO: planner buffer tail no longer needs to be volatile. only accessed by main program. - if (st.step_events_remaining == 0) { - plan_current_block = NULL; // Set flag that we are done with this planner block. - plan_discard_current_block(); - } - - - - - - step_buffer_head = step_next_head; - step_next_head = next_block_index(step_buffer_head); - - } -} diff --git a/stepper_new_dual_ISR.c b/stepper_new_dual_ISR.c deleted file mode 100644 index 87f87bc..0000000 --- a/stepper_new_dual_ISR.c +++ /dev/null @@ -1,425 +0,0 @@ -/* - stepper.c - stepper motor driver: executes motion plans using stepper motors - Part of Grbl - - Copyright (c) 2011-2013 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 - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define CRUISE_RAMP 0 -#define ACCEL_RAMP 1 -#define DECEL_RAMP 2 - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter[N_AXIS]; // Counter variables for the bresenham line tracer - uint32_t event_count; // Total event count. Retained for feed holds. - uint32_t step_events_remaining; // Steps remaining in motion - - // Used by Pramod Ranade inverse time algorithm - int32_t delta_d; // Ranade distance traveled per interrupt tick - int32_t d_counter; // Ranade distance traveled since last step event - uint8_t ramp_count; // Acceleration interrupt tick counter. - uint8_t ramp_type; // Ramp type variable. - uint8_t execute_step; // Flags step execution for each interrupt. - -} 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 - -// 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. - -// __________________________ -// /| |\ _________________ ^ -// / | | \ /| |\ | -// / | | \ / | | \ s -// / | | | | | \ p -// / | | | | | \ e -// +-----+------------------------+---+--+---------------+----+ e -// | BLOCK 1 | BLOCK 2 | d -// -// time -----> -// -// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta -// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after -// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as -// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. - - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - // Enable stepper driver interrupt - st.execute_step = false; - TCNT0 = 0; // Clear Timer2 - TIMSK0 |= (1<d_next; - st.execute_step = true; - - // Configure next step - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits - - // Execute step displacement profile by Bresenham line algorithm - st.counter[X_AXIS] -= current_block->steps_x; // Doesn't change when set up. - if (st.counter[X_AXIS] < 0) { - out_bits |= (1<steps_y; - if (st.counter[Y_AXIS] < 0) { - out_bits |= (1<steps_z; - if (st.counter[Z_AXIS] < 0) { - out_bits |= (1<direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits. - - // Initialize Bresenham variables - st.counter_x = (current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - st.event_count = current_block->step_event_count; - st.step_events_remaining = st.event_count; - - // During feed hold, do not update inverse time counter, rate, or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { - // Initialize Ranade variables - st.d_counter = current_block->d_next; - st.delta_d = current_block->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - - // Initialize ramp type. - if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } - else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } - else { st.ramp_type = ACCEL_RAMP; } - } - - } else { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - } - - // Adjust inverse time counter for ac/de-celerations - if (st.ramp_type) { - // Tick acceleration ramp counter - st.ramp_count--; - if (st.ramp_count == 0) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration - st.delta_d += current_block->rate_delta; - if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. - st.ramp_type = CRUISE_RAMP; - st.delta_d = current_block->nominal_rate; // Set cruise velocity - } - } 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 >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - } - } - } - } - - - // Check for feed hold state and execute accordingly. - if (sys.state == STATE_HOLD) { - if (st.ramp_type != DECEL_RAMP) { - st.ramp_type = DECEL_RAMP; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - } - if (st.delta_d <= current_block->rate_delta) { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); - return; - } - } - - if (st.ramp_type != DECEL_RAMP) { - // Acceleration and cruise handled by ramping. Just check for deceleration. - if (st.step_events_remaining <= current_block->decelerate_after) { - st.ramp_type = DECEL_RAMP; - if (st.step_events_remaining == current_block->decelerate_after) { - if (st.delta_d == current_block->nominal_rate) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - } - } - } - } else { - - busy = false; -} - - -// The Stepper Port Reset Interrupt: Timer2 OVF interrupt handles the falling edge of the -// step pulse. This should always trigger before the next Timer0 COMPA interrupt and independently -// finish, if Timer0 is disabled after completing a move. -ISR(TIMER2_OVF_vect) -{ - STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); - TCCR2B = 0; // Disable timer until needed. -} - - -// Reset and clear stepper subsystem variables -void st_reset() -{ - memset(&st, 0, sizeof(st)); - current_block = NULL; - busy = false; -} - - -// Initialize and start the stepper motor subsystem -void st_init() -{ - // Configure directions of interface pins - STEPPING_DDR |= STEPPING_MASK; - STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask; - STEPPERS_DISABLE_DDR |= 1<. -*/ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" -#include "nuts_bolts.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) - -#define RAMP_NOOP_CRUISE 0 -#define RAMP_ACCEL 1 -#define RAMP_DECEL 2 - -#define LOAD_NOOP 0 -#define LOAD_LINE 1 -#define LOAD_BLOCK 2 - -#define ST_NOOP 0 -#define ST_END_OF_BLOCK 1 - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; - int8_t segment_steps_remaining; // Steps remaining in line motion - - // Used by inverse time algorithm to track step rate - int32_t counter_d; // Inverse time distance traveled since last step event - int32_t delta_d; // Inverse time distance traveled per interrupt tick - int32_t d_per_tick; - - // Used by the stepper driver interrupt - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t out_bits; // The next stepping-bits to be output - uint8_t load_flag; - - uint8_t ramp_count; - uint8_t ramp_type; -} stepper_t; -static stepper_t st; - -#define SEGMENT_BUFFER_SIZE 5 -// Stores stepper buffer common data. Can change planner mid-block in special conditions. -typedef struct { - int32_t step_events_remaining; // Tracks step event count for the executing planner block - int32_t d_next; // Scaled distance to next step - float mm_per_step; -} st_data_t; -static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; - -// Primary stepper motion buffer -typedef struct { - uint8_t n_step; - int32_t rate; - uint8_t st_data_index; - uint8_t flag; -} st_segment_t; -static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; - -static volatile uint8_t segment_buffer_tail; -static uint8_t segment_buffer_head; -static uint8_t segment_next_head; - -static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. -static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced -static st_segment_t *st_current_segment; -static st_data_t *st_current_data; - -static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer -static uint8_t pl_prep_index; -static st_data_t *st_prep_data; -static uint8_t st_data_prep_index; - - -// Returns the index of the next block in the ring buffer -// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. -static uint8_t next_block_index(uint8_t block_index) -{ - block_index++; - if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; } - return(block_index); -} - - -/* __________________________ - /| |\ _________________ ^ - / | | \ /| |\ | - / | | \ / | | \ s - / | | | | | \ p - / | | | | | \ e - +-----+------------------------+---+--+---------------+----+ e - | BLOCK 1 | BLOCK 2 | d - - time -----> - - The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta - until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after - after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as - +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. -*/ - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - // Enable stepper driver interrupt - st.execute_step = false; - TCNT2 = 0; // Clear Timer2 - TIMSK2 |= (1<n_step; - st.delta_d = st_current_segment->rate; - - // Check if the counters need to be reset for a new planner block - if (st.load_flag == LOAD_BLOCK) { - pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. - st_current_data = &segment_data[st_current_segment->st_data_index]; - - // Initialize direction bits for block - st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits upon next ISR tick. - - // Initialize Bresenham line counters - st.counter_x = (pl_current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - - // Initialize inverse time and step rate counter data - st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. - } - st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete. - - } else { - // Can't discard planner block here if a feed hold stops in middle of block. - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - - } - - // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_d -= st.delta_d; - - // Execute Bresenham step event, when it's time to do so. - if (st.counter_d < 0) { - st.counter_d += st_current_data->d_next; // Reload inverse time counter - - st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits - st.execute_step = true; - - // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= pl_current_block->steps[X_AXIS]; - if (st.counter_x < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<flag == ST_END_OF_BLOCK) { - plan_discard_current_block(); - st.load_flag = LOAD_BLOCK; - } else { - st.load_flag = LOAD_LINE; - } - - // Discard current block - if (segment_buffer_head != segment_buffer_tail) { - segment_buffer_tail = next_block_index( segment_buffer_tail ); - } - - // NOTE: sys.position updates could be done here. The bresenham counters can have - // their own fast 8-bit addition-only counters. Here we would check the direction and - // apply it to sys.position accordingly. However, this could take too much time. - - } - - st.out_bits ^= settings.invert_mask; // Apply step port invert mask - } - busy = false; -// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); - st.ramp_type = RAMP_ACCEL; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - st.delta_d = 0; - sys.state = STATE_QUEUED; - } else { - sys.state = STATE_IDLE; - } -} - - -/* Preps stepper buffer. Called from main program. - - NOTE: There doesn't seem to be a great way to figure out how many steps occur within - a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a - critical problem. So, either numerical round-off checks could be made to account for - them, while CPU overhead could be minimized in some way, or we can flip the algorithm - around to have the stepper algorithm track number of steps over an indeterminant amount - of time instead. - In other words, we use the planner velocity floating point data to get an estimate of - the number of steps we want to execute. We then back out the approximate velocity for - the planner to use, which should be much more robust to round-off error. The main problem - now is that we are loading the stepper algorithm to handle acceleration now, rather than - pre-calculating with the main program. This approach does make sense in the way that - planner velocities and stepper profiles can be traced more accurately. - Which is better? Very hard to tell. The time-based algorithm would be able to handle - Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would - require some additional math in the stepper algorithm to adjust on the fly, plus adaptation - would occur in a non-deterministic manner. - I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. - - TODO: Need to describe the importance of continuations of step pulses between ramp states - and planner blocks. This has to do with Alden's problem with step "phase". The things I've - been doing here limit this phase issue by truncating some of the ramp timing for certain - events like deceleration initialization and end of block. -*/ -void st_prep_buffer() -{ - while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - - // Determine if we need to load a new planner block. - if (pl_prep_block == NULL) { - pl_prep_block = plan_get_block_by_index(pl_prep_index); - if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out. - - // Prepare commonly shared planner block data for the ensuing step buffer moves - st_data_prep_index = next_block_index(st_data_prep_index); - st_prep_data = &segment_data[st_data_prep_index]; - - // Initialize Bresenham variables - st_prep_data->step_events_remaining = pl_prep_block->step_event_count; - - // Convert new block to stepper variables. - // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must - // be maintained as these execute. - // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, - // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. -// st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) -// st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // This data doesn't change. Could be performed in the planner, but fits nicely here. - // Although, acceleration can change for S-curves. So keep it here. -// st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* -// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - // This definitely doesn't change, but could be precalculated in a way to help some of the - // math in this handler, i.e. millimeters per step event data. - st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) - - st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; - } - - - /* - // Check if planner has changed block exit parameters. If so, then we have to update the - // velocity profile for the remainder of this block. Otherwise, the block hasn't changed. - if (exit_speed_sqr != last_exit_speed_sqr) { - intersect_distance = 0.5*( millimeters + (entry_speed_sqr-exit_speed_sqr)/(2*acceleration) ); - if (intersect_distance <= 0) { // Deceleration only. - final_rate_sqr = initial_rate_sqr - 2*acceleration*segment_distance; - block->decelerate_after = 0; - } else { - decelerate_after = (block->nominal_speed_sqr - exit_speed_sqr)/(2*block->acceleration); - if (decelerate_after > intersect_distance) { decelerate_after = intersect_distance; } - if (decelerate_after > block->millimeters) { decelerate_after = block->millimeters; } - } - } - - - - - - n_step = 100; // Estimate distance we're going to travel in this segment - if (n_step > step_events_remaining) { n_step = step_events_remaining; }; - - segment_distance = n_step*mm_per_step; // True distance traveled - - // ISSUE: Either weighted average speed or truncated segments must be used. Otherwise - // accelerations, in particular high value, will not perform correctly. - if (distance_traveled < decelerate_after) { - if (segment_distance + distance_traveled > decelerate_after) { - n_step = ceil((decelerate_after-distance_traveled)/mm_per_step); - segment_distance = n_step*mm_per_step; - } - } - - - - - // based on time? - time = incremental time; - v_exit = v_entry + acceleration*time; - if (v_exit > v_nominal) { - time_accel = (v_nominal-v_entry)/acceleration; - distance = v_entry*time_accel + 0.5*acceleration*time_accel**2; - time_remaining = time - time_accel; - } - distance = v_entry*time + 0.5*acceleration*time*time; - - - t_accel = (v_nominal - v_entry) / acceleration; - t_decel = (v_exit - v_nominal) / -acceleration; - dt = (v_entry-v_exit)/acceleration; - if - - - - - // What's the speed of this segment? Computing per segment can get expensive, but this - // would allow S-curves to be installed pretty easily here. - if (initial_speed < nominal_speed) { - if (initial_distance < decelerate_after) { // Acceleration - exit_speed = sqrt(initial_speed*initial_speed + 2*acceleration*distance); - if (exit_speed > nominal_speed) { exit_speed = nominal_speed; } - } else { // Deceleration - exit_speed = sqrt(initial_speed*initial_speed - 2*acceleration*distance); - } - average_speed = 0.5*(initial_speed + exit_speed); - } else { // Cruise - average_speed = nominal_speed; - } - segment_rate = ceil(average_speed*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - if (segment_rate < MINIMUM_STEP_RATE) { segment_rate = MINIMUM_STEP_RATE; } - - - - - */ - - - - /* - TODO: Need to check for a planner flag to indicate a change to this planner block. - If so, need to check for a change in acceleration state, from deceleration to acceleration, - to reset the stepper ramp counters and the initial_rate data to trace the new - ac/de-celeration profile correctly. - No change conditions: - - From nominal speed to acceleration from feedrate override - - From nominal speed to new deceleration. - - From acceleration to new deceleration point later or cruising point. - - From acceleration to immediate deceleration? Can happen during feedrate override - and slowing down, but likely ok by enforcing the normal ramp counter protocol. - Change conditions: - - From deceleration to acceleration, i.e. common with jogging when new blocks are added. - */ - - st_segment_t *st_prep_block = &segment_buffer[segment_buffer_head]; - st_prep_block->st_data_index = st_data_prep_index; - - // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. - st_prep_block->n_step = 100; //floor( (exit_speed*approx_time)/mm_per_step ); -// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? -// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. - - // Check if n_step exceeds steps remaining in planner block. If so, truncate. - if (st_prep_block->n_step > st_prep_data->step_events_remaining) { - st_prep_block->n_step = st_prep_data->step_events_remaining; - } - - // Check if n_step exceeds decelerate point in block. Need to perform this so that the - // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should - // be OK since it is likely moving at a fast rate already. - if (st_prep_block->n_step > pl_prep_block->decelerate_after) { - st_prep_block->n_step = pl_prep_block->decelerate_after; - } - - float distance, exit_speed_sqr; - distance = st_prep_block->n_step*st_prep_data->mm_per_step; // Always greater than zero - if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) { - exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; - // Set ISR tick reset flag for deceleration ramp. - } else { // Acceleration or cruising ramp - if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) { - exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; - if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; } - } else { - exit_speed_sqr = pl_prep_block->nominal_speed_sqr; - } - } - - - // Adjust inverse time counter for ac/de-celerations - if (st.ramp_type) { - st.ramp_count--; // Tick acceleration ramp counter - if (st.ramp_count == 0) { // Adjust step rate when its time - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration - st.delta_d += st_current_data->rate_delta; - if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. - st.delta_d = st_current_data->nominal_rate; // Set cruising velocity - st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore - } - } else { // Adjust velocity for deceleration - if (st.delta_d > st_current_data->rate_delta) { - st.delta_d -= st_current_data->rate_delta; - } else { - - // Moving near zero feed rate. Gracefully slow down. - st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - - // Check for and handle feed hold exit? At this point, machine is stopped. - - } - } - // Finalize adjusted step rate. Ensure minimum. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } - else { st.d_per_tick = st.delta_d; } - } - } - // During feed hold, do not update rate or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { - st.delta_d = st_current_data->initial_rate; - if (st.delta_d == st_current_data->nominal_rate) { - ramp_type = RAMP_NOOP_CRUISE; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } - // Acceleration and cruise handled by ramping. Just check for deceleration. - if (st_current_segment->flag == ST_NOOP) { - if (st.ramp_type == RAMP_NOOP_CRUISE) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - st.ramp_type = RAMP_DECEL; - } - - - - - - // Update planner block variables. - pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr); -// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable. - pl_prep_block->millimeters -= distance; // Potential round-off error near end of block. - pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter. - - // Update stepper block variables. - st_prep_data->step_events_remaining -= st_prep_block->n_step; - if ( st_prep_data->step_events_remaining == 0 ) { - // Move planner pointer to next block - st_prep_block->flag = ST_END_OF_BLOCK; - pl_prep_index = next_block_index(pl_prep_index); - pl_prep_block = NULL; - } else { - st_prep_block->flag = ST_NOOP; - } - - // New step block completed. Increment step buffer indices. - segment_buffer_head = segment_next_head; - segment_next_head = next_block_index(segment_buffer_head); - - } -} From f7429ec79b621d1a4fb4486eac5bd03f6258589e Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Thu, 24 Oct 2013 22:12:13 -0600 Subject: [PATCH 11/13] Cleaned up stepper and planner code. - Added some compile-time error checking. Will add more in future pushes to ensure settings are correct and within parameters that won't break anything. - Pushed some master branch changes with MEGA pin settings. - Cleaned up planner code and comments to clarify some of the new changes. Still much to do here. - Cleaned up the new stepper code. May need to abstract some of the segment buffer more to fix the feed holds (and integrate homing into the main stepper routine). With what's planned, this should make the stepper algorithm easier to attach other types of processes to it, where it is now tightly integrated with the planner buffer and nothing else. --- README.md | 2 +- config.h | 24 +++-- pin_map.h | 65 ++++++------ planner.c | 300 +++++++++++++++++++++++++++--------------------------- planner.h | 2 + stepper.c | 112 +++++++++----------- 6 files changed, 252 insertions(+), 253 deletions(-) diff --git a/README.md b/README.md index 660a778..e035870 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ Grbl includes full acceleration management with look ahead. That means the contr ##Changelog for v0.9 from v0.8 - **ALPHA status: Under heavy development.** - - New stepper algorithm: Based on the Pramod Ranade inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. Overall, a much better, smoother stepper algorithm with the capability of very high speeds. + - New stepper algorithm: Based on an inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. Overall, a much better, smoother stepper algorithm with the capability of very high speeds. - Planner optimizations: Multiple changes to increase planner execution speed and removed redundant variables. - Acceleration independence: Each axes may be defined with different acceleration parameters and Grbl will automagically calculate the maximum acceleration through a path depending on the direction traveled. This is very useful for machine that have very different axes properties, like the ShapeOko z-axis. - Maximum velocity independence: As with acceleration, the maximum velocity of individual axes may be defined. All seek/rapids motions will move at these maximum rates, but never exceed any one axes. So, when two or more axes move, the limiting axis will move at its maximum rate, while the other axes are scaled down. diff --git a/config.h b/config.h index f8f0810..8277046 100644 --- a/config.h +++ b/config.h @@ -49,9 +49,9 @@ #define CMD_CYCLE_START '~' #define CMD_RESET 0x18 // ctrl-x -// 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. Recommended step frequencies are limited by the Ranade frequency by +// The "Stepper Driver Interrupt" employs an inverse time algorithm to manage the Bresenham line +// stepping algorithm. The value ISR_TICKS_PER_SECOND is the frequency(Hz) at which the inverse time +// algorithm ticks at. Recommended step frequencies are limited by the inverse time frequency by // approximately 0.75-0.9 * ISR_TICK_PER_SECOND. Meaning for 30kHz, the max step frequency is roughly // 22.5-27kHz, but 30kHz is still possible, just not optimal. An Arduino can safely complete a single // interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but @@ -71,7 +71,7 @@ // NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. #define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) -// The Ranade algorithm can use either floating point or long integers for its counters, but for +// The inverse time 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 @@ -82,10 +82,10 @@ #define INV_TIME_MULTIPLIER 10000000.0 // 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 +// in the stepper program and never runs slower than this value. If the INVE_TIME_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. -// NOTE: Compute by (desired_step_rate/60) * RANADE_MULTIPLIER/ISR_TICKS_PER_SECOND. (mm/min) +// INV_TIME_MULTIPLIER value, do the same to this value if you want to same response. +// NOTE: Compute by (desired_step_rate/60) * INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND. (mm/min) #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. @@ -158,7 +158,7 @@ // available RAM, like when re-compiling for a Mega or Sanguino. Or decrease if the Arduino // begins to crash due to the lack of available RAM or if the CPU is having trouble keeping // up with planning new incoming motions as they are executed. -// #define BLOCK_BUFFER_SIZE 17 // Uncomment to override default in planner.h. +// #define BLOCK_BUFFER_SIZE 18 // Uncomment to override default in planner.h. // Line buffer size from the serial input stream to be executed. Also, governs the size of // each of the startup blocks, as they are each stored as a string of this size. Make sure @@ -193,4 +193,12 @@ // TODO: Install compile-time option to send numeric status codes rather than strings. +// --------------------------------------------------------------------------------------- +// COMPILE-TIME ERROR CHECKING OF DEFINE VALUES: + +#if (ISR_TICKS_PER_ACCELERATION_TICK > 255) +#error Parameters ACCELERATION_TICKS / ISR_TICKS must be < 256 to prevent integer overflow. +#endif + +// --------------------------------------------------------------------------------------- #endif diff --git a/pin_map.h b/pin_map.h index 0167cc9..233b32a 100644 --- a/pin_map.h +++ b/pin_map.h @@ -97,41 +97,44 @@ #endif -#ifdef PIN_MAP_ARDUINO_MEGA_2560 // Unsupported. Doesn't work. Supplied by @elmom. +#ifdef PIN_MAP_ARDUINO_MEGA_2560 // Working @EliteEng // Serial port pins - #define SERIAL_RX USART0_RX_vect - #define SERIAL_UDRE USART0_UDRE_vect + #define SERIAL_RX USART0_RX_vect + #define SERIAL_UDRE USART0_UDRE_vect + + // Increase Buffers to make use of extra SRAM + #define RX_BUFFER_SIZE 256 + #define TX_BUFFER_SIZE 128 + #define BLOCK_BUFFER_SIZE 36 + #define LINE_BUFFER_SIZE 100 // NOTE: All step bit and direction pins must be on the same port. #define STEPPING_DDR DDRA #define STEPPING_PORT PORTA #define STEPPING_PIN PINA - #define X_STEP_BIT 0 // MEGA2560 Digital Pin 22 - #define Y_STEP_BIT 1 // MEGA2560 Digital Pin 23 - #define Z_STEP_BIT 2 // MEGA2560 Digital Pin 24 - // #define C_STEP_BIT 3 // MEGA2560 Digital Pin 25 - #define X_DIRECTION_BIT 4 // MEGA2560 Digital Pin 26 - #define Y_DIRECTION_BIT 5 // MEGA2560 Digital Pin 27 - #define Z_DIRECTION_BIT 6 // MEGA2560 Digital Pin 28 - // #define C_DIRECTION_BIT 7 // MEGA2560 Digital Pin 29 + #define X_STEP_BIT 2 // MEGA2560 Digital Pin 24 + #define Y_STEP_BIT 3 // MEGA2560 Digital Pin 25 + #define Z_STEP_BIT 4 // MEGA2560 Digital Pin 26 + #define X_DIRECTION_BIT 5 // MEGA2560 Digital Pin 27 + #define Y_DIRECTION_BIT 6 // MEGA2560 Digital Pin 28 + #define Z_DIRECTION_BIT 7 // MEGA2560 Digital Pin 29 #define STEP_MASK ((1<nominal_speed / \ @@ -119,55 +115,41 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr) +-------------+ time --> - Recalculates the motion plan according to the following algorithm: + Recalculates the motion plan according to the following basic guidelines: - 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. + 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds + (i.e. current->entry_speed) such that: + a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of + neighboring blocks. + b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) + with a maximum allowable deceleration over the block travel distance. + c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). 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. + a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable + acceleration over the block travel distance. - 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: + When these stages are complete, the planner will have maximized the velocity profiles throughout the all + of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In + other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements + are possible. If a new block is added to the buffer, the plan is recomputed according to the said + guidelines for a new optimal plan. - 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. + To increase computational efficiency of these guidelines, a set of planner block pointers have been + created to indicate stop-compute points for when the planner guidelines cannot logically make any further + changes or improvements to the plan when in normal operation and new blocks are streamed and added to the + planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are + bracketed by junction velocities at their maximums (or by the first planner block as well), no new block + added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute + them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute + point) are all accelerating, they are all optimal and can not be altered by a new block added to the + planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum + junction velocity is reached. However, if the operational conditions of the plan changes from infrequently + used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is + recomputed as stated in the general guidelines. - 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. - - Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's - constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and - nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile - through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting - conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile. - - Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line - segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled - in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the - buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be - able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. - The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_JUNCTION_SPEED. - Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost - steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the - bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory - is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering - up to a hundred or more. - - NOTE: Since this function is constantly re-calculating for every new incoming block, it 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 then starve and empty, leading - to weird hiccup-like jerky motions. - - Index mapping: - - block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The planner - never touches the exit speed of this block, which always defaults to MINIMUM_JUNCTION_SPEED. + Planner buffer index mapping: + - block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The + planner never touches the exit speed of this block, which always defaults to 0. - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible as long as the segment buffer is not empty. @@ -193,71 +175,47 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr) entry speed. !!! Need to check if this is the start of the non-optimal or the end of the optimal block. + + + NOTE: All planner computations are performed in floating point to minimize numerical round-off errors. + When a planner block is executed, the floating point values are converted to fast integers by the stepper + algorithm segment buffer. See the stepper module for details. + + NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short + line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't + enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then + decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and + becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner + will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line + segment(s) distance per block to a desired tolerance. The more combined distance the planner has to use, + the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance + for the planner to compute over. It also increases the number of computations the planner has to perform + to compute an optimal plan, so select carefully. The Arduino 328p memory is already maxed out, but future + ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more. + */ static void planner_recalculate() { + + // Initialize block index to the last block in the planner buffer. + uint8_t block_index = prev_block_index(block_buffer_head); + // Query stepper module for safe planner block index to recalculate to, which corresponds to the end // of the step segment buffer. uint8_t block_buffer_safe = st_get_prep_block_index(); + // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although, // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the // planner at all times. - - /* - In theory, the state of the segment buffer can exist anywhere within the planner buffer tail and head-1 - or is empty, when there is nothing in the segment queue. The safe pointer can be the buffer head only - when the planner queue has been entirely queued into the segment buffer and there are no more blocks - in the planner buffer. The segment buffer will to continue to execute the remainder of it, but the - planner should be able to treat a newly added block during this time as an empty planner buffer since - we can't touch the segment buffer. - - - The segment buffer is atomic to the planner buffer, because the main program computes these seperately. - Even if we move the planner head pointer early at the end of plan_buffer_line(), this shouldn't - effect the safe pointer. - - - If the safe pointer is at head-1, this means that the stepper algorithm has segments queued and may - be executing. This is the last block in the planner queue, so it has been planned to decelerate to - zero at its end. When adding a new block, there will be at least two blocks to work with. When resuming, - from a feed hold, we only have this block and will be computing nothing. The planner doesn't have to - do anything, since the trapezoid calculations called by the stepper module should complete the block plan. - - - In most cases, the safe pointer is at the plan tail or the block after, and rarely on the block two - beyond the tail. Since the safe pointer points to the block used at the end of the segment buffer, it - can be in any one of these states. As the stepper module executes the planner block, the buffer tail, - and hence the safe pointer, can push forward through the planner blocks and overcome the planned - pointer at any time. - - - Does the reverse pass not touch either the safe or the plan pointer blocks? The plan pointer only - allows the velocity profile within it to be altered, but not the entry speed, so the reverse pass - ignores this block. The safe pointer is the same way, where the entry speed does not change, but - the velocity profile within it does. - - - The planned pointer can exist anywhere in a given plan, except for the planner buffer head, if everything - operates as anticipated. Since the planner buffer can be executed by the stepper algorithm as any - rate and could empty the planner buffer quickly, the planner tail can overtake the planned pointer - at any time, but will never go around the ring buffer and re-encounter itself, the plan itself is not - changed by adding a new block or something else. - - - The planner recalculate function should always reset the planned pointer at the proper break points - or when it encounters the safe block pointer, but will only do so when there are more than one block - in the buffer. In the case of single blocks, the planned pointer should always be set to the first - write-able block in the buffer, aka safe block. - - - When does this not work? There might be an issue when the planned pointer moves from the tail to the - next head as a new block is being added and planned. Otherwise, the planned pointer should remain - static within the ring buffer no matter what the buffer is doing: being executed, adding new blocks, - or both simultaneously. Need to make sure that this case is covered. - */ - - + // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. - // NOTE: block_buffer_safe can be equal to block_buffer_head if the segment buffer has completely queued up - // the remainder of the planner buffer. In this case, a new planner block will be treated as a single block. - if (block_buffer_head == block_buffer_safe) { // Also catches head = tail + // NOTE: block_buffer_safe can be the last planner block if the segment buffer has completely queued up the + // remainder of the planner buffer. In this case, a new planner block will be treated as a single block. + if (block_index == block_buffer_safe) { // Also catches (head-1) = tail // Just set block_buffer_planned pointer. - block_buffer_planned = block_buffer_head; - printString("z"); + block_buffer_planned = block_index; // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For // a single added block and recalculate after a feed hold, we don't need to compute this, since we already @@ -271,7 +229,6 @@ static void planner_recalculate() // all junctions before proceeding. // Initialize planner buffer pointers and indexing. - uint8_t block_index = block_buffer_head; plan_block_t *current = &block_buffer[block_index]; // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. @@ -285,7 +242,7 @@ static void planner_recalculate() // will be recomputed within the plan. So, we need to update it if it is partially completed. float entry_speed_sqr; plan_block_t *next; - block_index = prev_block_index(block_index); + block_index = plan_prev_block_index(block_index); if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. @@ -294,7 +251,6 @@ static void planner_recalculate() // !!! Need to make the current entry speed calculation after this. plan_update_partial_block(block_index, 0.0); block_buffer_planned = block_index; -printString("y"); } else { @@ -306,7 +262,7 @@ printString("y"); // Increment block index early to check if the safe block is before the current block. If encountered, // this is an exit condition as we can't go further than this block in the reverse pass. - block_index = prev_block_index(block_index); + block_index = plan_prev_block_index(block_index); if (block_index == block_buffer_safe) { // Check if the safe block is partially completed. If so, update it before its exit speed // (=current->entry speed) is over-written. @@ -314,7 +270,7 @@ printString("y"); // the previous nominal speed to update this block with. There will need to be something along the // lines of a nominal speed change check and send the correct value to this function. plan_update_partial_block(block_index,current->entry_speed_sqr); -printString("x"); + // Set planned pointer at safe block and for loop exit after following computation is done. block_buffer_planned = block_index; } @@ -335,8 +291,8 @@ printString("x"); // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer - block_index = next_block_index(block_buffer_planned); - while (block_index != next_buffer_head) { + block_index = plan_next_block_index(block_buffer_planned); + while (block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; @@ -363,11 +319,63 @@ printString("x"); block_buffer_planned = block_index; // Set optimal plan pointer } - block_index = next_block_index( block_index ); + block_index = plan_next_block_index( block_index ); } } +/* + uint8_t block_buffer_safe = st_get_prep_block_index(); + if (block_buffer_head == block_buffer_safe) { // Also catches head = tail + block_buffer_planned = block_buffer_head; + } else { + uint8_t block_index = block_buffer_head; + plan_block_t *current = &block_buffer[block_index]; + current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); + float entry_speed_sqr; + plan_block_t *next; + block_index = plan_prev_block_index(block_index); + if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. + plan_update_partial_block(block_index, 0.0); + block_buffer_planned = block_index; + } else { + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + block_index = plan_prev_block_index(block_index); + if (block_index == block_buffer_safe) { + plan_update_partial_block(block_index,current->entry_speed_sqr); + block_buffer_planned = block_index; + } + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; + } + } + } + + } + next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer + block_index = plan_next_block_index(block_buffer_planned); + while (block_index != next_buffer_head) { + current = next; + next = &block_buffer[block_index]; + if (current->entry_speed_sqr < next->entry_speed_sqr) { + entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + block_buffer_planned = block_index; // Set optimal plan pointer. + } + } + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; // Set optimal plan pointer + } + block_index = plan_next_block_index( block_index ); + } + } */ } @@ -376,11 +384,12 @@ void plan_reset_buffer() block_buffer_planned = block_buffer_tail; } + void plan_init() { block_buffer_tail = 0; block_buffer_head = 0; // Empty = tail - next_buffer_head = 1; // next_block_index(block_buffer_head) + next_buffer_head = 1; // plan_next_block_index(block_buffer_head) plan_reset_buffer(); memset(&pl, 0, sizeof(pl)); // Clear planner struct } @@ -389,7 +398,7 @@ void plan_init() void plan_discard_current_block() { if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. - block_buffer_tail = next_block_index( block_buffer_tail ); + block_buffer_tail = plan_next_block_index( block_buffer_tail ); } } @@ -484,10 +493,8 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } - // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited - // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so - // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction - // and axes properties as well. + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled + // down such that no individual axes maximum values are exceeded with respect to the line direction. // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. float inverse_unit_vec_value; @@ -514,10 +521,6 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) if (block_buffer_head == block_buffer_tail) { // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. - // !!! Ensures when the first block starts from zero speed. If we do this in the planner, this will break - // feedrate overrides later, as you can override this single block and it maybe moving already at a given rate. - // Better to do it here and make it clean. - // !!! Shouldn't need this for anything other than a single block. block->entry_speed_sqr = 0.0; block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. @@ -541,10 +544,9 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) a continuous mode path, but ARM-based microcontrollers most certainly do. NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be - changed dynamically during operation nor can the line segment geometry. This must be kept in + changed dynamically during operation nor can the line move geometry. This must be kept in memory in the event of a feedrate override changing the nominal speeds of blocks, which can change the overall maximum entry speed conditions of all blocks. - */ // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. @@ -558,7 +560,6 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. - // TODO: Should call a function to determine this. The function can be used elsewhere for feedrate overrides later. block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); @@ -569,17 +570,16 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) // Update planner position memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] - planner_recalculate(); - - // Update buffer head and next buffer head indices. Advance only after new plan has been computed. + // New block is all set. Update buffer head and next buffer head indices. block_buffer_head = next_buffer_head; - next_buffer_head = next_block_index(block_buffer_head); + next_buffer_head = plan_next_block_index(block_buffer_head); + // Finish up by recalculating the plan with the new block. + planner_recalculate(); - -int32_t blength = block_buffer_head - block_buffer_tail; -if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } -printInteger(blength); +// int32_t blength = block_buffer_head - block_buffer_tail; +// if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } +// printInteger(blength); } @@ -595,36 +595,34 @@ void plan_sync_position() } - - /* STEPPER VELOCITY PROFILE DEFINITION - less than nominal rate-> + - +--------+ <- nominal_rate /|\ + less than nominal speed-> + + +--------+ <- nominal_speed /|\ / \ / | \ - initial_rate -> + \ / | + <- next->initial_rate - | + <- next->initial_rate / | | - +-------------+ initial_rate -> +----+--+ + entry_speed -> + \ / | + <- next->entry_speed + | + <- next->entry_speed / | | + +-------------+ entry_speed -> +----+--+ time --> ^ ^ ^ ^ | | | | decelerate distance decelerate distance - Calculates the "trapezoid" velocity profile parameters of a planner block for the stepper - algorithm. The planner computes the entry and exit speeds of each block, but does not bother to - determine the details of the velocity profiles within them, as they aren't needed for computing - an optimal plan. When the stepper algorithm begins to execute a block, the block velocity profiles - are computed ad hoc. + Calculates the type of velocity profile for a given planner block and provides the deceleration + distance for the stepper algorithm to use to accurately trace the profile exactly. The planner + computes the entry and exit speeds of each block, but does not bother to determine the details of + the velocity profiles within them, as they aren't needed for computing an optimal plan. When the + stepper algorithm begins to execute a block, the block velocity profiles are computed ad hoc. 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. Both of these velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, - as they can be influenced by the conditions of neighboring blocks. + as they can be influenced by the conditions of neighboring blocks, where the acceleration ramps + are defined by constant acceleration equal to the maximum allowable acceleration of a block. - The following function determines the type of velocity profile and stores the minimum required - information for the stepper algorithm to execute the calculated profiles. Since the stepper - algorithm always assumes to begin accelerating from the initial_rate and cruise if the nominal_rate - is reached, we only need to know when to begin deceleration to the end of the block. Hence, only - the distance from the end of the block to begin a deceleration ramp are computed. + Since the stepper algorithm already assumes to begin executing a planner block by accelerating + from the planner entry speed and cruise if the nominal speed is reached, we only need to know + when to begin deceleration to the end of the block. Hence, only the distance from the end of the + block to begin a deceleration ramp is computed for the stepper algorithm when requested. */ float plan_calculate_velocity_profile(uint8_t block_index) { @@ -632,7 +630,7 @@ float plan_calculate_velocity_profile(uint8_t block_index) // Determine current block exit speed float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed. - plan_block_t *next_block = plan_get_block_by_index(next_block_index(block_index)); + plan_block_t *next_block = plan_get_block_by_index(plan_next_block_index(block_index)); if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block // First determine intersection distance (in steps) from the exit point for a triangular profile. diff --git a/planner.h b/planner.h index 083d182..a52a4f5 100644 --- a/planner.h +++ b/planner.h @@ -64,6 +64,8 @@ void plan_discard_current_block(); // Gets the current block. Returns NULL if buffer empty plan_block_t *plan_get_current_block(); +uint8_t plan_next_block_index(uint8_t block_index); + plan_block_t *plan_get_block_by_index(uint8_t block_index); float plan_calculate_velocity_profile(uint8_t block_index); diff --git a/stepper.c b/stepper.c index 27cf7d6..de93000 100644 --- a/stepper.c +++ b/stepper.c @@ -115,23 +115,6 @@ static uint8_t st_data_prep_index; // Index of stepper common data block bein static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block -// Returns the index of the next block in the ring buffer -// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. -static uint8_t next_block_index(uint8_t block_index) -{ - block_index++; - if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; } - return(block_index); -} - -static uint8_t next_block_pl_index(uint8_t block_index) -{ - block_index++; - if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } - return(block_index); -} - - /* __________________________ /| |\ _________________ ^ / | | \ /| |\ | @@ -334,9 +317,7 @@ ISR(TIMER2_COMPA_vect) st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter if (st.delta_d > st_current_data->rate_delta) { st.delta_d -= st_current_data->rate_delta; - } else { - - // Moving near zero feed rate. Gracefully slow down. + } else { // Moving near zero feed rate. Gracefully slow down. st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. // TODO: Check for and handle feed hold exit? At this point, machine is stopped. @@ -392,16 +373,16 @@ ISR(TIMER2_COMPA_vect) // Check step events for trapezoid change or end of block. st.segment_steps_remaining--; // Decrement step events count if (st.segment_steps_remaining == 0) { - - // NOTE: sys.position updates could be done here. The bresenham counters can have - // their own fast 8-bit addition-only counters. Here we would check the direction and - // apply it to sys.position accordingly. However, this could take too much time - // combined with loading a new segment during next cycle too. - // TODO: Measure the time it would take in the worst case. It could still be faster - // overall during segment execution if uint8 step counters tracked this and was added - // to the system position variables here. Compared to worst case now, it wouldn't be - // that much different. /* + NOTE: sys.position updates could be done here. The bresenham counters can have + their own fast 8-bit addition-only counters. Here we would check the direction and + apply it to sys.position accordingly. However, this could take too much time + combined with loading a new segment during next cycle too. + TODO: Measure the time it would take in the worst case. It could still be faster + overall during segment execution if uint8 step counters tracked this and was added + to the system position variables here. Compared to worst case now, it wouldn't be + that much different. + // TODO: Upon loading, step counters would need to be zeroed. // TODO: For feedrate overrides, we will have to execute add these values.. although // for probing, this breaks. Current values won't be correct, unless we query it. @@ -429,8 +410,8 @@ ISR(TIMER2_COMPA_vect) st.load_flag = LOAD_SEGMENT; } - // Discard current segment - segment_buffer_tail = next_block_index( segment_buffer_tail ); + // Discard current segment by advancing buffer tail index + if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } } @@ -552,28 +533,32 @@ void st_cycle_reinitialize() /* Prepares step segment buffer. Continuously called from main program. - NOTE: There doesn't seem to be a great way to figure out how many steps occur within - a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a - critical problem. So, either numerical round-off checks could be made to account for - them, while CPU overhead could be minimized in some way, or we can flip the algorithm - around to have the stepper algorithm track number of steps over an indeterminant amount - of time instead. - In other words, we use the planner velocity floating point data to get an estimate of - the number of steps we want to execute. We then back out the approximate velocity for - the planner to use, which should be much more robust to round-off error. The main problem - now is that we are loading the stepper algorithm to handle acceleration now, rather than - pre-calculating with the main program. This approach does make sense in the way that - planner velocities and stepper profiles can be traced more accurately. - Which is better? Very hard to tell. The time-based algorithm would be able to handle - Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would - require some additional math in the stepper algorithm to adjust on the fly, plus adaptation - would occur in a non-deterministic manner. - I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. + The segment buffer is an intermediary buffer interface between the execution of steps + by the stepper algorithm and the velocity profiles generated by the planner. The stepper + algorithm only executes steps within the segment buffer and is filled by the main program + when steps are "checked-out" from the first block in the planner buffer. This keeps the + step execution and planning optimization processes atomic and protected from each other. + The number of steps "checked-out" from the planner buffer and the number of segments in + the segment buffer is sized and computed such that no operation in the main program takes + longer than the time it takes the stepper algorithm to empty it before refilling it. + Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. - TODO: Need to describe the importance of continuations of step pulses between ramp states - and planner blocks. This has to do with Alden's problem with step "phase". The things I've - been doing here limit this phase issue by truncating some of the ramp timing for certain - events like deceleration initialization and end of block. + NOTE: The segment buffer executes a set number of steps over an approximate time period. + If we try to execute over a set time period, it is difficult to guarantee or predict how + many steps will execute over it, especially when the step pulse phasing between the + neighboring segments are kept consistent. Meaning that, if the last segment step pulses + right before its end, the next segment must delay its first pulse so that the step pulses + are consistently spaced apart over time to keep the step pulse train nice and smooth. + Keeping track of phasing and ensuring that the exact number of steps are executed as + defined by the planner block, the related computational overhead gets quickly and + prohibitively expensive, especially in real-time. + Since the stepper algorithm automatically takes care of the step pulse phasing with + its ramp and inverse time counters, we don't have to explicitly and expensively track the + exact number of steps, time, or phasing of steps. All we need to do is approximate + the number of steps in each segment such that the segment buffer has enough execution time + for the main program to do what it needs to do and refill it when it has time. In other + words, we just need to compute a cheap approximation of the current velocity and the + number of steps over it. */ /* @@ -606,7 +591,7 @@ void st_prep_buffer() pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - // Increment stepper common data buffer index + // Increment stepper common data index if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } // Check if the planner has re-computed this block mid-execution. If so, push the previous segment @@ -666,7 +651,10 @@ void st_prep_buffer() // Set new segment to point to the current segment data block. prep_segment->st_data_index = st_data_prep_index; - // Approximate the velocity over the new segment + // Approximate the velocity over the new segment using the already computed rate values. + // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. + // We do this to minimize memory and computational requirements. However, this could easily be replaced with + // a more exact approximation or have a unique time per segment, if CPU and memory overhead allows. if (st_prep_data->decelerate_after <= 0) { if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } @@ -680,15 +668,15 @@ void st_prep_buffer() } } - // Compute the number of steps in the prepped segment based on the approximate current rate. The execution - // time of each segment should be about every ACCELERATION_TICK. + // Compute the number of steps in the prepped segment based on the approximate current rate. // NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. - // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255. prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next); - prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); // Ensure it moves for very slow motions? + // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps + // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. + prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); + // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow. // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. - // Check if n_step exceeds steps remaining in planner block. If so, truncate. if (prep_segment->n_step > st_prep_data->step_events_remaining) { @@ -703,7 +691,7 @@ void st_prep_buffer() } } - // Update stepper block variables. + // Update stepper common data variables. st_prep_data->decelerate_after -= prep_segment->n_step; st_prep_data->step_events_remaining -= prep_segment->n_step; @@ -712,13 +700,13 @@ void st_prep_buffer() // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. prep_segment->flag |= SEGMENT_END_OF_BLOCK; // Move planner pointer to next block and flag to load a new block for the next segment. - pl_prep_index = next_block_pl_index(pl_prep_index); + pl_prep_index = plan_next_block_index(pl_prep_index); pl_prep_block = NULL; } // New step segment completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; - segment_next_head = next_block_index(segment_buffer_head); + if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } // long a = prep_segment->n_step; // printInteger(a); From 27297d444ba8480ee27327c6644c48485601e57d Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Tue, 29 Oct 2013 08:31:48 -0600 Subject: [PATCH 12/13] Updated comments. Changed stepper variable names to be more understandable. Added step locking note. - Updated config comments and stepper code comments for the new changes. - Changed stepper algorithm variable names to be more understandable in what they actually do. - Added a stepper lock note in default.h per user request. - Started some code layout in handling feed holds and refactoring the homing routine to use the main stepper algorithm instead of a seperate version. --- config.h | 13 +- defaults.h | 8 +- limits.c | 19 +- protocol.c | 2 + stepper.c | 164 +++++------- stepper_old.c | 691 +++++++++++++++++++++++++++++++++++++------------ stepper_v0_9.c | 387 +++++++++++++++++++++++++++ 7 files changed, 998 insertions(+), 286 deletions(-) create mode 100644 stepper_v0_9.c diff --git a/config.h b/config.h index 8277046..9aa82f0 100644 --- a/config.h +++ b/config.h @@ -71,12 +71,13 @@ // NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. #define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) -// The inverse time 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 +// The inverse time algorithm can use either floating point or long integers for its counters (usually +// very small values ~10^-6), but with integers, the counter values must be scaled to be greater than +// one. This multiplier value scales the floating point counter values for use in a long integer, which +// are significantly faster to compute with a slightly higher precision ceiling than floats. 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 INV_TIME_MULTIPLIER 10000000.0 diff --git a/defaults.h b/defaults.h index d3ddc1c..7ee6606 100644 --- a/defaults.h +++ b/defaults.h @@ -50,7 +50,7 @@ #define DEFAULT_HOMING_FEEDRATE 25.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255) + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled) #define DEFAULT_DECIMAL_PLACES 3 #define DEFAULT_X_MAX_TRAVEL 200 // mm #define DEFAULT_Y_MAX_TRAVEL 200 // mm @@ -84,7 +84,7 @@ #define DEFAULT_HOMING_FEEDRATE 25.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255) + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled) #define DEFAULT_DECIMAL_PLACES 3 #define DEFAULT_X_MAX_TRAVEL 200 // mm #define DEFAULT_Y_MAX_TRAVEL 200 // mm @@ -121,7 +121,7 @@ #define DEFAULT_HOMING_FEEDRATE 25.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-255) + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-254, 255 keeps steppers enabled) #define DEFAULT_DECIMAL_PLACES 3 #define DEFAULT_X_MAX_TRAVEL 200 // mm #define DEFAULT_Y_MAX_TRAVEL 200 // mm @@ -156,7 +156,7 @@ #define DEFAULT_HOMING_FEEDRATE 50.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255) + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled) #define DEFAULT_DECIMAL_PLACES 3 #define DEFAULT_X_MAX_TRAVEL 200 // mm #define DEFAULT_Y_MAX_TRAVEL 200 // mm diff --git a/limits.c b/limits.c index a03d32e..4fde4c9 100644 --- a/limits.c +++ b/limits.c @@ -59,12 +59,6 @@ void limits_init() // your e-stop switch to the Arduino reset pin, since it is the most correct way to do this. ISR(LIMIT_INT_vect) { - // TODO: This interrupt may be used to manage the homing cycle directly with the main stepper - // interrupt without adding too much to it. All it would need is some way to stop one axis - // when its limit is triggered and continue the others. This may reduce some of the code, but - // would make Grbl a little harder to read and understand down road. Holding off on this until - // we move on to new hardware or flash space becomes an issue. If it ain't broke, don't fix it. - // Ignore limit switches if already in an alarm state or in-process of executing an alarm. // When in the alarm state, Grbl should have been reset or will force a reset, so any pending // moves in the planner and serial buffers are all cleared and newly sent blocks will be @@ -89,6 +83,19 @@ ISR(LIMIT_INT_vect) // NOTE: Only the abort runtime command can interrupt this process. static void homing_cycle(uint8_t cycle_mask, int8_t pos_dir, bool invert_pin, float homing_rate) { + + /* TODO: Change homing routine to call planner instead moving at the maximum seek rates + and (max_travel+10mm?) for each axes during the search phase. The routine should monitor + the state of the limit pins and when a pin is triggered, it can disable that axes by + setting the respective step_x, step_y, or step_z value in the executing planner block. + This keeps the stepper algorithm counters from triggering the step on that particular + axis. When all axes have been triggered, we can then disable the steppers and reset + the stepper and planner buffers. This same method can be used for the locate cycles. + This will also fix the slow max feedrate of the homing 'lite' stepper algorithm. + + Need to check if setting the planner steps will require them to be volatile or not. */ + + // Determine governing axes with finest step resolution per distance for the Bresenham // algorithm. This solves the issue when homing multiple axes that have different // resolutions without exceeding system acceleration setting. It doesn't have to be diff --git a/protocol.c b/protocol.c index 253ec1f..5466720 100644 --- a/protocol.c +++ b/protocol.c @@ -104,7 +104,9 @@ ISR(PINOUT_INT_vect) // limit switches, or the main program. void protocol_execute_runtime() { + // Reload step segment buffer st_prep_buffer(); + if (sys.execute) { // Enter only if any bit flag is true uint8_t rt_exec = sys.execute; // Avoid calling volatile multiple times diff --git a/stepper.c b/stepper.c index de93000..22f1edd 100644 --- a/stepper.c +++ b/stepper.c @@ -55,9 +55,9 @@ typedef struct { uint8_t segment_steps_remaining; // Steps remaining in line segment motion // Used by inverse time algorithm to track step rate - int32_t counter_d; // Inverse time distance traveled since last step event - uint32_t delta_d; // Inverse time distance traveled per interrupt tick - uint32_t d_per_tick; + int32_t counter_dist; // Inverse time distance traveled since last step event + uint32_t ramp_rate; // Inverse time distance traveled per interrupt tick + uint32_t dist_per_tick; // Used by the stepper driver interrupt uint8_t execute_step; // Flags step execution for each interrupt. @@ -65,7 +65,7 @@ typedef struct { uint8_t out_bits; // The next stepping-bits to be output uint8_t load_flag; - uint8_t ramp_count; + uint8_t counter_ramp; uint8_t ramp_type; } stepper_t; static stepper_t st; @@ -76,7 +76,7 @@ static stepper_t st; // the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). typedef struct { int32_t step_events_remaining; // Tracks step event count for the executing planner block - uint32_t d_next; // Scaled distance to next step + uint32_t dist_next_step; // Scaled distance to next step uint32_t initial_rate; // Initialized step rate at re/start of a planner block uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) @@ -154,7 +154,6 @@ void st_wake_up() TCNT2 = 0; // Clear Timer2 TIMSK2 |= (1<n_step; - // Check if the counters need to be reset for a new planner block + // If the new segment starts a new planner block, initialize stepper variables and counters. + // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous. if (st.load_flag == LOAD_BLOCK) { pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. - st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index]; + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; - // Initialize direction bits for block + // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits upon next ISR tick. + st.execute_step = true; // Initialize Bresenham line counters st.counter_x = (pl_current_block->step_event_count >> 1); st.counter_y = st.counter_x; st.counter_z = st.counter_x; - // Initialize inverse time and step rate counter data - st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } - else { st.d_per_tick = st.delta_d; } - - // During feed hold, do not update rate, ramp type, or ramp counters. Keep decelerating. -// if (sys.state == STATE_CYCLE) { - st.delta_d = st_current_data->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule - st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. -// } - + // Initialize inverse time, step rate data, and acceleration ramp counters + st.counter_dist = st_current_data->dist_next_step; // dist_next_step always greater than ramp_rate. + st.ramp_rate = st_current_data->initial_rate; + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule + st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. + + // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. + if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } + else { st.dist_per_tick = st.ramp_rate; } } - // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. + // Check if ramp conditions have changed. If so, update ramp counters and control variables. if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) { /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 - as mandated by the mid-point rule. For these conditions, the ramp count have been initialized - such that the following computation is still correct. */ - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; + as mandated by the mid-point rule. For the latter conditions, the ramp count have been + initialized such that the following computation is still correct. */ + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp; if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; } else { st.ramp_type = RAMP_ACCEL; } } @@ -300,46 +290,36 @@ ISR(TIMER2_COMPA_vect) } // Adjust inverse time counter for ac/de-celerations - // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally - // efficient on the Arduino AVR. This could may not be true with higher ISR frequencies or faster CPUs. - if (st.ramp_type) { // Ignored when ramp type is NOOP_CRUISE - st.ramp_count--; // Tick acceleration ramp counter - if (st.ramp_count == 0) { // Adjust step rate when its time + if (st.ramp_type) { // Ignored when ramp type is RAMP_NOOP_CRUISE + st.counter_ramp--; // Tick acceleration ramp counter + if (st.counter_ramp == 0) { // Adjust step rate when its time + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - st.delta_d += st_current_data->rate_delta; - if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. - st.delta_d = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_rate += st_current_data->rate_delta; + if (st.ramp_rate >= st_current_data->nominal_rate) { // Reached nominal rate. + st.ramp_rate = st_current_data->nominal_rate; // Set cruising velocity st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp. + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp change. } } else { // Adjust velocity for deceleration. - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.delta_d > st_current_data->rate_delta) { - st.delta_d -= st_current_data->rate_delta; + if (st.ramp_rate > st_current_data->rate_delta) { + st.ramp_rate -= st_current_data->rate_delta; } else { // Moving near zero feed rate. Gracefully slow down. - st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - - // TODO: Check for and handle feed hold exit? At this point, machine is stopped. - // - Set system flag to recompute plan and reset segment buffer. - // - Segment steps in buffer needs to be returned to planner correctly. - // busy = false; - // return; - + st.ramp_rate >>= 1; // Integer divide by 2 until complete. Also prevents overflow. } } - // Finalize adjusted step rate. Ensure minimum. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } - else { st.d_per_tick = st.delta_d; } + // Adjust for minimum step rate, but retain operating ramp rate for accurate velocity tracing. + if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } + else { st.dist_per_tick = st.ramp_rate; } } } // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_d -= st.d_per_tick; + st.counter_dist -= st.dist_per_tick; // Execute Bresenham step event, when it's time to do so. - if (st.counter_d < 0) { - st.counter_d += st_current_data->d_next; // Reload inverse time counter + if (st.counter_dist < 0) { + st.counter_dist += st_current_data->dist_next_step; // Reload inverse time counter st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; @@ -349,7 +329,6 @@ ISR(TIMER2_COMPA_vect) if (st.counter_x < 0) { st.out_bits |= (1<step_event_count; - // st.steps_x++; if (st.out_bits & (1<step_event_count; - // st.steps_y++; if (st.out_bits & (1<step_event_count; - // st.steps_z++; if (st.out_bits & (1< 0) { - if (st.out_bits & (1< 0) { - if (st.out_bits & (1< 0) { - if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { @@ -412,7 +361,6 @@ ISR(TIMER2_COMPA_vect) // Discard current segment by advancing buffer tail index if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } - } st.out_bits ^= settings.invert_mask; // Apply step port invert mask @@ -486,6 +434,7 @@ void st_cycle_start() { if (sys.state == STATE_QUEUED) { sys.state = STATE_CYCLE; + st_prep_buffer(); // Initialize step segment buffer before beginning cycle. st_wake_up(); } } @@ -520,8 +469,8 @@ void st_cycle_reinitialize() // plan_cycle_reinitialize(st_current_data->step_events_remaining); // st.ramp_type = RAMP_ACCEL; -// st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; -// st.delta_d = 0; +// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.ramp_rate = 0; // sys.state = STATE_QUEUED; // } else { // sys.state = STATE_IDLE; @@ -580,6 +529,7 @@ void st_cycle_reinitialize() */ void st_prep_buffer() { + if (sys.state != STATE_QUEUED) { // Block until a motion state is issued while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. // Initialize new segment @@ -605,7 +555,7 @@ void st_prep_buffer() st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; st_prep_data->rate_delta = last_st_prep_data->rate_delta; - st_prep_data->d_next = last_st_prep_data->d_next; + st_prep_data->dist_next_step = last_st_prep_data->dist_next_step; st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; @@ -625,7 +575,7 @@ void st_prep_buffer() st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + st_prep_data->dist_next_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) // TODO: Check if we really need to store this. st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; @@ -656,7 +606,7 @@ void st_prep_buffer() // We do this to minimize memory and computational requirements. However, this could easily be replaced with // a more exact approximation or have a unique time per segment, if CPU and memory overhead allows. if (st_prep_data->decelerate_after <= 0) { - if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } + if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } // Set segment deceleration flag else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } } else { @@ -669,9 +619,9 @@ void st_prep_buffer() } // Compute the number of steps in the prepped segment based on the approximate current rate. - // NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. + // NOTE: The dist_next_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* - (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next); + (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_next_step); // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); @@ -697,6 +647,11 @@ void st_prep_buffer() // Check for end of planner block if ( st_prep_data->step_events_remaining == 0 ) { + + // TODO: When a feed hold ends, the step_events_remaining will also be zero, even though a block + // have partially been completed. We need to flag the stepper algorithm to indicate a stepper shutdown + // when complete, but not remove the planner block unless it truly is the end of the block (rare). + // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. prep_segment->flag |= SEGMENT_END_OF_BLOCK; // Move planner pointer to next block and flag to load a new block for the next segment. @@ -713,6 +668,7 @@ void st_prep_buffer() // printString(" "); } + } } uint8_t st_get_prep_block_index() diff --git a/stepper_old.c b/stepper_old.c index 3191007..d702eba 100644 --- a/stepper_old.c +++ b/stepper_old.c @@ -19,20 +19,32 @@ along with Grbl. If not, see . */ -/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith - and Philipp Tiefenbacher. */ - #include #include "stepper.h" #include "config.h" #include "settings.h" #include "planner.h" +#include "nuts_bolts.h" // Some useful constants #define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define CRUISE_RAMP 0 -#define ACCEL_RAMP 1 -#define DECEL_RAMP 2 + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_SEGMENT 1 +#define LOAD_BLOCK 2 + +#define SEGMENT_NOOP 0 +#define SEGMENT_END_OF_BLOCK bit(0) +#define SEGMENT_ACCEL bit(1) +#define SEGMENT_DECEL bit(2) + +#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change + +#define SEGMENT_BUFFER_SIZE 6 // Stepper state variable. Contains running data and trapezoid variables. typedef struct { @@ -40,44 +52,85 @@ typedef struct { int32_t counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z; - int32_t event_count; // Total event count. Retained for feed holds. - int32_t step_events_remaining; // Steps remaining in motion + uint8_t segment_steps_remaining; // Steps remaining in line segment motion - // Used by Pramod Ranade inverse time algorithm - int32_t delta_d; // Ranade distance traveled per interrupt tick - int32_t d_counter; // Ranade distance traveled since last step event - uint8_t ramp_count; // Acceleration interrupt tick counter. - uint8_t ramp_type; // Ramp type variable. - uint8_t execute_step; // Flags step execution for each interrupt. + // Used by inverse time algorithm to track step rate + int32_t counter_d; // Inverse time distance traveled since last step event + uint32_t delta_d; // Inverse time distance traveled per interrupt tick + uint32_t d_per_tick; + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; + + uint8_t ramp_count; + uint8_t ramp_type; } stepper_t; static stepper_t st; -static block_t *current_block; // A pointer to the block currently being traced -// Used by the stepper driver interrupt -static uint8_t step_pulse_time; // Step pulse reset time after step rise -static uint8_t out_bits; // The next stepping-bits to be output +// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the +// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed +// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). +typedef struct { + int32_t step_events_remaining; // Tracks step event count for the executing planner block + uint32_t d_next; // Scaled distance to next step + uint32_t initial_rate; // Initialized step rate at re/start of a planner block + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + uint32_t current_approx_rate; // Tracks the approximate segment rate to predict steps per segment to execute + int32_t decelerate_after; // Tracks when to initiate deceleration according to the planner block + float mm_per_step; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; -// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then -// this blocking variable is no longer needed. Only here for safety reasons. -static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, +// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps +// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. +typedef struct { + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. + uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; -// __________________________ -// /| |\ _________________ ^ -// / | | \ /| |\ | -// / | | \ / | | \ s -// / | | | | | \ p -// / | | | | | \ e -// +-----+------------------------+---+--+---------------+----+ e -// | BLOCK 1 | BLOCK 2 | d -// -// time -----> -// -// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta -// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after -// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as -// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +// Step segment ring buffer indices +static volatile uint8_t segment_buffer_tail; +static volatile uint8_t segment_buffer_head; +static uint8_t segment_next_head; +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. +static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped +static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped +static uint8_t pl_prep_index; // Index of planner block being prepped +static uint8_t st_data_prep_index; // Index of stepper common data block being prepped +static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ // Stepper state initialization. Cycle should only start if the st.cycle_start flag is // enabled. Startup init and limits call this function but shouldn't start the cycle. @@ -91,14 +144,17 @@ void st_wake_up() } if (sys.state == STATE_CYCLE) { // Initialize stepper output bits - out_bits = settings.invert_mask; + st.out_bits = settings.invert_mask; // Initialize step pulse timing from settings. - step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); + st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Enable stepper driver interrupt st.execute_step = false; + st.load_flag = LOAD_BLOCK; + TCNT2 = 0; // Clear Timer2 TIMSK2 |= (1<direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits. + // NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is + // a small chance that this will load at the same time as a step event. Hopefully, + // the overhead for this loading event isn't too much.. possibly 2-5 usec. + + // NOTE: The stepper algorithm must control the planner buffer tail as it completes + // the block moves. Otherwise, a feed hold can leave a few step buffer line moves + // without the correct planner block information. - // Initialize Bresenham variables - st.counter_x = (current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - st.event_count = current_block->step_event_count; - st.step_events_remaining = st.event_count; + st_current_segment = &segment_buffer[segment_buffer_tail]; - // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { - // Initialize Ranade variables - st.d_counter = current_block->d_next; - st.delta_d = current_block->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + // Load number of steps to execute from stepper buffer + st.segment_steps_remaining = st_current_segment->n_step; + + // Check if the counters need to be reset for a new planner block + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index]; - // Initialize ramp type. - if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } - else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } - else { st.ramp_type = ACCEL_RAMP; } + // Initialize direction bits for block + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits upon next ISR tick. + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time and step rate counter data + st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } + + // During feed hold, do not update rate, ramp type, or ramp counters. Keep decelerating. +// if (sys.state == STATE_CYCLE) { + st.delta_d = st_current_data->initial_rate; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule + st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. +// } + + } + + // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. + if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) { + /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, + or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to + the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the + ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known + rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 + as mandated by the mid-point rule. For these conditions, the ramp count have been initialized + such that the following computation is still correct. */ + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; + if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; } + else { st.ramp_type = RAMP_ACCEL; } } + st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. + } else { + // Can't discard planner block here if a feed hold stops in middle of block. st_go_idle(); bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end return; // Nothing to do but exit. } + } // Adjust inverse time counter for ac/de-celerations - if (st.ramp_type) { - // Tick acceleration ramp counter - st.ramp_count--; - if (st.ramp_count == 0) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration - st.delta_d += current_block->rate_delta; - if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. - st.ramp_type = CRUISE_RAMP; - st.delta_d = current_block->nominal_rate; // Set cruise velocity + // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally + // efficient on the Arduino AVR. This could may not be true with higher ISR frequencies or faster CPUs. + if (st.ramp_type) { // Ignored when ramp type is NOOP_CRUISE + st.ramp_count--; // Tick acceleration ramp counter + if (st.ramp_count == 0) { // Adjust step rate when its time + if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + st.delta_d += st_current_data->rate_delta; + if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. + st.delta_d = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp. } - } 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 { + } else { // Adjust velocity for deceleration. + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.delta_d > st_current_data->rate_delta) { + st.delta_d -= st_current_data->rate_delta; + } else { // Moving near zero feed rate. Gracefully slow down. st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + + // TODO: Check for and handle feed hold exit? At this point, machine is stopped. + // - Set system flag to recompute plan and reset segment buffer. + // - Segment steps in buffer needs to be returned to planner correctly. + // busy = false; + // return; + } } + // Finalize adjusted step rate. Ensure minimum. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } } } - - // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; } - else { st.d_counter -= st.delta_d; } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.d_per_tick; // Execute Bresenham step event, when it's time to do so. - if (st.d_counter < 0) { - st.d_counter += current_block->d_next; - - // Check for feed hold state and execute accordingly. - if (sys.state == STATE_HOLD) { - if (st.ramp_type != DECEL_RAMP) { - st.ramp_type = DECEL_RAMP; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - } - if (st.delta_d <= current_block->rate_delta) { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); - return; - } - } - - // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). - - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + if (st.counter_d < 0) { + st.counter_d += st_current_data->d_next; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= current_block->steps[X_AXIS]; + st.counter_x -= pl_current_block->steps[X_AXIS]; if (st.counter_x < 0) { - out_bits |= (1<step_event_count; + // st.steps_x++; + if (st.out_bits & (1<steps[Y_AXIS]; + st.counter_y -= pl_current_block->steps[Y_AXIS]; if (st.counter_y < 0) { - out_bits |= (1<step_event_count; + // st.steps_y++; + if (st.out_bits & (1<steps[Z_AXIS]; + st.counter_z -= pl_current_block->steps[Z_AXIS]; if (st.counter_z < 0) { - out_bits |= (1<step_event_count; + // st.steps_z++; + if (st.out_bits & (1<decelerate_after) { - st.ramp_type = DECEL_RAMP; - if (st.step_events_remaining == current_block->decelerate_after) { - if (st.delta_d == current_block->nominal_rate) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - } - } + st.segment_steps_remaining--; // Decrement step events count + if (st.segment_steps_remaining == 0) { + /* + NOTE: sys.position updates could be done here. The bresenham counters can have + their own fast 8-bit addition-only counters. Here we would check the direction and + apply it to sys.position accordingly. However, this could take too much time + combined with loading a new segment during next cycle too. + TODO: Measure the time it would take in the worst case. It could still be faster + overall during segment execution if uint8 step counters tracked this and was added + to the system position variables here. Compared to worst case now, it wouldn't be + that much different. + + // TODO: Upon loading, step counters would need to be zeroed. + // TODO: For feedrate overrides, we will have to execute add these values.. although + // for probing, this breaks. Current values won't be correct, unless we query it. + // It makes things more complicated, but still manageable. + if (st.steps_x > 0) { + if (st.out_bits & (1< 0) { + if (st.out_bits & (1< 0) { + if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_SEGMENT; } - } else { - // If current block is finished, reset pointer - current_block = NULL; - plan_discard_current_block(); + + // Discard current segment by advancing buffer tail index + if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } + } - out_bits ^= settings.invert_mask; // Apply step port invert mask + st.out_bits ^= settings.invert_mask; // Apply step port invert mask } busy = false; // SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// st.ramp_type = RAMP_ACCEL; +// st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.delta_d = 0; +// sys.state = STATE_QUEUED; +// } else { +// sys.state = STATE_IDLE; +// } sys.state = STATE_IDLE; - } + +} + + +/* Prepares step segment buffer. Continuously called from main program. + + The segment buffer is an intermediary buffer interface between the execution of steps + by the stepper algorithm and the velocity profiles generated by the planner. The stepper + algorithm only executes steps within the segment buffer and is filled by the main program + when steps are "checked-out" from the first block in the planner buffer. This keeps the + step execution and planning optimization processes atomic and protected from each other. + The number of steps "checked-out" from the planner buffer and the number of segments in + the segment buffer is sized and computed such that no operation in the main program takes + longer than the time it takes the stepper algorithm to empty it before refilling it. + Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. + + NOTE: The segment buffer executes a set number of steps over an approximate time period. + If we try to execute over a set time period, it is difficult to guarantee or predict how + many steps will execute over it, especially when the step pulse phasing between the + neighboring segments are kept consistent. Meaning that, if the last segment step pulses + right before its end, the next segment must delay its first pulse so that the step pulses + are consistently spaced apart over time to keep the step pulse train nice and smooth. + Keeping track of phasing and ensuring that the exact number of steps are executed as + defined by the planner block, the related computational overhead gets quickly and + prohibitively expensive, especially in real-time. + Since the stepper algorithm automatically takes care of the step pulse phasing with + its ramp and inverse time counters, we don't have to explicitly and expensively track the + exact number of steps, time, or phasing of steps. All we need to do is approximate + the number of steps in each segment such that the segment buffer has enough execution time + for the main program to do what it needs to do and refill it when it has time. In other + words, we just need to compute a cheap approximation of the current velocity and the + number of steps over it. +*/ + +/* + TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. + The problem is that when an override is reduced, the planner may not plan back to + the current rate. Meaning that the velocity profiles for certain conditions no longer + are trapezoidal or triangular. For example, if the current block is cruising at a + nominal rate and the feedrate override is reduced, the new nominal rate will now be + lower. The velocity profile must first decelerate to the new nominal rate and then + follow on the new plan. So the remaining velocity profile will have a decelerate, + cruise, and another decelerate. + Another issue is whether or not a feedrate override reduction causes a deceleration + that acts over several planner blocks. For example, say that the plan is already + heavily decelerating throughout it, reducing the feedrate will not do much to it. So, + how do we determine when to resume the new plan? How many blocks do we have to wait + until the new plan intersects with the deceleration curve? One plus though, the + deceleration will never be more than the number of blocks in the entire planner buffer, + but it theoretically can be equal to it when all planner blocks are decelerating already. +*/ +void st_prep_buffer() +{ + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Initialize new segment + st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + prep_segment->flag = SEGMENT_NOOP; + + // Determine if we need to load a new planner block. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block + if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. + + // Increment stepper common data index + if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } + + // Check if the planner has re-computed this block mid-execution. If so, push the previous segment + // data. Otherwise, prepare a new segment data for the new planner block. + if (pl_partial_block_flag) { + + // Prepare new shared segment block data and copy the relevant last segment block data. + st_data_t *last_st_prep_data; + last_st_prep_data = st_prep_data; + st_prep_data = &segment_data[st_data_prep_index]; + + st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; + st_prep_data->rate_delta = last_st_prep_data->rate_delta; + st_prep_data->d_next = last_st_prep_data->d_next; + st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. + + st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; + + pl_partial_block_flag = false; // Reset flag + + } else { + + // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since + // the planner buffer can dynamically change the velocity profile data as blocks are added. + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert planner block velocity profile data to stepper rate and step distance data. + st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + + // TODO: Check if we really need to store this. + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + + } + + // Convert planner entry speed to stepper initial rate. + st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Nominal rate changes with feedrate override. + // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + st_prep_data->current_approx_rate = st_prep_data->initial_rate; + + // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. + float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); + st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); + if (st_prep_data->decelerate_after > 0) { // If 0, SEGMENT_DECEL flag is set later. + if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = SEGMENT_ACCEL; } + } + } + + // Set new segment to point to the current segment data block. + prep_segment->st_data_index = st_data_prep_index; + + // Approximate the velocity over the new segment using the already computed rate values. + // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. + // We do this to minimize memory and computational requirements. However, this could easily be replaced with + // a more exact approximation or have a unique time per segment, if CPU and memory overhead allows. + if (st_prep_data->decelerate_after <= 0) { + if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } // Set segment deceleration flag + else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } + if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } + } else { + if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate += st_prep_data->rate_delta; + if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate = st_prep_data->nominal_rate; + } + } + } + + // Compute the number of steps in the prepped segment based on the approximate current rate. + // NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. + prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* + (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next); + // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps + // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. + prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); + // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow. + // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + + // Check if n_step exceeds steps remaining in planner block. If so, truncate. + if (prep_segment->n_step > st_prep_data->step_events_remaining) { + prep_segment->n_step = st_prep_data->step_events_remaining; + } + + // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration + // ramp counters are set correctly during execution. + if (st_prep_data->decelerate_after > 0) { + if (prep_segment->n_step > st_prep_data->decelerate_after) { + prep_segment->n_step = st_prep_data->decelerate_after; + } + } + + // Update stepper common data variables. + st_prep_data->decelerate_after -= prep_segment->n_step; + st_prep_data->step_events_remaining -= prep_segment->n_step; + + // Check for end of planner block + if ( st_prep_data->step_events_remaining == 0 ) { + // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. + prep_segment->flag |= SEGMENT_END_OF_BLOCK; + // Move planner pointer to next block and flag to load a new block for the next segment. + pl_prep_index = plan_next_block_index(pl_prep_index); + pl_prep_block = NULL; + } + + // New step segment completed. Increment segment buffer indices. + segment_buffer_head = segment_next_head; + if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } + +// long a = prep_segment->n_step; +// printInteger(a); +// printString(" "); + + } +} + +uint8_t st_get_prep_block_index() +{ +// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this? + return(pl_prep_index); +} + +void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating) +{ + // if called, can we assume that this always changes and needs to be updated? if so, then + // we can perform all of the segment buffer setup tasks here to make sure the next time + // the segments are loaded, the st_data buffer is updated correctly. + // !!! Make sure that this is always pointing to the correct st_prep_data block. + + // When a mid-block acceleration occurs, we have to make sure the ramp counters are updated + // correctly, much in the same fashion as the deceleration counters. Need to think about this + // make sure this is right, but i'm pretty sure it is. + + // TODO: NULL means that the segment buffer has just completed a planner block. Clean up! + if (pl_prep_block != NULL) { + *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step; + if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } + else { *is_decelerating = true; } + + // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. + pl_partial_block_flag = true; + pl_prep_block = NULL; + } + return; } diff --git a/stepper_v0_9.c b/stepper_v0_9.c new file mode 100644 index 0000000..3191007 --- /dev/null +++ b/stepper_v0_9.c @@ -0,0 +1,387 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 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 + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith + and Philipp Tiefenbacher. */ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define CRUISE_RAMP 0 +#define ACCEL_RAMP 1 +#define DECEL_RAMP 2 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int32_t event_count; // Total event count. Retained for feed holds. + int32_t step_events_remaining; // Steps remaining in motion + + // Used by Pramod Ranade inverse time algorithm + int32_t delta_d; // Ranade distance traveled per interrupt tick + int32_t d_counter; // Ranade distance traveled since last step event + uint8_t ramp_count; // Acceleration interrupt tick counter. + uint8_t ramp_type; // Ramp type variable. + uint8_t execute_step; // Flags step execution for each interrupt. + +} 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 + +// 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. + +// __________________________ +// /| |\ _________________ ^ +// / | | \ /| |\ | +// / | | \ / | | \ s +// / | | | | | \ p +// / | | | | | \ e +// +-----+------------------------+---+--+---------------+----+ e +// | BLOCK 1 | BLOCK 2 | d +// +// time -----> +// +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta +// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after +// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as +// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. + + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + + // Initialize Bresenham variables + st.counter_x = (current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + st.event_count = current_block->step_event_count; + st.step_events_remaining = st.event_count; + + // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { + // Initialize Ranade variables + st.d_counter = current_block->d_next; + st.delta_d = current_block->initial_rate; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + + // Initialize ramp type. + if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } + else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } + else { st.ramp_type = ACCEL_RAMP; } + } + + } else { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + } + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { + // Tick acceleration ramp counter + st.ramp_count--; + if (st.ramp_count == 0) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration + st.delta_d += current_block->rate_delta; + if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. + st.ramp_type = CRUISE_RAMP; + st.delta_d = current_block->nominal_rate; // Set cruise velocity + } + } 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 >>= 1; // Integer divide by 2 until complete. Also prevents 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 = ISR_TICKS_PER_ACCELERATION_TICK/2; + } + if (st.delta_d <= current_block->rate_delta) { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); + return; + } + } + + // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). + + out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + out_bits |= (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + out_bits |= (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + out_bits |= (1<decelerate_after) { + st.ramp_type = DECEL_RAMP; + if (st.step_events_remaining == current_block->decelerate_after) { + if (st.delta_d == current_block->nominal_rate) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + } + } + } + } 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 + } + busy = false; +// SPINDLE_ENABLE_PORT ^= 1< Date: Tue, 29 Oct 2013 18:55:55 -0600 Subject: [PATCH 13/13] Planner function call fix. More clean up. --- planner.c | 172 +++++++++++++++++++++++++---------------------------- protocol.c | 2 + stepper.c | 61 +++++++++---------- 3 files changed, 115 insertions(+), 120 deletions(-) diff --git a/planner.c b/planner.c index c1d0350..0afa018 100644 --- a/planner.c +++ b/planner.c @@ -148,34 +148,18 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr) recomputed as stated in the general guidelines. Planner buffer index mapping: - - block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The - planner never touches the exit speed of this block, which always defaults to 0. - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. - Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible - as long as the segment buffer is not empty. - - next_buffer_head: Points to next planner buffer block after the last block. Should always be empty. - - block_buffer_safe: Points to the first planner block in the buffer for which it is safe to change. Since - the stepper can be executing the first block and if the planner changes its conditions, this will cause - a discontinuity and error in the stepper profile with lost steps likely. With the new stepper algorithm, - the block_buffer_safe is always where the stepper segment buffer ends and can never be overwritten, but - this can change the state of the block profile from a pure trapezoid assumption. Meaning, if that block - is decelerating, the planner conditions can change such that the block can new accelerate mid-block. - - !!! I need to make sure that the stepper algorithm can modify the acceleration mid-block. Needed for feedrate overrides too. - - !!! planner_recalculate() may not work correctly with re-planning.... may need to artificially set both the - block_buffer_head and next_buffer_head back one index so that this works correctly, or allow the operation - of this function to accept two different conditions to operate on. - - - block_buffer_planned: Points to the first buffer block after the last optimally fixed block, which can no longer be - improved. This block and the trailing buffer blocks that can still be altered when new blocks are added. This planned - block points to the transition point between the fixed and non-fixed states and is handled slightly different. The entry - speed is fixed, indicating the reverse pass cannot maximize the speed further, but the velocity profile within it - can still be changed, meaning the forward pass calculations must start from here and influence the following block - entry speed. - - !!! Need to check if this is the start of the non-optimal or the end of the optimal block. - + - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether + the buffer is full or empty. As described for standard ring buffers, this block is always empty. + - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the + buffer tail, this indicates the buffer is full. + - block_buffer_safe: Points to the first sequential planner block for which it is safe to recompute, which + is defined to be where the stepper's step segment buffer ends. This may or may not be the buffer tail, + since the step segment buffer queues steps which may have not finished executing and could span a few + blocks, if the block moves are very short. + - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal + streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the + planner buffer that don't change with the addition of a new block, as describe above. NOTE: All planner computations are performed in floating point to minimize numerical round-off errors. When a planner block is executed, the floating point values are converted to fast integers by the stepper @@ -198,7 +182,7 @@ static void planner_recalculate() { // Initialize block index to the last block in the planner buffer. - uint8_t block_index = prev_block_index(block_buffer_head); + uint8_t block_index = plan_prev_block_index(block_buffer_head); // Query stepper module for safe planner block index to recalculate to, which corresponds to the end // of the step segment buffer. @@ -324,58 +308,6 @@ static void planner_recalculate() } -/* - uint8_t block_buffer_safe = st_get_prep_block_index(); - if (block_buffer_head == block_buffer_safe) { // Also catches head = tail - block_buffer_planned = block_buffer_head; - } else { - uint8_t block_index = block_buffer_head; - plan_block_t *current = &block_buffer[block_index]; - current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); - float entry_speed_sqr; - plan_block_t *next; - block_index = plan_prev_block_index(block_index); - if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. - plan_update_partial_block(block_index, 0.0); - block_buffer_planned = block_index; - } else { - while (block_index != block_buffer_planned) { - next = current; - current = &block_buffer[block_index]; - block_index = plan_prev_block_index(block_index); - if (block_index == block_buffer_safe) { - plan_update_partial_block(block_index,current->entry_speed_sqr); - block_buffer_planned = block_index; - } - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { - entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < current->max_entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - } else { - current->entry_speed_sqr = current->max_entry_speed_sqr; - } - } - } - - } - next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer - block_index = plan_next_block_index(block_buffer_planned); - while (block_index != next_buffer_head) { - current = next; - next = &block_buffer[block_index]; - if (current->entry_speed_sqr < next->entry_speed_sqr) { - entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. - block_buffer_planned = block_index; // Set optimal plan pointer. - } - } - if (next->entry_speed_sqr == next->max_entry_speed_sqr) { - block_buffer_planned = block_index; // Set optimal plan pointer - } - block_index = plan_next_block_index( block_index ); - } - } */ } @@ -439,16 +371,16 @@ void plan_synchronize() } -// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position -// in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed -// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. -// All position data passed to the planner must be in terms of machine position to keep the planner -// independent of any coordinate system changes and offsets, which are handled by the g-code parser. -// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. -// In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value -// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if -// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and -// invert_feed_rate always false). +/* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position + in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed + rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. + All position data passed to the planner must be in terms of machine position to keep the planner + independent of any coordinate system changes and offsets, which are handled by the g-code parser. + NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. + In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value + is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if + invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and + invert_feed_rate always false). */ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) { // Prepare and initialize new block @@ -675,3 +607,63 @@ void plan_cycle_reinitialize(int32_t step_events_remaining) block_buffer_planned = block_buffer_tail; planner_recalculate(); } + + +/* +TODO: + When a feed hold or feedrate override is reduced, the velocity profile must execute a + deceleration over the existing plan. By logic, since the plan already decelerates to zero + at the end of the buffer, any replanned deceleration mid-way will never exceed this. It + will only asymptotically approach this in the worst case scenario. + + - For a feed hold, we simply need to plan and compute the stopping point within a block + when velocity decelerates to zero. We then can recompute the plan with the already + existing partial block planning code and set the system to a QUEUED state. + - When a feed hold is initiated, the main program should be able to continue doing what + it has been, i.e. arcs, parsing, but needs to be able to reinitialize the plan after + it has come to a stop. + + - For a feed rate override (reduce-only), we need to enforce a deceleration until we + intersect the reduced nominal speed of a block after it's been planned with the new + overrides and the newly planned block is accelerating or cruising only. If the new plan + block is decelerating at the intersection point, we keep decelerating until we find a + valid intersection point. Once we find this point, we can then resume onto the new plan, + but we may need to adjust the deceleration point in the intersection block since the + feedrate override could have intersected at an acceleration ramp. This would change the + acceleration ramp to a cruising, so the deceleration point will have changed, but the + plan will have not. It should still be valid for the rest of the buffer. Coding this + can get complicated, but it should be doable. One issue could be is in how to handle + scenarios when a user issues several feedrate overrides and inundates this code. Does + this method still work and is robust enough to compute all of this on the fly? This is + the critical question. However, we could block user input until the planner has time to + catch to solve this as well. + + - When the feed rate override increases, we don't have to do anything special. We just + replan the entire buffer with the new nominal speeds and adjust the maximum junction + speeds accordingly. + +void plan_compute_deceleration() { + +} + + +void plan_recompute_max_junction_velocity() { + // Assumes the nominal_speed_sqr values have been updated. May need to just multiply + // override values here. + // PROBLEM: Axes-limiting velocities get screwed up. May need to store an int8 value for the + // max override value possible for each block when the line is added. So the nominal_speed + // is computed with that ceiling, but still retained if the rates change again. + uint8_t block_index = block_buffer_tail; + plan_block_t *block = &block_buffer[block_index]; + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + block_index = plan_next_block_index(block_index); + while (block_index != block_buffer_head) { + block = &block_buffer[block_index]; + block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, + min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + block_index = plan_next_block_index(block_index); + } +} + +*/ diff --git a/protocol.c b/protocol.c index 5466720..014b607 100644 --- a/protocol.c +++ b/protocol.c @@ -153,6 +153,8 @@ void protocol_execute_runtime() // Initiate stepper feed hold if (rt_exec & EXEC_FEED_HOLD) { + // !!! During a cycle, the segment buffer has just been reloaded and full. So the math involved + // with the feed hold should be fine for most, if not all, operational scenarios. st_feed_hold(); // Initiate feed hold. bit_false(sys.execute,EXEC_FEED_HOLD); } diff --git a/stepper.c b/stepper.c index 22f1edd..e3e4b25 100644 --- a/stepper.c +++ b/stepper.c @@ -39,8 +39,8 @@ #define SEGMENT_NOOP 0 #define SEGMENT_END_OF_BLOCK bit(0) -#define SEGMENT_ACCEL bit(1) -#define SEGMENT_DECEL bit(2) +#define RAMP_CHANGE_ACCEL bit(1) +#define RAMP_CHANGE_DECEL bit(2) #define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change @@ -76,7 +76,7 @@ static stepper_t st; // the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). typedef struct { int32_t step_events_remaining; // Tracks step event count for the executing planner block - uint32_t dist_next_step; // Scaled distance to next step + uint32_t dist_per_step; // Scaled distance to next step uint32_t initial_rate; // Initialized step rate at re/start of a planner block uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) @@ -254,7 +254,7 @@ ISR(TIMER2_COMPA_vect) st.counter_z = st.counter_x; // Initialize inverse time, step rate data, and acceleration ramp counters - st.counter_dist = st_current_data->dist_next_step; // dist_next_step always greater than ramp_rate. + st.counter_dist = st_current_data->dist_per_step; // dist_per_step always greater than ramp_rate. st.ramp_rate = st_current_data->initial_rate; st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. @@ -265,7 +265,7 @@ ISR(TIMER2_COMPA_vect) } // Check if ramp conditions have changed. If so, update ramp counters and control variables. - if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) { + if ( st_current_segment->flag & (RAMP_CHANGE_DECEL | RAMP_CHANGE_ACCEL) ) { /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the @@ -274,7 +274,7 @@ ISR(TIMER2_COMPA_vect) as mandated by the mid-point rule. For the latter conditions, the ramp count have been initialized such that the following computation is still correct. */ st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp; - if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; } + if ( st_current_segment->flag & RAMP_CHANGE_DECEL ) { st.ramp_type = RAMP_DECEL; } else { st.ramp_type = RAMP_ACCEL; } } @@ -319,7 +319,7 @@ ISR(TIMER2_COMPA_vect) // Execute Bresenham step event, when it's time to do so. if (st.counter_dist < 0) { - st.counter_dist += st_current_data->dist_next_step; // Reload inverse time counter + st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; @@ -493,21 +493,21 @@ void st_cycle_reinitialize() Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. NOTE: The segment buffer executes a set number of steps over an approximate time period. - If we try to execute over a set time period, it is difficult to guarantee or predict how - many steps will execute over it, especially when the step pulse phasing between the - neighboring segments are kept consistent. Meaning that, if the last segment step pulses - right before its end, the next segment must delay its first pulse so that the step pulses - are consistently spaced apart over time to keep the step pulse train nice and smooth. - Keeping track of phasing and ensuring that the exact number of steps are executed as - defined by the planner block, the related computational overhead gets quickly and + If we try to execute over a fixed time period, it is difficult to guarantee or predict + how many steps will execute over it, especially when the step pulse phasing between the + neighboring segments must also be kept consistent. Meaning that, if the last segment step + pulses right before a segment end, the next segment must delay its first pulse so that the + step pulses are consistently spaced apart over time to keep the step pulse train nice and + smooth. Keeping track of phasing and ensuring that the exact number of steps are executed + as defined by the planner block, the related computational overhead can get quickly and prohibitively expensive, especially in real-time. Since the stepper algorithm automatically takes care of the step pulse phasing with - its ramp and inverse time counters, we don't have to explicitly and expensively track the - exact number of steps, time, or phasing of steps. All we need to do is approximate - the number of steps in each segment such that the segment buffer has enough execution time - for the main program to do what it needs to do and refill it when it has time. In other - words, we just need to compute a cheap approximation of the current velocity and the - number of steps over it. + its ramp and inverse time counters by retaining the count remainders, we don't have to + explicitly and expensively track and synchronize the exact number of steps, time, and + phasing of steps. All we need to do is approximate the number of steps in each segment + such that the segment buffer has enough execution time for the main program to do what + it needs to do and refill it when it comes back. In other words, we just need to compute + a cheap approximation of the current velocity and the number of steps over it. */ /* @@ -529,7 +529,7 @@ void st_cycle_reinitialize() */ void st_prep_buffer() { - if (sys.state != STATE_QUEUED) { // Block until a motion state is issued + if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. // Initialize new segment @@ -555,7 +555,7 @@ void st_prep_buffer() st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; st_prep_data->rate_delta = last_st_prep_data->rate_delta; - st_prep_data->dist_next_step = last_st_prep_data->dist_next_step; + st_prep_data->dist_per_step = last_st_prep_data->dist_per_step; st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; @@ -575,7 +575,7 @@ void st_prep_buffer() st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - st_prep_data->dist_next_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + st_prep_data->dist_per_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) // TODO: Check if we really need to store this. st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; @@ -593,8 +593,8 @@ void st_prep_buffer() // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); - if (st_prep_data->decelerate_after > 0) { // If 0, SEGMENT_DECEL flag is set later. - if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = SEGMENT_ACCEL; } + if (st_prep_data->decelerate_after > 0) { // If 0, RAMP_CHANGE_DECEL flag is set later. + if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = RAMP_CHANGE_ACCEL; } } } @@ -604,9 +604,9 @@ void st_prep_buffer() // Approximate the velocity over the new segment using the already computed rate values. // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. // We do this to minimize memory and computational requirements. However, this could easily be replaced with - // a more exact approximation or have a unique time per segment, if CPU and memory overhead allows. + // a more exact approximation or have an user-defined time per segment, if CPU and memory overhead allows. if (st_prep_data->decelerate_after <= 0) { - if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } // Set segment deceleration flag + if (st_prep_data->decelerate_after == 0) { prep_segment->flag = RAMP_CHANGE_DECEL; } // Set segment deceleration flag else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } } else { @@ -618,10 +618,12 @@ void st_prep_buffer() } } + // TODO: Look into replacing the following dist_per_step divide with multiplying its inverse to save cycles. + // Compute the number of steps in the prepped segment based on the approximate current rate. - // NOTE: The dist_next_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. + // NOTE: The dist_per_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* - (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_next_step); + (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_per_step); // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); @@ -668,7 +670,6 @@ void st_prep_buffer() // printString(" "); } - } } uint8_t st_get_prep_block_index()