From 23462de85acc595ba4a36783c3b45743e8c86b26 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Thu, 17 Feb 2011 20:23:12 +0100 Subject: [PATCH 01/82] cleaned up settings.h --- settings.c | 13 +++++++++++++ settings.h | 13 ------------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/settings.c b/settings.c index a3c33dd..6669cc9 100644 --- a/settings.c +++ b/settings.c @@ -39,6 +39,19 @@ typedef struct { double mm_per_arc_segment; } settings_v1_t; +// Default settings (used when resetting eeprom-settings) +#define MICROSTEPS 8 +#define DEFAULT_X_STEPS_PER_MM (94.488188976378*MICROSTEPS) +#define DEFAULT_Y_STEPS_PER_MM (94.488188976378*MICROSTEPS) +#define DEFAULT_Z_STEPS_PER_MM (94.488188976378*MICROSTEPS) +#define DEFAULT_STEP_PULSE_MICROSECONDS 30 +#define DEFAULT_MM_PER_ARC_SEGMENT 0.1 +#define DEFAULT_RAPID_FEEDRATE 480.0 // in millimeters per minute +#define DEFAULT_FEEDRATE 480.0 +#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/100.0) +#define DEFAULT_MAX_JERK 50.0 +#define DEFAULT_STEPPING_INVERT_MASK 0 + void settings_reset() { settings.steps_per_mm[X_AXIS] = DEFAULT_X_STEPS_PER_MM; settings.steps_per_mm[Y_AXIS] = DEFAULT_Y_STEPS_PER_MM; diff --git a/settings.h b/settings.h index 5831e0f..505cc3b 100644 --- a/settings.h +++ b/settings.h @@ -54,17 +54,4 @@ void settings_dump(); // A helper method to set new settings from command line void settings_store_setting(int parameter, double value); -// Default settings (used when resetting eeprom-settings) -#define MICROSTEPS 8 -#define DEFAULT_X_STEPS_PER_MM (94.488188976378*MICROSTEPS) -#define DEFAULT_Y_STEPS_PER_MM (94.488188976378*MICROSTEPS) -#define DEFAULT_Z_STEPS_PER_MM (94.488188976378*MICROSTEPS) -#define DEFAULT_STEP_PULSE_MICROSECONDS 30 -#define DEFAULT_MM_PER_ARC_SEGMENT 0.1 -#define DEFAULT_RAPID_FEEDRATE 480.0 // in millimeters per minute -#define DEFAULT_FEEDRATE 480.0 -#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/100.0) -#define DEFAULT_MAX_JERK 50.0 -#define DEFAULT_STEPPING_INVERT_MASK 0 - #endif From 2ff680a12584c7683f6228921356430dc5740c77 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Thu, 17 Feb 2011 20:24:04 +0100 Subject: [PATCH 02/82] improved some defaults --- settings.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/settings.c b/settings.c index 6669cc9..2f50564 100644 --- a/settings.c +++ b/settings.c @@ -46,10 +46,10 @@ typedef struct { #define DEFAULT_Z_STEPS_PER_MM (94.488188976378*MICROSTEPS) #define DEFAULT_STEP_PULSE_MICROSECONDS 30 #define DEFAULT_MM_PER_ARC_SEGMENT 0.1 -#define DEFAULT_RAPID_FEEDRATE 480.0 // in millimeters per minute -#define DEFAULT_FEEDRATE 480.0 -#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/100.0) -#define DEFAULT_MAX_JERK 50.0 +#define DEFAULT_RAPID_FEEDRATE 500.0 // in millimeters per minute +#define DEFAULT_FEEDRATE 500.0 +#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/10.0) +#define DEFAULT_MAX_JERK 300.0 #define DEFAULT_STEPPING_INVERT_MASK 0 void settings_reset() { From b8eee5ac9a403b84de4ddc86b824606e06390d8f Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Thu, 17 Feb 2011 21:41:41 +0100 Subject: [PATCH 03/82] clamped safe speed to 100% of nominal speed. Fixes a problem that might have spelled trouble at very low feed rates --- planner.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/planner.c b/planner.c index 7684632..6e9721d 100644 --- a/planner.c +++ b/planner.c @@ -168,7 +168,11 @@ inline double junction_jerk(block_t *before, block_t *after) { // Calculate a braking factor to reach baseline speed which is max_jerk/2, e.g. the // speed under which you cannot exceed max_jerk no matter what you do. double factor_for_safe_speed(block_t *block) { - return(settings.max_jerk/block->nominal_speed); + if(settings.max_jerk < block->nominal_speed) { + return(settings.max_jerk/block->nominal_speed); + } else { + return(1.0); + } } // The kernel called by planner_recalculate() when scanning the plan from last to first entry. From 124bc363bdbecd6ea99509d9f3a03d1dbb93a083 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Thu, 17 Feb 2011 23:59:10 +0100 Subject: [PATCH 04/82] removed inline-keywords because gcc ignores them anyway --- gcode.c | 2 +- planner.c | 12 ++++++------ planner.h | 4 ++-- stepper.c | 4 ++-- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gcode.c b/gcode.c index becac9d..ccb5d48 100644 --- a/gcode.c +++ b/gcode.c @@ -98,7 +98,7 @@ void gc_init() { gc.absolute_mode = TRUE; } -inline float to_millimeters(double value) { +float to_millimeters(double value) { return(gc.inches_mode ? (value * MM_PER_INCH) : value); } diff --git a/planner.c b/planner.c index 6e9721d..16566bc 100644 --- a/planner.c +++ b/planner.c @@ -79,7 +79,7 @@ static uint8_t acceleration_manager_enabled; // Acceleration management active // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // given acceleration: -inline double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) { +double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) { return( (target_rate*target_rate-initial_rate*initial_rate)/ (2L*acceleration) @@ -101,7 +101,7 @@ inline double estimate_acceleration_distance(double initial_rate, double target_ | | intersection_distance distance */ -inline double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) { +double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) { return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ (4*acceleration) @@ -148,7 +148,7 @@ void calculate_trapezoid_for_block(block_t *block, double entry_factor, double e // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // acceleration within the allotted distance. -inline double max_allowable_speed(double acceleration, double target_velocity, double distance) { +double max_allowable_speed(double acceleration, double target_velocity, double distance) { return( sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) ); @@ -157,7 +157,7 @@ inline double max_allowable_speed(double acceleration, double target_velocity, d // "Junction jerk" in this context is the immediate change in speed at the junction of two blocks. // This method will calculate the junction jerk as the euclidean distance between the nominal // velocities of the respective blocks. -inline double junction_jerk(block_t *before, block_t *after) { +double junction_jerk(block_t *before, block_t *after) { return(sqrt( pow(before->speed_x-after->speed_x, 2)+ pow(before->speed_y-after->speed_y, 2)+ @@ -322,13 +322,13 @@ int plan_is_acceleration_manager_enabled() { return(acceleration_manager_enabled); } -inline void plan_discard_current_block() { +void plan_discard_current_block() { if (block_buffer_head != block_buffer_tail) { block_buffer_tail = (block_buffer_tail + 1) % BLOCK_BUFFER_SIZE; } } -inline block_t *plan_get_current_block() { +block_t *plan_get_current_block() { if (block_buffer_head == block_buffer_tail) { return(NULL); } return(&block_buffer[block_buffer_tail]); } diff --git a/planner.h b/planner.h index c07242a..a132118 100644 --- a/planner.h +++ b/planner.h @@ -62,10 +62,10 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // Called when the current block is no longer needed. Discards the block and makes the memory // availible for new blocks. -inline void plan_discard_current_block(); +void plan_discard_current_block(); // Gets the current block. Returns NULL if buffer empty -inline block_t *plan_get_current_block(); +block_t *plan_get_current_block(); // Enables or disables acceleration-management for upcoming blocks void plan_set_acceleration_manager_enabled(int enabled); diff --git a/stepper.c b/stepper.c index 5b4efcc..2fcbec5 100644 --- a/stepper.c +++ b/stepper.c @@ -88,7 +88,7 @@ void st_wake_up() { // Initializes the trapezoid generator from the current block. Called whenever a new // block begins. -inline void trapezoid_generator_reset() { +void trapezoid_generator_reset() { trapezoid_adjusted_rate = current_block->initial_rate; trapezoid_tick_cycle_counter = 0; // Always start a new trapezoid with a full acceleration tick set_step_events_per_minute(trapezoid_adjusted_rate); @@ -97,7 +97,7 @@ inline void trapezoid_generator_reset() { // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event // interrupt. It can be assumed that the trapezoid-generator-parameters and the // current_block stays untouched by outside handlers for the duration of this function call. -inline void trapezoid_generator_tick() { +void trapezoid_generator_tick() { if (current_block) { if (step_events_completed < current_block->accelerate_until) { trapezoid_adjusted_rate += current_block->rate_delta; From 6152d16205ebd153c1c4414a3078c7753f91d501 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 18 Feb 2011 21:46:18 +0100 Subject: [PATCH 05/82] circle buffers now power of two size and indicies are unsigned to help gcc make a few nice optimizations --- planner.c | 6 +++--- wiring_serial.c | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/planner.c b/planner.c index 16566bc..d1109bd 100644 --- a/planner.c +++ b/planner.c @@ -64,11 +64,11 @@ #include "wiring_serial.h" // The number of linear motions that can be in the plan at any give time -#define BLOCK_BUFFER_SIZE 20 +#define BLOCK_BUFFER_SIZE 16 static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions -static volatile int block_buffer_head; // Index of the next block to be pushed -static volatile int block_buffer_tail; // Index of the block to process now +static volatile uint8_t block_buffer_head; // Index of the next block to be pushed +static volatile uint8_t block_buffer_tail; // Index of the block to process now // The current position of the tool in absolute steps static int32_t position[3]; diff --git a/wiring_serial.c b/wiring_serial.c index 20f8756..5109d39 100644 --- a/wiring_serial.c +++ b/wiring_serial.c @@ -31,12 +31,12 @@ // using a ring buffer (I think), in which rx_buffer_head is the index of the // location to which to write the next incoming character and rx_buffer_tail // is the index of the location from which to read. -#define RX_BUFFER_SIZE 200 +#define RX_BUFFER_SIZE 256 unsigned char rx_buffer[RX_BUFFER_SIZE]; -int rx_buffer_head = 0; -int rx_buffer_tail = 0; +uint8_t rx_buffer_head = 0; +uint8_t rx_buffer_tail = 0; void beginSerial(long baud) { From 85f62111b5d82140bc6d5cb15a89a4724270de6c Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 18 Feb 2011 22:11:53 +0100 Subject: [PATCH 06/82] renamed serial_protocol -> protocol --- Makefile | 2 +- gcode.c | 2 +- main.c | 2 +- serial_protocol.c => protocol.c | 4 ++-- serial_protocol.h => protocol.h | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) rename serial_protocol.c => protocol.c (96%) rename serial_protocol.h => protocol.h (93%) diff --git a/Makefile b/Makefile index 6a10604..06be840 100644 --- a/Makefile +++ b/Makefile @@ -30,7 +30,7 @@ DEVICE = atmega328p CLOCK = 16000000 PROGRAMMER = -c avrisp2 -P usb -OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o \ +OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o protocol.o stepper.o \ eeprom.o settings.o planner.o # FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m FUSES = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m diff --git a/gcode.c b/gcode.c index ccb5d48..320ac0b 100644 --- a/gcode.c +++ b/gcode.c @@ -30,7 +30,7 @@ #include "motion_control.h" #include "spindle_control.h" #include "errno.h" -#include "serial_protocol.h" +#include "protocol.h" #define MM_PER_INCH (25.4) diff --git a/main.c b/main.c index 0b9a354..ac9c54d 100644 --- a/main.c +++ b/main.c @@ -26,7 +26,7 @@ #include "spindle_control.h" #include "motion_control.h" #include "gcode.h" -#include "serial_protocol.h" +#include "protocol.h" #include "settings.h" #include "wiring_serial.h" diff --git a/serial_protocol.c b/protocol.c similarity index 96% rename from serial_protocol.c rename to protocol.c index f2dcd47..488836b 100644 --- a/serial_protocol.c +++ b/protocol.c @@ -1,5 +1,5 @@ /* - serial_protocol.c - the serial protocol master control unit + protocol.c - the serial protocol master control unit Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud @@ -19,7 +19,7 @@ */ #include -#include "serial_protocol.h" +#include "protocol.h" #include "gcode.h" #include "wiring_serial.h" #include "settings.h" diff --git a/serial_protocol.h b/protocol.h similarity index 93% rename from serial_protocol.h rename to protocol.h index ee9ecf4..597205e 100644 --- a/serial_protocol.h +++ b/protocol.h @@ -1,5 +1,5 @@ /* - serial_protocol.h - the serial protocol master control unit + protocol.h - the serial protocol master control unit Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud From 6893463e80a6d0d3726ae4d6c831fc8c17596fef Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 18 Feb 2011 22:19:01 +0100 Subject: [PATCH 07/82] renamed protocol methods to reflect the new module name --- doc/structure.txt | 4 ++-- main.c | 4 ++-- protocol.c | 4 ++-- protocol.h | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/doc/structure.txt b/doc/structure.txt index 93cb1bd..21c5757 100644 --- a/doc/structure.txt +++ b/doc/structure.txt @@ -3,10 +3,10 @@ The general structure of Grbl The main processing stack: -'serial_protocol' : Accepts command lines from the serial port and passes them to 'gcode' for execution. +'protocol' : Accepts command lines from the serial port and passes them to 'gcode' for execution. Provides status responses for each command. -'gcode' : Recieves gcode from 'serial_protocol', parses it according to the current state +'gcode' : Recieves gcode from 'protocol', parses it according to the current state of the parser and issues commands via '..._control' modules 'spindle_control' : Commands for controlling the spindle. diff --git a/main.c b/main.c index ac9c54d..5179c6b 100644 --- a/main.c +++ b/main.c @@ -37,7 +37,7 @@ int main(void) { - sp_init(); + protocol_init(); settings_init(); plan_init(); st_init(); @@ -46,7 +46,7 @@ int main(void) for(;;){ sleep_mode(); // Wait for it ... - sp_process(); // ... process the serial protocol + protocol_process(); // ... process the serial protocol } return 0; /* never reached */ } diff --git a/protocol.c b/protocol.c index 488836b..bdd5cfc 100644 --- a/protocol.c +++ b/protocol.c @@ -51,14 +51,14 @@ void status_message(int status_code) { } } -void sp_init() +void protocol_init() { beginSerial(BAUD_RATE); printPgmString(PSTR("\r\nGrbl " GRBL_VERSION)); printPgmString(PSTR("\r\n")); } -void sp_process() +void protocol_process() { char c; while((c = serialRead()) != -1) diff --git a/protocol.h b/protocol.h index 597205e..318adcf 100644 --- a/protocol.h +++ b/protocol.h @@ -21,10 +21,10 @@ #define serial_h // Initialize the serial protocol -void sp_init(); +void protocol_init(); // Read command lines from the serial port and execute them as they // come in. Blocks until the serial buffer is emptied. -void sp_process(); +void protocol_process(); #endif From 9876e14f0be06ccde0b5514164142cd7161e8e1c Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 18 Feb 2011 22:59:16 +0100 Subject: [PATCH 08/82] refactored handling of settings '$' command out of gcode module and into settings module --- Makefile | 2 +- gcode.c | 56 ++++++++++------------------------------------------ gcode.h | 6 ------ nuts_bolts.c | 18 +++++++++++++++++ nuts_bolts.h | 3 +++ protocol.c | 41 ++++++++++++++++++++++++-------------- protocol.h | 9 +++++++++ settings.c | 23 +++++++++++++++++++++ settings.h | 3 +++ 9 files changed, 93 insertions(+), 68 deletions(-) create mode 100644 nuts_bolts.c diff --git a/Makefile b/Makefile index 06be840..ae641fc 100644 --- a/Makefile +++ b/Makefile @@ -31,7 +31,7 @@ DEVICE = atmega328p CLOCK = 16000000 PROGRAMMER = -c avrisp2 -P usb OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o protocol.o stepper.o \ - eeprom.o settings.o planner.o + eeprom.o settings.o planner.o nuts_bolts.o # FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m FUSES = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m # update that line with this when programmer is back up: diff --git a/gcode.c b/gcode.c index 320ac0b..d4f38b7 100644 --- a/gcode.c +++ b/gcode.c @@ -22,7 +22,6 @@ by Kramer, Proctor and Messina. */ #include "gcode.h" -#include #include #include "nuts_bolts.h" #include @@ -63,7 +62,7 @@ typedef struct { uint8_t inches_mode; /* 0 = millimeter mode, 1 = inches mode {G20, G21} */ uint8_t absolute_mode; /* 0 = relative motion, 1 = absolute motion {G90, G91} */ uint8_t program_flow; - int spindle_direction; + int8_t spindle_direction; double feed_rate, seek_rate; /* Millimeters/second */ double position[3]; /* Where the interpreter considers the tool to be at this point in the code */ uint8_t tool; @@ -76,11 +75,7 @@ static parser_state_t gc; #define FAIL(status) gc.status_code = status; -int read_double(char *line, // <- string: line of RS274/NGC code being processed - int *char_counter, // <- pointer to a counter for position on the line - double *double_ptr); // <- pointer to double to be read - -int next_statement(char *letter, double *double_ptr, char *line, int *char_counter); +int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter); void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) @@ -122,7 +117,7 @@ double theta(double x, double y) // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase // characters and signed floating point values (no whitespace). uint8_t gc_execute_line(char *line) { - int char_counter = 0; + uint8_t char_counter = 0; char letter; double value; double unit_converted_value; @@ -140,27 +135,12 @@ uint8_t gc_execute_line(char *line) { clear_vector(target); clear_vector(offset); - gc.status_code = GCSTATUS_OK; + gc.status_code = STATUS_OK; // Disregard comments and block delete if (line[0] == '(') { return(gc.status_code); } if (line[0] == '/') { char_counter++; } // ignore block delete - // If the line starts with an '$' it is a configuration-command - if (line[0] == '$') { - // Parameter lines are on the form '$4=374.3' or '$' to dump current settings - char_counter = 1; - if(line[char_counter] == 0) { settings_dump(); return(GCSTATUS_OK); } - read_double(line, &char_counter, &p); - if(line[char_counter++] != '=') { return(GCSTATUS_UNSUPPORTED_STATEMENT); } - read_double(line, &char_counter, &value); - if(line[char_counter] != 0) { return(GCSTATUS_UNSUPPORTED_STATEMENT); } - settings_store_setting(p, value); - return(gc.status_code); - } - - /* We'll handle this as g-code. First: parse all statements */ - // Pass 1: Commands while(next_statement(&letter, &value, line, &char_counter)) { int_value = trunc(value); @@ -184,7 +164,7 @@ uint8_t gc_execute_line(char *line) { case 91: gc.absolute_mode = FALSE; break; case 93: gc.inverse_feed_rate_mode = TRUE; break; case 94: gc.inverse_feed_rate_mode = FALSE; break; - default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT); + default: FAIL(STATUS_UNSUPPORTED_STATEMENT); } break; @@ -195,7 +175,7 @@ uint8_t gc_execute_line(char *line) { case 3: gc.spindle_direction = 1; break; case 4: gc.spindle_direction = -1; break; case 5: gc.spindle_direction = 0; break; - default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT); + default: FAIL(STATUS_UNSUPPORTED_STATEMENT); } break; case 'T': gc.tool = trunc(value); break; @@ -324,7 +304,7 @@ uint8_t gc_execute_line(char *line) { double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d) // If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any // real CNC, and thus - for practical reasons - we will terminate promptly: - if(isnan(h_x2_div_d)) { FAIL(GCSTATUS_FLOATING_POINT_ERROR); return(gc.status_code); } + if(isnan(h_x2_div_d)) { FAIL(STATUS_FLOATING_POINT_ERROR); return(gc.status_code); } // 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; } @@ -407,40 +387,24 @@ 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). -int next_statement(char *letter, double *double_ptr, char *line, int *char_counter) { +int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter) { if (line[*char_counter] == 0) { return(0); // No more statements } *letter = line[*char_counter]; if((*letter < 'A') || (*letter > 'Z')) { - FAIL(GCSTATUS_EXPECTED_COMMAND_LETTER); + FAIL(STATUS_EXPECTED_COMMAND_LETTER); return(0); } (*char_counter)++; if (!read_double(line, char_counter, double_ptr)) { + FAIL(STATUS_BAD_NUMBER_FORMAT); return(0); }; return(1); } -int read_double(char *line, //!< string: line of RS274/NGC code being processed - int *char_counter, //!< pointer to a counter for position on the line - double *double_ptr) //!< pointer to double to be read -{ - char *start = line + *char_counter; - char *end; - - *double_ptr = strtod(start, &end); - if(end == start) { - FAIL(GCSTATUS_BAD_NUMBER_FORMAT); - return(0); - }; - - *char_counter = end - line; - return(1); -} - /* Intentionally not supported: diff --git a/gcode.h b/gcode.h index 8adb836..7ad4e0e 100644 --- a/gcode.h +++ b/gcode.h @@ -23,12 +23,6 @@ #define gcode_h #include -#define GCSTATUS_OK 0 -#define GCSTATUS_BAD_NUMBER_FORMAT 1 -#define GCSTATUS_EXPECTED_COMMAND_LETTER 2 -#define GCSTATUS_UNSUPPORTED_STATEMENT 3 -#define GCSTATUS_FLOATING_POINT_ERROR 4 - // Initialize the parser void gc_init(); diff --git a/nuts_bolts.c b/nuts_bolts.c new file mode 100644 index 0000000..614f3cd --- /dev/null +++ b/nuts_bolts.c @@ -0,0 +1,18 @@ +#include "nuts_bolts.h" +#include +#include + +int read_double(char *line, uint8_t *char_counter, double *double_ptr) +{ + char *start = line + *char_counter; + char *end; + + *double_ptr = strtod(start, &end); + if(end == start) { + return(0); + }; + + *char_counter = end - line; + return(1); +} + diff --git a/nuts_bolts.h b/nuts_bolts.h index ca4ecf5..614cc12 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -21,6 +21,7 @@ #ifndef nuts_bolts_h #define nuts_bolts_h #include +#include #define FALSE 0 #define TRUE 1 @@ -32,4 +33,6 @@ #define clear_vector(a) memset(a, 0, sizeof(a)) #define max(a,b) (((a) > (b)) ? (a) : (b)) +int read_double(char *line, uint8_t *char_counter, double *double_ptr); + #endif diff --git a/protocol.c b/protocol.c index bdd5cfc..aaed597 100644 --- a/protocol.c +++ b/protocol.c @@ -33,21 +33,23 @@ static char line[LINE_BUFFER_SIZE]; static uint8_t char_counter; void status_message(int status_code) { - switch(status_code) { - case GCSTATUS_OK: - printPgmString(PSTR("ok\n\r")); break; - case GCSTATUS_BAD_NUMBER_FORMAT: - printPgmString(PSTR("error: Bad number format\n\r")); break; - case GCSTATUS_EXPECTED_COMMAND_LETTER: - printPgmString(PSTR("error: Expected command letter\n\r")); break; - case GCSTATUS_UNSUPPORTED_STATEMENT: - printPgmString(PSTR("error: Unsupported statement\n\r")); break; - case GCSTATUS_FLOATING_POINT_ERROR: - printPgmString(PSTR("error: Floating point error\n\r")); break; - default: + if (status_code == 0) { + printPgmString(PSTR("ok\n\r")); + } else { printPgmString(PSTR("error: ")); - printInteger(status_code); - printPgmString(PSTR("\n\r")); + switch(status_code) { + case STATUS_BAD_NUMBER_FORMAT: + printPgmString(PSTR("Bad number format\n\r")); break; + case STATUS_EXPECTED_COMMAND_LETTER: + printPgmString(PSTR("Expected command letter\n\r")); break; + case STATUS_UNSUPPORTED_STATEMENT: + printPgmString(PSTR("Unsupported statement\n\r")); break; + case STATUS_FLOATING_POINT_ERROR: + printPgmString(PSTR("Floating point error\n\r")); break; + default: + printInteger(status_code); + printPgmString(PSTR("\n\r")); + } } } @@ -58,6 +60,15 @@ void protocol_init() printPgmString(PSTR("\r\n")); } +// Executes one line of input according to protocol +uint8_t protocol_execute_line(char *line) { + if(line[0] == '$') { + return(settings_execute_line(line)); // Delegate lines starting with '$' to the settings module + } else { + return(gc_execute_line(line)); // Everything else is gcode + } +} + void protocol_process() { char c; @@ -65,7 +76,7 @@ void protocol_process() { if((char_counter > 0) && ((c == '\n') || (c == '\r'))) { // Line is complete. Then execute! line[char_counter] = 0; // treminate string - status_message(gc_execute_line(line)); + status_message(protocol_execute_line(line)); char_counter = 0; // reset line buffer index } else if (c <= ' ') { // Throw away whitepace and control characters } else if (c >= 'a' && c <= 'z') { // Upcase lowercase diff --git a/protocol.h b/protocol.h index 318adcf..7fa7704 100644 --- a/protocol.h +++ b/protocol.h @@ -20,6 +20,12 @@ #ifndef serial_h #define serial_h +#define STATUS_OK 0 +#define STATUS_BAD_NUMBER_FORMAT 1 +#define STATUS_EXPECTED_COMMAND_LETTER 2 +#define STATUS_UNSUPPORTED_STATEMENT 3 +#define STATUS_FLOATING_POINT_ERROR 4 + // Initialize the serial protocol void protocol_init(); @@ -27,4 +33,7 @@ void protocol_init(); // come in. Blocks until the serial buffer is emptied. void protocol_process(); +// Executes one line of input according to protocol +uint8_t protocol_execute_line(char *line); + #endif diff --git a/settings.c b/settings.c index 2f50564..e5b2583 100644 --- a/settings.c +++ b/settings.c @@ -25,6 +25,7 @@ #include "eeprom.h" #include "wiring_serial.h" #include +#include "protocol.h" settings_t settings; @@ -81,6 +82,28 @@ void settings_dump() { printPgmString(PSTR("\r\n'$x=value' to set parameter or just '$' to dump current settings\r\n")); } +// Parameter lines are on the form '$4=374.3' or '$' to dump current settings +uint8_t settings_execute_line(char *line) { + uint8_t char_counter = 1; + double parameter, value; + if(line[0] != '$') { + return(STATUS_UNSUPPORTED_STATEMENT); + } + if(line[char_counter] == 0) { + settings_dump(); return(STATUS_OK); + } + read_double(line, &char_counter, ¶meter); + if(line[char_counter++] != '=') { + return(STATUS_UNSUPPORTED_STATEMENT); + } + read_double(line, &char_counter, &value); + if(line[char_counter] != 0) { + return(STATUS_UNSUPPORTED_STATEMENT); + } + settings_store_setting(parameter, value); + return(STATUS_OK); +} + void write_settings() { eeprom_put_char(0, SETTINGS_VERSION); memcpy_to_eeprom_with_checksum(1, (char*)&settings, sizeof(settings_t)); diff --git a/settings.h b/settings.h index 505cc3b..93feede 100644 --- a/settings.h +++ b/settings.h @@ -51,6 +51,9 @@ void settings_init(); // Print current settings void settings_dump(); +// Handle settings command +uint8_t settings_execute_line(char *line); + // A helper method to set new settings from command line void settings_store_setting(int parameter, double value); From 6edbbe322c2a67a9c1b875e397e8f4420d35cca5 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 18 Feb 2011 23:04:12 +0100 Subject: [PATCH 09/82] lowercased boolean constants --- gcode.c | 24 ++++++++++++------------ motion_control.c | 2 +- nuts_bolts.h | 4 ++-- planner.c | 2 +- settings.c | 8 ++++---- stepper.c | 6 +++--- 6 files changed, 23 insertions(+), 23 deletions(-) diff --git a/gcode.c b/gcode.c index d4f38b7..7cf1b8e 100644 --- a/gcode.c +++ b/gcode.c @@ -90,7 +90,7 @@ void gc_init() { gc.feed_rate = settings.default_feed_rate/60; gc.seek_rate = settings.default_seek_rate/60; select_plane(X_AXIS, Y_AXIS, Z_AXIS); - gc.absolute_mode = TRUE; + gc.absolute_mode = true; } float to_millimeters(double value) { @@ -122,9 +122,9 @@ uint8_t gc_execute_line(char *line) { double value; double unit_converted_value; double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified - int radius_mode = FALSE; + int radius_mode = false; - uint8_t absolute_override = FALSE; /* 1 = absolute motion for this block only {G53} */ + uint8_t absolute_override = false; /* 1 = absolute motion for this block only {G53} */ uint8_t next_action = NEXT_ACTION_DEFAULT; /* The action that will be taken by the parsed line */ double target[3], offset[3]; @@ -155,15 +155,15 @@ uint8_t gc_execute_line(char *line) { case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break; case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break; case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break; - case 20: gc.inches_mode = TRUE; break; - case 21: gc.inches_mode = FALSE; break; + case 20: gc.inches_mode = true; break; + case 21: gc.inches_mode = false; break; case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break; - case 53: absolute_override = TRUE; break; + case 53: absolute_override = true; break; case 80: gc.motion_mode = MOTION_MODE_CANCEL; break; - case 90: gc.absolute_mode = TRUE; break; - case 91: gc.absolute_mode = FALSE; break; - case 93: gc.inverse_feed_rate_mode = TRUE; break; - case 94: gc.inverse_feed_rate_mode = FALSE; break; + case 90: gc.absolute_mode = true; break; + case 91: gc.absolute_mode = false; break; + case 93: gc.inverse_feed_rate_mode = true; break; + case 94: gc.inverse_feed_rate_mode = false; break; default: FAIL(STATUS_UNSUPPORTED_STATEMENT); } break; @@ -208,7 +208,7 @@ uint8_t gc_execute_line(char *line) { break; case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break; case 'P': p = value; break; - case 'R': r = unit_converted_value; radius_mode = TRUE; break; + case 'R': r = unit_converted_value; radius_mode = true; break; case 'S': gc.spindle_speed = value; break; case 'X': case 'Y': case 'Z': if (gc.absolute_mode || absolute_override) { @@ -238,7 +238,7 @@ uint8_t gc_execute_line(char *line) { switch (gc.motion_mode) { case MOTION_MODE_CANCEL: break; case MOTION_MODE_SEEK: - mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, FALSE); + mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false); break; case MOTION_MODE_LINEAR: mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], diff --git a/motion_control.c b/motion_control.c index f1f1b45..f799cca 100644 --- a/motion_control.c +++ b/motion_control.c @@ -48,7 +48,7 @@ void mc_arc(double theta, double angular_travel, double radius, double linear_tr int axis_linear, double feed_rate, int invert_feed_rate, double *position) { int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); - plan_set_acceleration_manager_enabled(FALSE); // disable acceleration management for the duration of the arc + plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel)); if (millimeters_of_travel == 0.0) { return; } uint16_t segments = ceil(millimeters_of_travel/settings.mm_per_arc_segment); diff --git a/nuts_bolts.h b/nuts_bolts.h index 614cc12..7cd745d 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -23,8 +23,8 @@ #include #include -#define FALSE 0 -#define TRUE 1 +#define false 0 +#define true 1 #define X_AXIS 0 #define Y_AXIS 1 diff --git a/planner.c b/planner.c index d1109bd..ce7f184 100644 --- a/planner.c +++ b/planner.c @@ -307,7 +307,7 @@ void planner_recalculate() { void plan_init() { block_buffer_head = 0; block_buffer_tail = 0; - plan_set_acceleration_manager_enabled(TRUE); + plan_set_acceleration_manager_enabled(true); clear_vector(position); } diff --git a/settings.c b/settings.c index e5b2583..1e6f290 100644 --- a/settings.c +++ b/settings.c @@ -116,19 +116,19 @@ int read_settings() { if (version == SETTINGS_VERSION) { // Read settings-record and check checksum if (!(memcpy_from_eeprom_with_checksum((char*)&settings, 1, sizeof(settings_t)))) { - return(FALSE); + return(false); } } else if (version == 1) { // Migrate from old settings version if (!(memcpy_from_eeprom_with_checksum((char*)&settings, 1, sizeof(settings_v1_t)))) { - return(FALSE); + return(false); } settings.acceleration = DEFAULT_ACCELERATION; settings.max_jerk = DEFAULT_MAX_JERK; } else { - return(FALSE); + return(false); } - return(TRUE); + return(true); } // A helper method to set settings from command line diff --git a/stepper.c b/stepper.c index 2fcbec5..964082d 100644 --- a/stepper.c +++ b/stepper.c @@ -55,7 +55,7 @@ static int32_t counter_x, // Counter variables for the bresenham line trac counter_y, counter_z; static uint32_t step_events_completed; // The number of step events executed in the current block -static volatile int busy; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler. +static volatile int busy; // true when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler. // Variables used by the trapezoid generation static uint32_t cycles_per_step_event; // The number of machine cycles between each step event @@ -138,7 +138,7 @@ SIGNAL(TIMER1_COMPA_vect) // exactly settings.pulse_microseconds microseconds. TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND)/8); - busy = TRUE; + busy = true; sei(); // Re enable interrupts (normally disabled while inside an interrupt handler) // ((We re-enable interrupts in order for SIG_OVERFLOW2 to be able to be triggered // at exactly the right time even if we occasionally spend a lot of time inside this handler.)) @@ -195,7 +195,7 @@ SIGNAL(TIMER1_COMPA_vect) trapezoid_generator_tick(); } - busy=FALSE; + busy=false; } // This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets From 6be195ba38164ae044e31b02e5f8f5ebc641054d Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 18 Feb 2011 23:08:06 +0100 Subject: [PATCH 10/82] cleaned up handling of number parsing in settings_execute_command --- nuts_bolts.c | 4 ++-- nuts_bolts.h | 3 +++ settings.c | 8 ++++++-- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/nuts_bolts.c b/nuts_bolts.c index 614f3cd..0a8bf5c 100644 --- a/nuts_bolts.c +++ b/nuts_bolts.c @@ -9,10 +9,10 @@ int read_double(char *line, uint8_t *char_counter, double *double_ptr) *double_ptr = strtod(start, &end); if(end == start) { - return(0); + return(false); }; *char_counter = end - line; - return(1); + return(true); } diff --git a/nuts_bolts.h b/nuts_bolts.h index 7cd745d..bc0b892 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -33,6 +33,9 @@ #define clear_vector(a) memset(a, 0, sizeof(a)) #define max(a,b) (((a) > (b)) ? (a) : (b)) +// Read a floating point value from a string. Line points to the input buffer, char_counter +// is the indexer pointing to the current character of the line, while double_ptr is +// a pointer to the result variable. Returns true when it succeeds int read_double(char *line, uint8_t *char_counter, double *double_ptr); #endif diff --git a/settings.c b/settings.c index 1e6f290..ca1bc05 100644 --- a/settings.c +++ b/settings.c @@ -92,11 +92,15 @@ uint8_t settings_execute_line(char *line) { if(line[char_counter] == 0) { settings_dump(); return(STATUS_OK); } - read_double(line, &char_counter, ¶meter); + if(!read_double(line, &char_counter, ¶meter)) { + return(STATUS_BAD_NUMBER_FORMAT); + }; if(line[char_counter++] != '=') { return(STATUS_UNSUPPORTED_STATEMENT); } - read_double(line, &char_counter, &value); + if(!read_double(line, &char_counter, &value)) { + return(STATUS_BAD_NUMBER_FORMAT); + } if(line[char_counter] != 0) { return(STATUS_UNSUPPORTED_STATEMENT); } From 464dcd12e8d56a2c7dfa716f129fec6ebcd3b730 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sat, 19 Feb 2011 00:32:36 +0100 Subject: [PATCH 11/82] formatting --- doc/planner-maths.txt | 30 +++++++++++++++++++++++ doc/structure.txt | 2 +- motion_control.h | 4 +++- planner.c | 55 +++++++++---------------------------------- planner.h | 3 --- 5 files changed, 45 insertions(+), 49 deletions(-) create mode 100644 doc/planner-maths.txt diff --git a/doc/planner-maths.txt b/doc/planner-maths.txt new file mode 100644 index 0000000..49f371f --- /dev/null +++ b/doc/planner-maths.txt @@ -0,0 +1,30 @@ +Reasoning behind the mathematics in 'planner' module (in the key of 'Mathematica') +================================================================================== + + +s == speed, a == acceleration, t == time, d == distance + +Basic definitions: + + Speed[s_, a_, t_] := s + (a*t) + Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t] + +Distance to reach a specific speed with a constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t] + d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance() + +Speed after a given distance of travel with constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t] + m -> Sqrt[2 a d + s^2] + + DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2] + +When to start braking (di) to reach a specified destionation speed (s2) after accelerating +from initial speed s1 without ever stopping at a plateau: + + Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di] + di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance() + + IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) diff --git a/doc/structure.txt b/doc/structure.txt index 21c5757..f87feb3 100644 --- a/doc/structure.txt +++ b/doc/structure.txt @@ -3,7 +3,7 @@ The general structure of Grbl The main processing stack: -'protocol' : Accepts command lines from the serial port and passes them to 'gcode' for execution. +'protocol' : Accepts command lines from the serial port and passes them to 'gcode' for execution. Provides status responses for each command. 'gcode' : Recieves gcode from 'protocol', parses it according to the current state diff --git a/motion_control.h b/motion_control.h index d2f409b..c205e2f 100644 --- a/motion_control.h +++ b/motion_control.h @@ -27,14 +27,16 @@ // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. -#define mc_line(x, y, z, feed_rate, invert_feed_rate) plan_buffer_line(x, y, z, feed_rate, invert_feed_rate) // NOTE: Although this function structurally belongs in this module, there is nothing to do but // to forward the request to the planner. For efficiency the function is implemented with a macro. +#define mc_line(x, y, z, feed_rate, invert_feed_rate) plan_buffer_line(x, y, z, feed_rate, invert_feed_rate) + // Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc, // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the // circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining // axis in axis_l which will be the axis for linear travel if you are tracing a helical motion. + void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, int axis_linear, double feed_rate, int invert_feed_rate, double *position); diff --git a/planner.c b/planner.c index ce7f184..4054f58 100644 --- a/planner.c +++ b/planner.c @@ -20,38 +20,6 @@ /* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ -/* - Reasoning behind the mathematics in this module (in the key of 'Mathematica'): - - s == speed, a == acceleration, t == time, d == distance - - Basic definitions: - - Speed[s_, a_, t_] := s + (a*t) - Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t] - - Distance to reach a specific speed with a constant acceleration: - - Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t] - d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance() - - Speed after a given distance of travel with constant acceleration: - - Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t] - m -> Sqrt[2 a d + s^2] - - DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2] - - When to start braking (di) to reach a specified destionation speed (s2) after accelerating - from initial speed s1 without ever stopping at a plateau: - - Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di] - di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance() - - IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) -*/ - - #include #include #include @@ -67,8 +35,8 @@ #define BLOCK_BUFFER_SIZE 16 static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions -static volatile uint8_t block_buffer_head; // Index of the next block to be pushed -static volatile uint8_t block_buffer_tail; // Index of the block to process now +static volatile uint8_t block_buffer_head; // Index of the next block to be pushed +static volatile uint8_t block_buffer_tail; // Index of the block to process now // The current position of the tool in absolute steps static int32_t position[3]; @@ -86,11 +54,6 @@ double estimate_acceleration_distance(double initial_rate, double target_rate, d ); } -// This function gives you the point at which you must start braking (at the rate of -acceleration) if -// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after -// a total travel of distance. This can be used to compute the intersection point between acceleration and -// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) - /* + <- some maximum rate we don't care about /|\ / | \ @@ -101,6 +64,12 @@ double estimate_acceleration_distance(double initial_rate, double target_rate, d | | intersection_distance distance */ + +// This function gives you the point at which you must start braking (at the rate of -acceleration) if +// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after +// a total travel of distance. This can be used to compute the intersection point between acceleration and +// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) + double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) { return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ @@ -108,10 +77,6 @@ double intersection_distance(double initial_rate, double final_rate, double acce ); } - -// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. -// The factors represent a factor of braking and must be in the range 0.0-1.0. - /* +--------+ <- nominal_rate / \ @@ -121,6 +86,9 @@ double intersection_distance(double initial_rate, double final_rate, double acce time --> */ +// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. +// The factors represent a factor of braking and must be in the range 0.0-1.0. + void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { block->initial_rate = ceil(block->nominal_rate*entry_factor); block->final_rate = ceil(block->nominal_rate*exit_factor); @@ -418,4 +386,3 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert if (acceleration_manager_enabled) { planner_recalculate(); } st_wake_up(); } - diff --git a/planner.h b/planner.h index a132118..ab622d5 100644 --- a/planner.h +++ b/planner.h @@ -18,9 +18,6 @@ along with Grbl. If not, see . */ -// This module is to be considered a sub-module of stepper.c. Please don't include -// this file from any other module. - #ifndef planner_h #define planner_h From d21a791eae51d3b21f7537b643bf0743de7488f0 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sat, 19 Feb 2011 21:04:40 +0100 Subject: [PATCH 12/82] spindle operations are now synchronized --- spindle_control.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/spindle_control.c b/spindle_control.c index 0dc48c8..548e477 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -21,6 +21,7 @@ #include "spindle_control.h" #include "settings.h" #include "config.h" +#include "stepper.h" #include @@ -31,6 +32,7 @@ void spindle_init() void spindle_run(int direction, uint32_t rpm) { + st_synchronize(); if(direction >= 0) { SPINDLE_DIRECTION_PORT &= ~(1< Date: Sat, 19 Feb 2011 23:03:10 +0100 Subject: [PATCH 13/82] made most internal function static to allow gcc to inline them --- gcode.c | 11 +++++------ planner.c | 24 ++++++++++++------------ protocol.c | 2 +- stepper.c | 10 +++++----- 4 files changed, 23 insertions(+), 24 deletions(-) diff --git a/gcode.c b/gcode.c index 7cf1b8e..c5e656e 100644 --- a/gcode.c +++ b/gcode.c @@ -75,10 +75,9 @@ static parser_state_t gc; #define FAIL(status) gc.status_code = status; -int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter); +static int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter); - -void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) +static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) { gc.plane_axis_0 = axis_0; gc.plane_axis_1 = axis_1; @@ -93,13 +92,13 @@ void gc_init() { gc.absolute_mode = true; } -float to_millimeters(double value) { +static float to_millimeters(double value) { return(gc.inches_mode ? (value * MM_PER_INCH) : value); } // Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis, // positive to the right. -double theta(double x, double y) +static double theta(double x, double y) { double theta = atan(x/fabs(y)); if (y>0) { @@ -387,7 +386,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). -int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter) { +static int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter) { if (line[*char_counter] == 0) { return(0); // No more statements } diff --git a/planner.c b/planner.c index 4054f58..45a9526 100644 --- a/planner.c +++ b/planner.c @@ -47,7 +47,7 @@ static uint8_t acceleration_manager_enabled; // Acceleration management active // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // given acceleration: -double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) { +static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) { return( (target_rate*target_rate-initial_rate*initial_rate)/ (2L*acceleration) @@ -70,7 +70,7 @@ double estimate_acceleration_distance(double initial_rate, double target_rate, d // a total travel of distance. This can be used to compute the intersection point between acceleration and // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) -double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) { +static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) { return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ (4*acceleration) @@ -89,7 +89,7 @@ double intersection_distance(double initial_rate, double final_rate, double acce // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. // The factors represent a factor of braking and must be in the range 0.0-1.0. -void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { +static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { block->initial_rate = ceil(block->nominal_rate*entry_factor); block->final_rate = ceil(block->nominal_rate*exit_factor); int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; @@ -116,7 +116,7 @@ void calculate_trapezoid_for_block(block_t *block, double entry_factor, double e // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // acceleration within the allotted distance. -double max_allowable_speed(double acceleration, double target_velocity, double distance) { +static double max_allowable_speed(double acceleration, double target_velocity, double distance) { return( sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) ); @@ -125,7 +125,7 @@ double max_allowable_speed(double acceleration, double target_velocity, double d // "Junction jerk" in this context is the immediate change in speed at the junction of two blocks. // This method will calculate the junction jerk as the euclidean distance between the nominal // velocities of the respective blocks. -double junction_jerk(block_t *before, block_t *after) { +static double junction_jerk(block_t *before, block_t *after) { return(sqrt( pow(before->speed_x-after->speed_x, 2)+ pow(before->speed_y-after->speed_y, 2)+ @@ -135,7 +135,7 @@ double junction_jerk(block_t *before, block_t *after) { // Calculate a braking factor to reach baseline speed which is max_jerk/2, e.g. the // speed under which you cannot exceed max_jerk no matter what you do. -double factor_for_safe_speed(block_t *block) { +static double factor_for_safe_speed(block_t *block) { if(settings.max_jerk < block->nominal_speed) { return(settings.max_jerk/block->nominal_speed); } else { @@ -144,7 +144,7 @@ double factor_for_safe_speed(block_t *block) { } // The kernel called by planner_recalculate() when scanning the plan from last to first entry. -void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { +static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { if(!current) { return; } double entry_factor = 1.0; @@ -181,7 +181,7 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This // implements the reverse pass. -void planner_reverse_pass() { +static void planner_reverse_pass() { auto int8_t block_index = block_buffer_head; block_t *block[3] = {NULL, NULL, NULL}; while(block_index != block_buffer_tail) { @@ -198,7 +198,7 @@ void planner_reverse_pass() { } // The kernel called by planner_recalculate() when scanning the plan from first to last entry. -void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) { +static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) { if(!current) { return; } // If the previous block is an acceleration block, but it is not long enough to // complete the full speed change within the block, we need to adjust out entry @@ -216,7 +216,7 @@ void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *n // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This // implements the forward pass. -void planner_forward_pass() { +static void planner_forward_pass() { int8_t block_index = block_buffer_tail; block_t *block[3] = {NULL, NULL, NULL}; @@ -233,7 +233,7 @@ void planner_forward_pass() { // Recalculates the trapezoid speed profiles for all blocks in the plan according to the // entry_factor for each junction. Must be called by planner_recalculate() after // updating the blocks. -void planner_recalculate_trapezoids() { +static void planner_recalculate_trapezoids() { int8_t block_index = block_buffer_tail; block_t *current; block_t *next = NULL; @@ -266,7 +266,7 @@ void planner_recalculate_trapezoids() { // // 3. Recalculate trapezoids for all blocks. -void planner_recalculate() { +static void planner_recalculate() { planner_reverse_pass(); planner_forward_pass(); planner_recalculate_trapezoids(); diff --git a/protocol.c b/protocol.c index aaed597..083fcac 100644 --- a/protocol.c +++ b/protocol.c @@ -32,7 +32,7 @@ static char line[LINE_BUFFER_SIZE]; static uint8_t char_counter; -void status_message(int status_code) { +static void status_message(int status_code) { if (status_code == 0) { printPgmString(PSTR("ok\n\r")); } else { diff --git a/stepper.c b/stepper.c index 964082d..640ab8c 100644 --- a/stepper.c +++ b/stepper.c @@ -80,7 +80,7 @@ static uint32_t trapezoid_adjusted_rate; // The current rate of step_events // The slope of acceleration is always +/- block->rate_delta and is applied at a constant rate by trapezoid_generator_tick() // that is called ACCELERATION_TICKS_PER_SECOND times per second. -void set_step_events_per_minute(uint32_t steps_per_minute); +static void set_step_events_per_minute(uint32_t steps_per_minute); void st_wake_up() { ENABLE_STEPPER_DRIVER_INTERRUPT(); @@ -88,7 +88,7 @@ void st_wake_up() { // Initializes the trapezoid generator from the current block. Called whenever a new // block begins. -void trapezoid_generator_reset() { +static void trapezoid_generator_reset() { trapezoid_adjusted_rate = current_block->initial_rate; trapezoid_tick_cycle_counter = 0; // Always start a new trapezoid with a full acceleration tick set_step_events_per_minute(trapezoid_adjusted_rate); @@ -97,7 +97,7 @@ void trapezoid_generator_reset() { // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event // interrupt. It can be assumed that the trapezoid-generator-parameters and the // current_block stays untouched by outside handlers for the duration of this function call. -void trapezoid_generator_tick() { +static void trapezoid_generator_tick() { if (current_block) { if (step_events_completed < current_block->accelerate_until) { trapezoid_adjusted_rate += current_block->rate_delta; @@ -248,7 +248,7 @@ void st_synchronize() // Configures the prescaler and ceiling of timer 1 to produce the given rate as accurately as possible. // Returns the actual number of cycles per interrupt -uint32_t config_step_timer(uint32_t cycles) +static uint32_t config_step_timer(uint32_t cycles) { uint16_t ceiling; uint16_t prescaler; @@ -286,7 +286,7 @@ uint32_t config_step_timer(uint32_t cycles) return(actual_cycles); } -void set_step_events_per_minute(uint32_t steps_per_minute) { +static void set_step_events_per_minute(uint32_t steps_per_minute) { if (steps_per_minute < MINIMUM_STEPS_PER_MINUTE) { steps_per_minute = MINIMUM_STEPS_PER_MINUTE; } cycles_per_step_event = config_step_timer((TICKS_PER_MICROSECOND*1000000*60)/steps_per_minute); } From d5d6298de3f5acddd885277671576410e08bd846 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sun, 20 Feb 2011 00:29:56 +0100 Subject: [PATCH 14/82] added support for limit switches and homing action --- Makefile | 2 +- config.h | 2 +- gcode.c | 2 +- limits.c | 103 +++++++++++++++++++++++++++++++++++++++++++++++++++ limits.h | 30 +++++++++++++++ main.c | 4 +- nuts_bolts.h | 1 + planner.c | 7 ++++ planner.h | 3 ++ stepper.c | 7 ++-- stepper.h | 5 +++ 11 files changed, 158 insertions(+), 8 deletions(-) create mode 100644 limits.c create mode 100644 limits.h diff --git a/Makefile b/Makefile index ae641fc..65a33c2 100644 --- a/Makefile +++ b/Makefile @@ -31,7 +31,7 @@ DEVICE = atmega328p CLOCK = 16000000 PROGRAMMER = -c avrisp2 -P usb OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o protocol.o stepper.o \ - eeprom.o settings.o planner.o nuts_bolts.o + eeprom.o settings.o planner.o nuts_bolts.o limits.o # FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m FUSES = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m # update that line with this when programmer is back up: diff --git a/config.h b/config.h index 1f2e84b..3a82e46 100644 --- a/config.h +++ b/config.h @@ -40,7 +40,7 @@ #define Z_DIRECTION_BIT 7 #define LIMIT_DDR DDRB -#define LIMIT_PORT PORTB +#define LIMIT_PIN PINB #define X_LIMIT_BIT 1 #define Y_LIMIT_BIT 2 #define Z_LIMIT_BIT 3 diff --git a/gcode.c b/gcode.c index c5e656e..48dc64a 100644 --- a/gcode.c +++ b/gcode.c @@ -231,7 +231,7 @@ uint8_t gc_execute_line(char *line) { // Perform any physical actions switch (next_action) { - case NEXT_ACTION_GO_HOME: mc_go_home(); break; + case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break; case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break; case NEXT_ACTION_DEFAULT: switch (gc.motion_mode) { diff --git a/limits.c b/limits.c new file mode 100644 index 0000000..6444c8d --- /dev/null +++ b/limits.c @@ -0,0 +1,103 @@ +/* + limits.h - code pertaining to limit-switches and performing the homing cycle + Part of Grbl + + 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 +#include "stepper.h" +#include "settings.h" +#include "nuts_bolts.h" +#include "config.h" + +void limits_init() { + LIMIT_DDR &= ~(LIMIT_MASK); +} + +static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, bool reverse_direction, uint32_t microseconds_per_pulse) { + // First home the Z axis + uint32_t step_delay = microseconds_per_pulse - settings.pulse_microseconds; + uint8_t out_bits = DIRECTION_MASK; + uint8_t limit_bits; + + if (x_axis) { out_bits |= (1<. +*/ + +#ifndef limits_h +#define limits_h + +// initialize the limits module +void limits_init(); + +// perform the homing cycle +void limits_go_home(); + +#endif \ No newline at end of file diff --git a/main.c b/main.c index 5179c6b..a6847d3 100644 --- a/main.c +++ b/main.c @@ -27,6 +27,7 @@ #include "motion_control.h" #include "gcode.h" #include "protocol.h" +#include "limits.h" #include "settings.h" #include "wiring_serial.h" @@ -42,7 +43,8 @@ int main(void) plan_init(); st_init(); spindle_init(); - gc_init(); + gc_init(); + limits_init(); for(;;){ sleep_mode(); // Wait for it ... diff --git a/nuts_bolts.h b/nuts_bolts.h index bc0b892..e5a2ea8 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -22,6 +22,7 @@ #define nuts_bolts_h #include #include +#include #define false 0 #define true 1 diff --git a/planner.c b/planner.c index 45a9526..bcc2a91 100644 --- a/planner.c +++ b/planner.c @@ -386,3 +386,10 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert if (acceleration_manager_enabled) { planner_recalculate(); } st_wake_up(); } + +// Reset the position vector +void plan_set_current_position(double x, double y, double z) { + position[X_AXIS] = x; + position[Y_AXIS] = y; + position[Z_AXIS] = z; +} diff --git a/planner.h b/planner.h index ab622d5..c39e8fc 100644 --- a/planner.h +++ b/planner.h @@ -70,4 +70,7 @@ void plan_set_acceleration_manager_enabled(int enabled); // Is acceleration-management currently enabled? int plan_is_acceleration_manager_enabled(); +// Reset the position vector +void plan_set_current_position(double x, double y, double z); + #endif \ No newline at end of file diff --git a/stepper.c b/stepper.c index 640ab8c..9eb5d7d 100644 --- a/stepper.c +++ b/stepper.c @@ -31,13 +31,12 @@ #include #include "planner.h" #include "wiring_serial.h" - +#include "limits.h" // Some useful constants #define STEP_MASK ((1< #include +#define LIMIT_MASK ((1< Date: Sun, 20 Feb 2011 22:00:12 +0100 Subject: [PATCH 15/82] acceleration-Grbl now works with atmega 168 by disabling arc motion --- gcode.c | 4 ++++ main.c | 6 +++--- motion_control.c | 2 ++ motion_control.h | 2 ++ planner.c | 4 ++++ wiring_serial.c | 4 ++++ 6 files changed, 19 insertions(+), 3 deletions(-) diff --git a/gcode.c b/gcode.c index 48dc64a..cd4e504 100644 --- a/gcode.c +++ b/gcode.c @@ -148,8 +148,10 @@ uint8_t gc_execute_line(char *line) { switch(int_value) { case 0: gc.motion_mode = MOTION_MODE_SEEK; break; case 1: gc.motion_mode = MOTION_MODE_LINEAR; break; +#ifdef __AVR_ATmega328P__ case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break; case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break; +#endif case 4: next_action = NEXT_ACTION_DWELL; break; case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break; case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break; @@ -243,6 +245,7 @@ uint8_t gc_execute_line(char *line) { 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); break; +#ifdef __AVR_ATmega328P__ case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: if (radius_mode) { /* @@ -373,6 +376,7 @@ uint8_t gc_execute_line(char *line) { 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); break; +#endif } } diff --git a/main.c b/main.c index a6847d3..60349cf 100644 --- a/main.c +++ b/main.c @@ -32,9 +32,9 @@ #include "settings.h" #include "wiring_serial.h" -#ifndef __AVR_ATmega328P__ -# error "As of version 0.6 Grbl only supports atmega328p. If you want to run Grbl on an 168 check out 0.51 ('git co v0_51')" -#endif +// #ifndef __AVR_ATmega328P__ +// # error "As of version 0.6 Grbl only supports atmega328p. If you want to run Grbl on an 168 check out 0.51 ('git co v0_51')" +// #endif int main(void) { diff --git a/motion_control.c b/motion_control.c index f799cca..ebef597 100644 --- a/motion_control.c +++ b/motion_control.c @@ -42,6 +42,7 @@ void mc_dwell(uint32_t milliseconds) // axis in axis_l which will be the axis for linear travel if you are tracing a helical motion. // position is a pointer to a vector representing the current position in millimeters. +#ifdef __AVR_ATmega328P__ // The arc is approximated by generating a huge number of tiny, linear segments. The length of each // segment is configured in settings.mm_per_arc_segment. void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, @@ -77,6 +78,7 @@ void mc_arc(double theta, double angular_travel, double radius, double linear_tr } plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled); } +#endif void mc_go_home() { diff --git a/motion_control.h b/motion_control.h index c205e2f..89020ad 100644 --- a/motion_control.h +++ b/motion_control.h @@ -32,6 +32,7 @@ #define mc_line(x, y, z, feed_rate, invert_feed_rate) plan_buffer_line(x, y, z, feed_rate, invert_feed_rate) +#ifdef __AVR_ATmega328P__ // Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc, // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the // circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining @@ -39,6 +40,7 @@ void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, int axis_linear, double feed_rate, int invert_feed_rate, double *position); +#endif // Dwell for a couple of time units void mc_dwell(uint32_t milliseconds); diff --git a/planner.c b/planner.c index bcc2a91..af75a06 100644 --- a/planner.c +++ b/planner.c @@ -32,7 +32,11 @@ #include "wiring_serial.h" // The number of linear motions that can be in the plan at any give time +#ifdef __AVR_ATmega328P__ #define BLOCK_BUFFER_SIZE 16 +#else +#define BLOCK_BUFFER_SIZE 5 +#endif static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions static volatile uint8_t block_buffer_head; // Index of the next block to be pushed diff --git a/wiring_serial.c b/wiring_serial.c index 5109d39..3dd67b7 100644 --- a/wiring_serial.c +++ b/wiring_serial.c @@ -31,7 +31,11 @@ // using a ring buffer (I think), in which rx_buffer_head is the index of the // location to which to write the next incoming character and rx_buffer_tail // is the index of the location from which to read. +#ifdef __AVR_ATmega328P__ #define RX_BUFFER_SIZE 256 +#else +#define RX_BUFFER_SIZE 64 +#endif unsigned char rx_buffer[RX_BUFFER_SIZE]; From 68ff56a8b1f81e1a900a1f75434e084997912b42 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Mon, 21 Feb 2011 10:00:16 +0100 Subject: [PATCH 16/82] stepper enable pin now toggles to reflect the active/passive state of the stepper subsystem --- stepper.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/stepper.c b/stepper.c index 9eb5d7d..f011a6a 100644 --- a/stepper.c +++ b/stepper.c @@ -82,6 +82,7 @@ static uint32_t trapezoid_adjusted_rate; // The current rate of step_events static void set_step_events_per_minute(uint32_t steps_per_minute); void st_wake_up() { + STEPPERS_ENABLE_PORT |= (1< Date: Mon, 21 Feb 2011 22:26:02 +0100 Subject: [PATCH 17/82] added support for G92 --- gcode.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gcode.c b/gcode.c index cd4e504..ce762e9 100644 --- a/gcode.c +++ b/gcode.c @@ -36,6 +36,7 @@ #define NEXT_ACTION_DEFAULT 0 #define NEXT_ACTION_DWELL 1 #define NEXT_ACTION_GO_HOME 2 +#define NEXT_ACTION_SET_COORDINATE_OFFSET 3 #define MOTION_MODE_SEEK 0 // G0 #define MOTION_MODE_LINEAR 1 // G1 @@ -234,7 +235,8 @@ uint8_t gc_execute_line(char *line) { // Perform any physical actions switch (next_action) { case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break; - case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break; + case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break; + case NEXT_ACTION_SET_COORDINATE_OFFSET: break; // no action needed case NEXT_ACTION_DEFAULT: switch (gc.motion_mode) { case MOTION_MODE_CANCEL: break; From ef20be9f4c819a3e8893abcc92b1fdf3af286e05 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Mon, 21 Feb 2011 22:32:42 +0100 Subject: [PATCH 18/82] result of G92 also affects planner --- gcode.c | 6 +++++- motion_control.h | 8 +++++--- planner.c | 8 ++++---- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/gcode.c b/gcode.c index ce762e9..4e0278d 100644 --- a/gcode.c +++ b/gcode.c @@ -97,6 +97,7 @@ static float to_millimeters(double value) { return(gc.inches_mode ? (value * MM_PER_INCH) : value); } +#ifdef __AVR_ATmega328P__ // Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis, // positive to the right. static double theta(double x, double y) @@ -113,6 +114,7 @@ static double theta(double x, double y) } } } +#endif // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase // characters and signed floating point values (no whitespace). @@ -236,7 +238,9 @@ uint8_t gc_execute_line(char *line) { switch (next_action) { case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break; case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break; - case NEXT_ACTION_SET_COORDINATE_OFFSET: break; // no action needed + case NEXT_ACTION_SET_COORDINATE_OFFSET: + mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]); + break; case NEXT_ACTION_DEFAULT: switch (gc.motion_mode) { case MOTION_MODE_CANCEL: break; diff --git a/motion_control.h b/motion_control.h index 89020ad..a66123e 100644 --- a/motion_control.h +++ b/motion_control.h @@ -24,14 +24,16 @@ #include #include "planner.h" +// NOTE: Although the following functions structurally belongs in this module, there is nothing to do but +// to forward the request to the planner. + // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. -// NOTE: Although this function structurally belongs in this module, there is nothing to do but -// to forward the request to the planner. For efficiency the function is implemented with a macro. - #define mc_line(x, y, z, feed_rate, invert_feed_rate) plan_buffer_line(x, y, z, feed_rate, invert_feed_rate) +#define mc_set_current_position(x, y, z) plan_set_current_position(x, y, z) + #ifdef __AVR_ATmega328P__ // Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc, // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the diff --git a/planner.c b/planner.c index af75a06..2859881 100644 --- a/planner.c +++ b/planner.c @@ -391,9 +391,9 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert st_wake_up(); } -// Reset the position vector +// Reset the planner position vector void plan_set_current_position(double x, double y, double z) { - position[X_AXIS] = x; - position[Y_AXIS] = y; - position[Z_AXIS] = z; + position[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); + position[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); + position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); } From 60f417b57021d5d72ca1fd3cb8fa936b842f96e0 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Wed, 23 Feb 2011 20:06:55 +0100 Subject: [PATCH 19/82] =?UTF-8?q?fixed=20a=20double=20rounding=20error=20c?= =?UTF-8?q?ompensation=20bug=20in=20mc=5Farc=20thanks=20to=20Etienne=20Cho?= =?UTF-8?q?v=C3=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- motion_control.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/motion_control.c b/motion_control.c index ebef597..e51d519 100644 --- a/motion_control.c +++ b/motion_control.c @@ -52,7 +52,7 @@ void mc_arc(double theta, double angular_travel, double radius, double linear_tr plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel)); if (millimeters_of_travel == 0.0) { return; } - uint16_t segments = ceil(millimeters_of_travel/settings.mm_per_arc_segment); + uint16_t segments = round(millimeters_of_travel/settings.mm_per_arc_segment); // Multiply inverse feed_rate to compensate for the fact that this movement is approximated // by a number of discrete segments. The inverse feed_rate should be correct for the sum of // all segments. @@ -69,7 +69,7 @@ void mc_arc(double theta, double angular_travel, double radius, double linear_tr int i; // Initialize the linear axis target[axis_linear] = position[axis_linear]; - for (i=0; i<=segments; i++) { + for (i=0; i Date: Thu, 24 Feb 2011 15:03:45 +0100 Subject: [PATCH 20/82] added missing line from G92 patch --- gcode.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gcode.c b/gcode.c index 4e0278d..af91fce 100644 --- a/gcode.c +++ b/gcode.c @@ -163,9 +163,10 @@ uint8_t gc_execute_line(char *line) { case 21: gc.inches_mode = false; break; case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break; case 53: absolute_override = true; break; - case 80: gc.motion_mode = MOTION_MODE_CANCEL; break; + case 80: gc.motion_mode = MOTION_MODE_CANCEL; break; case 90: gc.absolute_mode = true; break; case 91: gc.absolute_mode = false; break; + case 92: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break; case 93: gc.inverse_feed_rate_mode = true; break; case 94: gc.inverse_feed_rate_mode = false; break; default: FAIL(STATUS_UNSUPPORTED_STATEMENT); From a18a89c77962d2dded0edb42ad98a4167fbb230b Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Thu, 24 Feb 2011 16:08:06 +0100 Subject: [PATCH 21/82] fixed linebreaks, was LFCR, is CRLF --- protocol.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/protocol.c b/protocol.c index 083fcac..611ec51 100644 --- a/protocol.c +++ b/protocol.c @@ -34,21 +34,21 @@ static uint8_t char_counter; static void status_message(int status_code) { if (status_code == 0) { - printPgmString(PSTR("ok\n\r")); + printPgmString(PSTR("ok\r\n")); } else { printPgmString(PSTR("error: ")); switch(status_code) { case STATUS_BAD_NUMBER_FORMAT: - printPgmString(PSTR("Bad number format\n\r")); break; + printPgmString(PSTR("Bad number format\r\n")); break; case STATUS_EXPECTED_COMMAND_LETTER: - printPgmString(PSTR("Expected command letter\n\r")); break; + printPgmString(PSTR("Expected command letter\r\n")); break; case STATUS_UNSUPPORTED_STATEMENT: - printPgmString(PSTR("Unsupported statement\n\r")); break; + printPgmString(PSTR("Unsupported statement\r\n")); break; case STATUS_FLOATING_POINT_ERROR: - printPgmString(PSTR("Floating point error\n\r")); break; + printPgmString(PSTR("Floating point error\r\n")); break; default: printInteger(status_code); - printPgmString(PSTR("\n\r")); + printPgmString(PSTR("\r\n")); } } } From 4cac11ec86415cb364c8a15701c96e948cf5996d Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 25 Feb 2011 13:30:03 +0100 Subject: [PATCH 22/82] fixed synchronization problem with spindle_control --- spindle_control.c | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/spindle_control.c b/spindle_control.c index 548e477..34d559b 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -25,24 +25,28 @@ #include +static int current_direction; + void spindle_init() { - SPINDLE_ENABLE_DDR |= 1<= 0) { - SPINDLE_DIRECTION_PORT &= ~(1<= 0) { + SPINDLE_DIRECTION_PORT &= ~(1< Date: Fri, 25 Feb 2011 13:40:48 +0100 Subject: [PATCH 23/82] foolproofed steps/mm setting (must be > 0) --- settings.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/settings.c b/settings.c index ca1bc05..a172795 100644 --- a/settings.c +++ b/settings.c @@ -139,6 +139,10 @@ int read_settings() { void settings_store_setting(int parameter, double value) { switch(parameter) { case 0: case 1: case 2: + if (value <= 0.0) { + printPgmString(PSTR("Steps/mm must be > 0.0\r\n")); + return; + } settings.steps_per_mm[parameter] = value; break; case 3: settings.pulse_microseconds = round(value); break; case 4: settings.default_feed_rate = value; break; From d914089e91c92ab2f48f755eb1b19feeffb68149 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 25 Feb 2011 15:00:47 +0100 Subject: [PATCH 24/82] spindle could not be stopped. Thanks again to Etienne for reporting --- spindle_control.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/spindle_control.c b/spindle_control.c index 34d559b..0321c89 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -36,12 +36,16 @@ void spindle_run(int direction, uint32_t rpm) { if (direction != current_direction) { st_synchronize(); - if(direction >= 0) { - SPINDLE_DIRECTION_PORT &= ~(1< 0) { + SPINDLE_DIRECTION_PORT &= ~(1< Date: Fri, 25 Feb 2011 15:02:55 +0100 Subject: [PATCH 25/82] simplification in gcode.c also thanks to etienne --- gcode.c | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/gcode.c b/gcode.c index af91fce..be62ce8 100644 --- a/gcode.c +++ b/gcode.c @@ -229,11 +229,7 @@ uint8_t gc_execute_line(char *line) { if (gc.status_code) { return(gc.status_code); } // Update spindle state - if (gc.spindle_direction) { - spindle_run(gc.spindle_direction, gc.spindle_speed); - } else { - spindle_stop(); - } + spindle_run(gc.spindle_direction, gc.spindle_speed); // Perform any physical actions switch (next_action) { From c2aec12004be91b97e58933eb26d9c3a594c602e Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Tue, 31 May 2011 13:08:42 +0200 Subject: [PATCH 26/82] converted the STEPPER_ENABLE_PIN to a STEPPER_DISABLE_PIN as per the request of Alden Hart of Grbl Shield fame. --- config.h | 12 ++++++------ main.c | 5 ++++- stepper.c | 30 ++++++++++++++++-------------- 3 files changed, 26 insertions(+), 21 deletions(-) diff --git a/config.h b/config.h index 3a82e46..e2811cd 100644 --- a/config.h +++ b/config.h @@ -26,9 +26,9 @@ // Updated default pin-assignments from 0.6 onwards // (see bottom of file for a copy of the old config) -#define STEPPERS_ENABLE_DDR DDRB -#define STEPPERS_ENABLE_PORT PORTB -#define STEPPERS_ENABLE_BIT 0 +#define STEPPERS_DISABLE_DDR DDRB +#define STEPPERS_DISABLE_PORT PORTB +#define STEPPERS_DISABLE_BIT 0 #define STEPPING_DDR DDRD #define STEPPING_PORT PORTD @@ -61,9 +61,9 @@ // Pin-assignments from Grbl 0.5 -// #define STEPPERS_ENABLE_DDR DDRD -// #define STEPPERS_ENABLE_PORT PORTD -// #define STEPPERS_ENABLE_BIT 2 +// #define STEPPERS_DISABLE_DDR DDRD +// #define STEPPERS_DISABLE_PORT PORTD +// #define STEPPERS_DISABLE_BIT 2 // // #define STEPPING_DDR DDRC // #define STEPPING_PORT PORTC diff --git a/main.c b/main.c index 60349cf..e156a14 100644 --- a/main.c +++ b/main.c @@ -20,6 +20,7 @@ #include #include +#include #include #include "planner.h" #include "stepper.h" @@ -44,7 +45,9 @@ int main(void) st_init(); spindle_init(); gc_init(); - limits_init(); + limits_init(); + + sei(); for(;;){ sleep_mode(); // Wait for it ... diff --git a/stepper.c b/stepper.c index f011a6a..802f8ac 100644 --- a/stepper.c +++ b/stepper.c @@ -43,9 +43,6 @@ #define MINIMUM_STEPS_PER_MINUTE 1200 // The stepper subsystem will never run slower than this, exept when sleeping -#define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1< Date: Tue, 31 May 2011 13:13:45 +0200 Subject: [PATCH 27/82] made inverted stepper driving the default as per the request of the Grbl Shield team and frankly myself. Who doesn't need their stepper signals inverted anyway? --- settings.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/settings.c b/settings.c index a172795..78bdec2 100644 --- a/settings.c +++ b/settings.c @@ -26,6 +26,7 @@ #include "wiring_serial.h" #include #include "protocol.h" +#include "config.h" settings_t settings; @@ -51,7 +52,7 @@ typedef struct { #define DEFAULT_FEEDRATE 500.0 #define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/10.0) #define DEFAULT_MAX_JERK 300.0 -#define DEFAULT_STEPPING_INVERT_MASK 0 +#define DEFAULT_STEPPING_INVERT_MASK ((1< Date: Tue, 31 May 2011 18:37:37 +0200 Subject: [PATCH 28/82] added a fix for division by zero errors that would occur on very low feed rates (thanks to Arthur Wolf for hardcore investigation) --- planner.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planner.c b/planner.c index 2859881..e49cd72 100644 --- a/planner.c +++ b/planner.c @@ -354,6 +354,10 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert block->nominal_rate = ceil(block->step_event_count * multiplier); block->entry_factor = 0.0; + // This is a temporary fix to avoid a situation where very low nominal_speeds would be rounded + // down to zero and cause a division by zero. TODO: Grbl deserves a less patchy fix for this problem + if (block->nominal_speed < 60.0) { block->nominal_speed = 60.0; } + // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line // average travel per step event changes. For a line along one axis the travel per step event // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both From 33a940cdf847e111d9dcb908973812ebdfdbc29f Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Tue, 31 May 2011 22:29:50 +0200 Subject: [PATCH 29/82] added asynchronous write as provided by Arthur Wolf --- wiring_serial.c | 62 ++++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 56 insertions(+), 6 deletions(-) diff --git a/wiring_serial.c b/wiring_serial.c index 3dd67b7..9a8cb32 100644 --- a/wiring_serial.c +++ b/wiring_serial.c @@ -37,10 +37,12 @@ #define RX_BUFFER_SIZE 64 #endif +#define TX_BUFFER_SIZE 16 + unsigned char rx_buffer[RX_BUFFER_SIZE]; -uint8_t rx_buffer_head = 0; -uint8_t rx_buffer_tail = 0; +int rx_buffer_head = 0; +int rx_buffer_tail = 0; void beginSerial(long baud) { @@ -60,12 +62,60 @@ void beginSerial(long baud) // defaults to 8-bit, no parity, 1 stop bit } -void serialWrite(unsigned char c) -{ - while (!(UCSR0A & (1 << UDRE0))) - ; +unsigned char tx_buffer[TX_BUFFER_SIZE]; +unsigned char tx_buffer_head = 0; +volatile unsigned char tx_buffer_tail = 0; + +void serialWrite(unsigned char c) { + + if ((!(UCSR0A & (1 << UDRE0))) || (tx_buffer_head != tx_buffer_tail)) { + // maybe checking if buffer is empty is not necessary, + // not sure if there can be a state when the data register empty flag is set + // and read here without the interrupt being executed + // well, it shouldn't happen, right? + + // data register is not empty, use the buffer + unsigned char newhead = tx_buffer_head + 1; + newhead %= TX_BUFFER_SIZE; + + // wait until there's a space in the buffer + while (newhead == tx_buffer_tail) ; + + tx_buffer[tx_buffer_head] = c; + tx_buffer_head = newhead; + + // enable the Data Register Empty Interrupt + sei(); + UCSR0B |= (1 << UDRIE0); + + } + else { UDR0 = c; + } +} + +// interrupt called on Data Register Empty +SIGNAL(USART_UDRE_vect) { + // temporary tx_buffer_tail + // (to optimize for volatile, there are no interrupts inside an interrupt routine) + unsigned char tail = tx_buffer_tail; + + // get a byte from the buffer + unsigned char c = tx_buffer[tail]; + // send the byte + UDR0 = c; + + // update tail position + tail ++; + tail %= TX_BUFFER_SIZE; + + // if the buffer is empty, disable the interrupt + if (tail == tx_buffer_head) { + UCSR0B &= ~(1 << UDRIE0); + } + + tx_buffer_tail = tail; } int serialAvailable() From defabc80ed8c42ba9ec57417dd83f736564cfab8 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Tue, 31 May 2011 22:45:38 +0200 Subject: [PATCH 30/82] renamed wiring_serial to serial to reflect its complete remake --- Makefile | 2 +- doc/structure.txt | 2 +- main.c | 2 +- motion_control.c | 2 +- planner.c | 2 +- protocol.c | 2 +- protocol.h | 4 ++-- wiring_serial.c => serial.c | 4 +--- wiring_serial.h => serial.h | 4 ++-- settings.c | 2 +- stepper.c | 2 +- 11 files changed, 13 insertions(+), 15 deletions(-) rename wiring_serial.c => serial.c (98%) rename wiring_serial.h => serial.h (97%) diff --git a/Makefile b/Makefile index 65a33c2..e9c2c99 100644 --- a/Makefile +++ b/Makefile @@ -30,7 +30,7 @@ DEVICE = atmega328p CLOCK = 16000000 PROGRAMMER = -c avrisp2 -P usb -OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o protocol.o stepper.o \ +OBJECTS = main.o motion_control.o gcode.o spindle_control.o serial.o protocol.o stepper.o \ eeprom.o settings.o planner.o nuts_bolts.o limits.o # FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m FUSES = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m diff --git a/doc/structure.txt b/doc/structure.txt index f87feb3..51e8dee 100644 --- a/doc/structure.txt +++ b/doc/structure.txt @@ -36,4 +36,4 @@ Supporting files: 'nuts_bolts.h' : A tiny collection of useful constants and macros used everywhere -'wiring_serial' : Low level serial library initially from an old version of the Arduino software \ No newline at end of file +'serial' : Low level serial communications \ No newline at end of file diff --git a/main.c b/main.c index e156a14..1951171 100644 --- a/main.c +++ b/main.c @@ -31,7 +31,7 @@ #include "limits.h" #include "settings.h" -#include "wiring_serial.h" +#include "serial.h" // #ifndef __AVR_ATmega328P__ // # error "As of version 0.6 Grbl only supports atmega328p. If you want to run Grbl on an 168 check out 0.51 ('git co v0_51')" diff --git a/motion_control.c b/motion_control.c index e51d519..170499e 100644 --- a/motion_control.c +++ b/motion_control.c @@ -27,7 +27,7 @@ #include "nuts_bolts.h" #include "stepper.h" #include "planner.h" -#include "wiring_serial.h" +#include "serial.h" void mc_dwell(uint32_t milliseconds) diff --git a/planner.c b/planner.c index e49cd72..d7504cb 100644 --- a/planner.c +++ b/planner.c @@ -29,7 +29,7 @@ #include "stepper.h" #include "settings.h" #include "config.h" -#include "wiring_serial.h" +#include "serial.h" // The number of linear motions that can be in the plan at any give time #ifdef __AVR_ATmega328P__ diff --git a/protocol.c b/protocol.c index 611ec51..c64280e 100644 --- a/protocol.c +++ b/protocol.c @@ -21,7 +21,7 @@ #include #include "protocol.h" #include "gcode.h" -#include "wiring_serial.h" +#include "serial.h" #include "settings.h" #include "config.h" #include diff --git a/protocol.h b/protocol.h index 7fa7704..3ad6597 100644 --- a/protocol.h +++ b/protocol.h @@ -17,8 +17,8 @@ You should have received a copy of the GNU General Public License along with Grbl. If not, see . */ -#ifndef serial_h -#define serial_h +#ifndef protocol_h +#define protocol_h #define STATUS_OK 0 #define STATUS_BAD_NUMBER_FORMAT 1 diff --git a/wiring_serial.c b/serial.c similarity index 98% rename from wiring_serial.c rename to serial.c index 9a8cb32..c10d4ad 100644 --- a/wiring_serial.c +++ b/serial.c @@ -1,5 +1,5 @@ /* - wiring_serial.c - serial functions. + serial.c - serial functions. Part of Arduino - http://www.arduino.cc/ Copyright (c) 2005-2006 David A. Mellis @@ -19,10 +19,8 @@ Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - $Id: wiring.c 248 2007-02-03 15:36:30Z mellis $ */ -//#include "wiring_private.h" #include #include #include diff --git a/wiring_serial.h b/serial.h similarity index 97% rename from wiring_serial.h rename to serial.h index 516a0ea..ac64387 100644 --- a/wiring_serial.h +++ b/serial.h @@ -22,8 +22,8 @@ $Id: wiring.h 387 2008-03-08 21:30:00Z mellis $ */ -#ifndef wiring_h -#define wiring_h +#ifndef serial_h +#define serial_h void beginSerial(long); void serialWrite(unsigned char); diff --git a/settings.c b/settings.c index 78bdec2..7782627 100644 --- a/settings.c +++ b/settings.c @@ -23,7 +23,7 @@ #include "nuts_bolts.h" #include "settings.h" #include "eeprom.h" -#include "wiring_serial.h" +#include "serial.h" #include #include "protocol.h" #include "config.h" diff --git a/stepper.c b/stepper.c index 802f8ac..a145475 100644 --- a/stepper.c +++ b/stepper.c @@ -30,7 +30,7 @@ #include "nuts_bolts.h" #include #include "planner.h" -#include "wiring_serial.h" +#include "serial.h" #include "limits.h" // Some useful constants From ee3139d283219484d32f779a79310ae3b491f901 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Wed, 1 Jun 2011 09:36:15 +0200 Subject: [PATCH 31/82] changed serialAvailable to serialAnyAvailable which does not calculate the number of bytes, only if there are any at all --- serial.c | 37 ++++--------------------------------- serial.h | 2 +- 2 files changed, 5 insertions(+), 34 deletions(-) diff --git a/serial.c b/serial.c index c10d4ad..6c5fdcb 100644 --- a/serial.c +++ b/serial.c @@ -116,15 +116,16 @@ SIGNAL(USART_UDRE_vect) { tx_buffer_tail = tail; } -int serialAvailable() +// Returns true if there is any data in the read buffer +int serialAnyAvailable() { - return (RX_BUFFER_SIZE + rx_buffer_head - rx_buffer_tail) % RX_BUFFER_SIZE; + return (rx_buffer_head != rx_buffer_tail); } int serialRead() { // if the head isn't ahead of the tail, we don't have any characters - if (rx_buffer_head == rx_buffer_tail) { + if (serialAnyAvailable()) { return -1; } else { unsigned char c = rx_buffer[rx_buffer_tail]; @@ -227,33 +228,3 @@ void printFloat(double n) printInteger(round(fractional_part*1000)); } -// void printHex(unsigned long n) -// { -// printIntegerInBase(n, 16); -// } -// -// void printOctal(unsigned long n) -// { -// printIntegerInBase(n, 8); -// } -// -// void printBinary(unsigned long n) -// { -// printIntegerInBase(n, 2); -// } - -/* Including print() adds approximately 1500 bytes to the binary size, - * so we replace it with the smaller and less-confusing printString(), - * printInteger(), etc. -void print(const char *format, ...) -{ - char buf[256]; - va_list ap; - - va_start(ap, format); - vsnprintf(buf, 256, format, ap); - va_end(ap); - - printString(buf); -} -*/ diff --git a/serial.h b/serial.h index ac64387..8b528ac 100644 --- a/serial.h +++ b/serial.h @@ -27,7 +27,7 @@ void beginSerial(long); void serialWrite(unsigned char); -int serialAvailable(void); +int serialAnyAvailable(void); int serialRead(void); void serialFlush(void); void printMode(int); From 8793b555e0ed0352fe2451b173a365b3fc4d4158 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Wed, 1 Jun 2011 09:45:15 +0200 Subject: [PATCH 32/82] cleaned up data types in serial module, all data now uint8_t --- serial.c | 93 +++++++++++++++++--------------------------------------- serial.h | 5 ++- 2 files changed, 30 insertions(+), 68 deletions(-) diff --git a/serial.c b/serial.c index 6c5fdcb..bcaa66f 100644 --- a/serial.c +++ b/serial.c @@ -37,10 +37,14 @@ #define TX_BUFFER_SIZE 16 -unsigned char rx_buffer[RX_BUFFER_SIZE]; +uint8_t rx_buffer[RX_BUFFER_SIZE]; +uint8_t rx_buffer_head = 0; +uint8_t rx_buffer_tail = 0; + +uint8_t tx_buffer[TX_BUFFER_SIZE]; +uint8_t tx_buffer_head = 0; +volatile uint8_t tx_buffer_tail = 0; -int rx_buffer_head = 0; -int rx_buffer_tail = 0; void beginSerial(long baud) { @@ -60,49 +64,29 @@ void beginSerial(long baud) // defaults to 8-bit, no parity, 1 stop bit } +void serialWrite(uint8_t data) { + uint8_t next_head = (tx_buffer_head + 1) % TX_BUFFER_SIZE; -unsigned char tx_buffer[TX_BUFFER_SIZE]; -unsigned char tx_buffer_head = 0; -volatile unsigned char tx_buffer_tail = 0; + // wait until there's a space in the buffer + while (next_head == tx_buffer_tail) ; -void serialWrite(unsigned char c) { + tx_buffer[tx_buffer_head] = data; + tx_buffer_head = next_head; - if ((!(UCSR0A & (1 << UDRE0))) || (tx_buffer_head != tx_buffer_tail)) { - // maybe checking if buffer is empty is not necessary, - // not sure if there can be a state when the data register empty flag is set - // and read here without the interrupt being executed - // well, it shouldn't happen, right? - - // data register is not empty, use the buffer - unsigned char newhead = tx_buffer_head + 1; - newhead %= TX_BUFFER_SIZE; - - // wait until there's a space in the buffer - while (newhead == tx_buffer_tail) ; - - tx_buffer[tx_buffer_head] = c; - tx_buffer_head = newhead; - - // enable the Data Register Empty Interrupt - sei(); - UCSR0B |= (1 << UDRIE0); - - } - else { - UDR0 = c; - } + // enable the Data Register Empty Interrupt + UCSR0B |= (1 << UDRIE0); } // interrupt called on Data Register Empty SIGNAL(USART_UDRE_vect) { // temporary tx_buffer_tail // (to optimize for volatile, there are no interrupts inside an interrupt routine) - unsigned char tail = tx_buffer_tail; + uint8_t tail = tx_buffer_tail; // get a byte from the buffer - unsigned char c = tx_buffer[tail]; + uint8_t data = tx_buffer[tail]; // send the byte - UDR0 = c; + UDR0 = data; // update tail position tail ++; @@ -122,58 +106,37 @@ int serialAnyAvailable() return (rx_buffer_head != rx_buffer_tail); } -int serialRead() +uint8_t serialRead() { - // if the head isn't ahead of the tail, we don't have any characters - if (serialAnyAvailable()) { + if (!serialAnyAvailable()) { return -1; } else { - unsigned char c = rx_buffer[rx_buffer_tail]; + uint8_t data = rx_buffer[rx_buffer_tail]; rx_buffer_tail = (rx_buffer_tail + 1) % RX_BUFFER_SIZE; - return c; + return data; } } -void serialFlush() -{ - // don't reverse this or there may be problems if the RX interrupt - // occurs after reading the value of rx_buffer_head but before writing - // the value to rx_buffer_tail; the previous value of rx_buffer_head - // may be written to rx_buffer_tail, making it appear as if the buffer - // were full, not empty. - rx_buffer_head = rx_buffer_tail; -} - SIGNAL(USART_RX_vect) { - unsigned char c = UDR0; - int i = (rx_buffer_head + 1) % RX_BUFFER_SIZE; + uint8_t data = UDR0; + uint8_t next_head = (rx_buffer_head + 1) % RX_BUFFER_SIZE; // if we should be storing the received character into the location // just before the tail (meaning that the head would advance to the // current location of the tail), we're about to overflow the buffer // and so we don't write the character or advance the head. - if (i != rx_buffer_tail) { - rx_buffer[rx_buffer_head] = c; - rx_buffer_head = i; + if (next_head != rx_buffer_tail) { + rx_buffer[rx_buffer_head] = data; + rx_buffer_head = next_head; } } -// void printMode(int mode) -// { -// // do nothing, we only support serial printing, not lcd. -// } - void printByte(unsigned char c) { - serialWrite(c); + serialWrite((uint8_t) c); } -// void printNewline() -// { -// printByte('\n'); -// } -// void printString(const char *s) { while (*s) diff --git a/serial.h b/serial.h index 8b528ac..e6f3055 100644 --- a/serial.h +++ b/serial.h @@ -26,10 +26,9 @@ #define serial_h void beginSerial(long); -void serialWrite(unsigned char); +void serialWrite(uint8_t); int serialAnyAvailable(void); -int serialRead(void); -void serialFlush(void); +uint8_t serialRead(void); void printMode(int); void printByte(unsigned char c); void printNewline(void); From c0b4b8309ab3ca02d3431b4c689be3c45ee2f0bc Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 3 Jun 2011 15:28:14 +0200 Subject: [PATCH 33/82] cleaned up serial completing support for non blocking tx and refactoring formatting functions into a new module 'print' --- Makefile | 2 +- doc/resources.txt | 2 +- doc/structure.txt | 4 +- main.c | 6 +-- motion_control.c | 1 - planner.c | 1 - print.c | 58 +++++++++++++++++++++++ print.h | 14 ++++++ protocol.c | 4 +- serial.c | 114 ++++++++++------------------------------------ serial.h | 18 ++------ settings.c | 2 +- stepper.c | 1 - 13 files changed, 109 insertions(+), 118 deletions(-) create mode 100644 print.c create mode 100644 print.h diff --git a/Makefile b/Makefile index e9c2c99..6011ae0 100644 --- a/Makefile +++ b/Makefile @@ -31,7 +31,7 @@ DEVICE = atmega328p CLOCK = 16000000 PROGRAMMER = -c avrisp2 -P usb OBJECTS = main.o motion_control.o gcode.o spindle_control.o serial.o protocol.o stepper.o \ - eeprom.o settings.o planner.o nuts_bolts.o limits.o + eeprom.o settings.o planner.o nuts_bolts.o limits.o print.o # FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m FUSES = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m # update that line with this when programmer is back up: diff --git a/doc/resources.txt b/doc/resources.txt index 2754464..293a6f7 100644 --- a/doc/resources.txt +++ b/doc/resources.txt @@ -3,7 +3,7 @@ Allocation of AVR peripherals in Grbl See config.h for pin allocation. -The UART is handled by 'wiring_serial' and used primarily for streaming gcode +The UART is handled by 'serial' and used primarily for streaming gcode 16 bit Timer 1 and the TIMER1_COMPA interrupt is used by the 'stepper' module to handle step events diff --git a/doc/structure.txt b/doc/structure.txt index 51e8dee..4992f80 100644 --- a/doc/structure.txt +++ b/doc/structure.txt @@ -36,4 +36,6 @@ Supporting files: 'nuts_bolts.h' : A tiny collection of useful constants and macros used everywhere -'serial' : Low level serial communications \ No newline at end of file +'serial' : Low level serial communications + +'print' : Functions to print strings of different formats (using serial) \ No newline at end of file diff --git a/main.c b/main.c index 1951171..29d37da 100644 --- a/main.c +++ b/main.c @@ -22,6 +22,7 @@ #include #include #include +#include "config.h" #include "planner.h" #include "stepper.h" #include "spindle_control.h" @@ -33,12 +34,9 @@ #include "settings.h" #include "serial.h" -// #ifndef __AVR_ATmega328P__ -// # error "As of version 0.6 Grbl only supports atmega328p. If you want to run Grbl on an 168 check out 0.51 ('git co v0_51')" -// #endif - int main(void) { + serial_init(BAUD_RATE); protocol_init(); settings_init(); plan_init(); diff --git a/motion_control.c b/motion_control.c index 170499e..ef4a6b3 100644 --- a/motion_control.c +++ b/motion_control.c @@ -27,7 +27,6 @@ #include "nuts_bolts.h" #include "stepper.h" #include "planner.h" -#include "serial.h" void mc_dwell(uint32_t milliseconds) diff --git a/planner.c b/planner.c index d7504cb..28f4e2c 100644 --- a/planner.c +++ b/planner.c @@ -29,7 +29,6 @@ #include "stepper.h" #include "settings.h" #include "config.h" -#include "serial.h" // The number of linear motions that can be in the plan at any give time #ifdef __AVR_ATmega328P__ diff --git a/print.c b/print.c new file mode 100644 index 0000000..ffd3f33 --- /dev/null +++ b/print.c @@ -0,0 +1,58 @@ +#include +#include +#include "serial.h" + +void printString(const char *s) +{ + while (*s) + serial_write(*s++); +} + +// Print a string stored in PGM-memory +void printPgmString(const char *s) +{ + char c; + while ((c = pgm_read_byte_near(s++))) + serial_write(c); +} + +void printIntegerInBase(unsigned long n, unsigned long base) +{ + unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. + unsigned long i = 0; + + if (n == 0) { + serial_write('0'); + return; + } + + while (n > 0) { + buf[i++] = n % base; + n /= base; + } + + for (; i > 0; i--) + serial_write(buf[i - 1] < 10 ? + '0' + buf[i - 1] : + 'A' + buf[i - 1] - 10); +} + +void printInteger(long n) +{ + if (n < 0) { + serial_write('-'); + n = -n; + } + + printIntegerInBase(n, 10); +} + +void printFloat(double n) +{ + double integer_part, fractional_part; + fractional_part = modf(n, &integer_part); + printInteger(integer_part); + serial_write('.'); + printInteger(round(fractional_part*1000)); +} + diff --git a/print.h b/print.h new file mode 100644 index 0000000..62b76ea --- /dev/null +++ b/print.h @@ -0,0 +1,14 @@ +#ifndef print_h +#define print_h + +void printNewline(void); +void printString(const char *s); +void printPgmString(const char *s); +void printInteger(long n); +void printHex(unsigned long n); +void printOctal(unsigned long n); +void printBinary(unsigned long n); +void printIntegerInBase(unsigned long n, unsigned long base); +void printFloat(double n); + +#endif \ No newline at end of file diff --git a/protocol.c b/protocol.c index c64280e..a504d3a 100644 --- a/protocol.c +++ b/protocol.c @@ -22,6 +22,7 @@ #include "protocol.h" #include "gcode.h" #include "serial.h" +#include "print.h" #include "settings.h" #include "config.h" #include @@ -55,7 +56,6 @@ static void status_message(int status_code) { void protocol_init() { - beginSerial(BAUD_RATE); printPgmString(PSTR("\r\nGrbl " GRBL_VERSION)); printPgmString(PSTR("\r\n")); } @@ -72,7 +72,7 @@ uint8_t protocol_execute_line(char *line) { void protocol_process() { char c; - while((c = serialRead()) != -1) + while((c = serial_read()) != 0xff) { if((char_counter > 0) && ((c == '\n') || (c == '\r'))) { // Line is complete. Then execute! line[char_counter] = 0; // treminate string diff --git a/serial.c b/serial.c index bcaa66f..cc78802 100644 --- a/serial.c +++ b/serial.c @@ -21,9 +21,8 @@ */ -#include -#include #include +#include // Define constants and variables for buffering incoming serial data. We're // using a ring buffer (I think), in which rx_buffer_head is the index of the @@ -46,7 +45,7 @@ uint8_t tx_buffer_head = 0; volatile uint8_t tx_buffer_tail = 0; -void beginSerial(long baud) +void serial_init(long baud) { UBRR0H = ((F_CPU / 16 + baud / 2) / baud - 1) >> 8; UBRR0L = ((F_CPU / 16 + baud / 2) / baud - 1); @@ -60,56 +59,50 @@ void beginSerial(long baud) // enable interrupt on complete reception of a byte UCSR0B |= 1< 0) { - buf[i++] = n % base; - n /= base; - } - - for (; i > 0; i--) - printByte(buf[i - 1] < 10 ? - '0' + buf[i - 1] : - 'A' + buf[i - 1] - 10); -} - -void printInteger(long n) -{ - if (n < 0) { - printByte('-'); - n = -n; - } - - printIntegerInBase(n, 10); -} - -void printFloat(double n) -{ - double integer_part, fractional_part; - fractional_part = modf(n, &integer_part); - printInteger(integer_part); - printByte('.'); - printInteger(round(fractional_part*1000)); -} - diff --git a/serial.h b/serial.h index e6f3055..1f7e420 100644 --- a/serial.h +++ b/serial.h @@ -25,20 +25,8 @@ #ifndef serial_h #define serial_h -void beginSerial(long); -void serialWrite(uint8_t); -int serialAnyAvailable(void); -uint8_t serialRead(void); -void printMode(int); -void printByte(unsigned char c); -void printNewline(void); -void printString(const char *s); -void printPgmString(const char *s); -void printInteger(long n); -void printHex(unsigned long n); -void printOctal(unsigned long n); -void printBinary(unsigned long n); -void printIntegerInBase(unsigned long n, unsigned long base); -void printFloat(double n); +void serial_init(long); +void serial_write(uint8_t); +uint8_t serial_read(void); #endif diff --git a/settings.c b/settings.c index 7782627..3497304 100644 --- a/settings.c +++ b/settings.c @@ -23,7 +23,7 @@ #include "nuts_bolts.h" #include "settings.h" #include "eeprom.h" -#include "serial.h" +#include "print.h" #include #include "protocol.h" #include "config.h" diff --git a/stepper.c b/stepper.c index a145475..164db81 100644 --- a/stepper.c +++ b/stepper.c @@ -30,7 +30,6 @@ #include "nuts_bolts.h" #include #include "planner.h" -#include "serial.h" #include "limits.h" // Some useful constants From 9a5fed4fda7012f87d48a72c919d92ef6f4b9f70 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 3 Jun 2011 15:31:59 +0200 Subject: [PATCH 34/82] changed file headers to reflect the totally refactored state of the serial and print modules --- print.c | 24 ++++++++++++++++++++++++ print.h | 23 +++++++++++++++++++++++ serial.c | 36 ++++++++++++++++-------------------- serial.h | 31 +++++++++++++++---------------- 4 files changed, 78 insertions(+), 36 deletions(-) diff --git a/print.c b/print.c index ffd3f33..de3a1ad 100644 --- a/print.c +++ b/print.c @@ -1,3 +1,27 @@ +/* + print.c - Functions for formatting output strings + Part of Grbl + + 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 . +*/ + +/* This code was initially inspired by the wiring_serial module by David A. Mellis which + used to be a part of the Arduino project. */ + + #include #include #include "serial.h" diff --git a/print.h b/print.h index 62b76ea..b8241f6 100644 --- a/print.h +++ b/print.h @@ -1,3 +1,26 @@ +/* + print.h - Functions for formatting output strings + Part of Grbl + + 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 . +*/ + +/* This code was initially inspired by the wiring_serial module by David A. Mellis which + used to be a part of the Arduino project. */ + #ifndef print_h #define print_h diff --git a/serial.c b/serial.c index cc78802..d71eded 100644 --- a/serial.c +++ b/serial.c @@ -1,26 +1,26 @@ /* - serial.c - serial functions. - Part of Arduino - http://www.arduino.cc/ + serial.c - Low level functions for sending and recieving bytes via the serial port + Part of Grbl - Copyright (c) 2005-2006 David A. Mellis + Copyright (c) 2009-2011 Simen Svale Skogsrud - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. + 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. - This library is distributed in the hope that it will be useful, + 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 - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General - Public License along with this library; if not, write to the - Free Software Foundation, Inc., 59 Temple Place, Suite 330, - Boston, MA 02111-1307 USA + 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 . */ +/* This code was initially inspired by the wiring_serial module by David A. Mellis which + used to be a part of the Arduino project. */ + #include #include @@ -93,10 +93,7 @@ SIGNAL(USART_UDRE_vect) { tx_buffer_tail = tail; // Turn off Data Register Empty Interrupt if this concludes the transfer - if (tail == tx_buffer_head) { - UCSR0B &= ~(1 << UDRIE0); - } - + if (tail == tx_buffer_head) { UCSR0B &= ~(1 << UDRIE0); } } uint8_t serial_read() @@ -124,4 +121,3 @@ SIGNAL(USART_RX_vect) rx_buffer_head = next_head; } } - diff --git a/serial.h b/serial.h index 1f7e420..ec41433 100644 --- a/serial.h +++ b/serial.h @@ -1,27 +1,26 @@ /* - Based on wiring.h - Partial implementation of the Wiring API for the ATmega8. - Part of Arduino - http://www.arduino.cc/ + serial.c - Low level functions for sending and recieving bytes via the serial port + Part of Grbl - Copyright (c) 2005-2006 David A. Mellis + Copyright (c) 2009-2011 Simen Svale Skogsrud - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. + 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. - This library is distributed in the hope that it will be useful, + 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 - Lesser General Public License for more details. + 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 Lesser General - Public License along with this library; if not, write to the - Free Software Foundation, Inc., 59 Temple Place, Suite 330, - Boston, MA 02111-1307 USA - - $Id: wiring.h 387 2008-03-08 21:30:00Z mellis $ + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . */ +/* This code was initially inspired by the wiring_serial module by David A. Mellis which + used to be a part of the Arduino project. */ + #ifndef serial_h #define serial_h From ab8bae74c1915e0cbf4b8a2fd4f53267fddd440c Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 3 Jun 2011 15:33:12 +0200 Subject: [PATCH 35/82] pruned some undefined declarations from the print module --- print.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/print.h b/print.h index b8241f6..c2b76a6 100644 --- a/print.h +++ b/print.h @@ -24,13 +24,9 @@ #ifndef print_h #define print_h -void printNewline(void); void printString(const char *s); void printPgmString(const char *s); void printInteger(long n); -void printHex(unsigned long n); -void printOctal(unsigned long n); -void printBinary(unsigned long n); void printIntegerInBase(unsigned long n, unsigned long base); void printFloat(double n); From 5eea9f4c7c49494b199707f04b329621eb901b8d Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 3 Jun 2011 15:33:56 +0200 Subject: [PATCH 36/82] pruned extraneous comment --- serial.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/serial.c b/serial.c index d71eded..01cf8c1 100644 --- a/serial.c +++ b/serial.c @@ -24,10 +24,7 @@ #include #include -// Define constants and variables for buffering incoming serial data. We're -// using a ring buffer (I think), in which rx_buffer_head is the index of the -// location to which to write the next incoming character and rx_buffer_tail -// is the index of the location from which to read. + #ifdef __AVR_ATmega328P__ #define RX_BUFFER_SIZE 256 #else From 9488cb329c72dffc6d48d0b14d4f6c908fc0400a Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 3 Jun 2011 15:36:14 +0200 Subject: [PATCH 37/82] cleaned up function definitions for serial and print modules and added a comment about the deeply flawed printFloat method --- print.c | 3 +++ print.h | 4 ++++ serial.h | 8 +++++--- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/print.c b/print.c index de3a1ad..697ffb5 100644 --- a/print.c +++ b/print.c @@ -71,6 +71,9 @@ void printInteger(long n) printIntegerInBase(n, 10); } +// TODO: This is nasty. I can't remember where I got this, but this monster +// will truncate leading zeroes from the fractional part so that +// 3.5, 3.05 and 3.000005 all will print as 3.5!!! Needs fixing fast. void printFloat(double n) { double integer_part, fractional_part; diff --git a/print.h b/print.h index c2b76a6..b2581ba 100644 --- a/print.h +++ b/print.h @@ -25,9 +25,13 @@ #define print_h void printString(const char *s); + void printPgmString(const char *s); + void printInteger(long n); + void printIntegerInBase(unsigned long n, unsigned long base); + void printFloat(double n); #endif \ No newline at end of file diff --git a/serial.h b/serial.h index ec41433..4991d5c 100644 --- a/serial.h +++ b/serial.h @@ -24,8 +24,10 @@ #ifndef serial_h #define serial_h -void serial_init(long); -void serial_write(uint8_t); -uint8_t serial_read(void); +void serial_init(long baud); + +void serial_write(uint8_t data); + +uint8_t serial_read(); #endif From bfe7a2ad84dc62e2b32a2c22ef9ffb338f2c56a1 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 3 Jun 2011 15:42:28 +0200 Subject: [PATCH 38/82] flossed the serial module --- serial.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/serial.c b/serial.c index 01cf8c1..1d0cb41 100644 --- a/serial.c +++ b/serial.c @@ -71,13 +71,12 @@ void serial_write(uint8_t data) { tx_buffer[tx_buffer_head] = data; tx_buffer_head = next_head; - // Enable Data Register Empty Interrupt + // Enable Data Register Empty Interrupt to make sure tx-streaming is running UCSR0B |= (1 << UDRIE0); } // Data Register Empty Interrupt handler -SIGNAL(USART_UDRE_vect) { - +SIGNAL(USART_UDRE_vect) { // temporary tx_buffer_tail (to optimize for volatile) uint8_t tail = tx_buffer_tail; @@ -87,15 +86,17 @@ SIGNAL(USART_UDRE_vect) { // Update tail position tail ++; tail %= TX_BUFFER_SIZE; - tx_buffer_tail = tail; - // Turn off Data Register Empty Interrupt if this concludes the transfer + // Turn off Data Register Empty Interrupt to stop tx-streaming if this concludes the transfer if (tail == tx_buffer_head) { UCSR0B &= ~(1 << UDRIE0); } + + tx_buffer_tail = tail; } uint8_t serial_read() { if (rx_buffer_head != rx_buffer_tail) { + // Return magic number if no data pending return 0xff; } else { uint8_t data = rx_buffer[rx_buffer_tail]; From 69be1240be4ea68779eebbda0670ab0216428bdc Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 3 Jun 2011 15:43:14 +0200 Subject: [PATCH 39/82] refactored a complex comment --- serial.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/serial.c b/serial.c index 1d0cb41..b2786b9 100644 --- a/serial.c +++ b/serial.c @@ -110,10 +110,7 @@ SIGNAL(USART_RX_vect) uint8_t data = UDR0; uint8_t next_head = (rx_buffer_head + 1) % RX_BUFFER_SIZE; - // if we should be storing the received character into the location - // just before the tail (meaning that the head would advance to the - // current location of the tail), we're about to overflow the buffer - // and so we don't write the character or advance the head. + // Write data to buffer unless it is full. if (next_head != rx_buffer_tail) { rx_buffer[rx_buffer_head] = data; rx_buffer_head = next_head; From f0843db46e953beda585846c87c0c11fdabf95a9 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 3 Jun 2011 21:50:02 +0200 Subject: [PATCH 40/82] refactored printIntegerInBase to work without a buffer + minor cleanup --- print.c | 21 ++++++++++----------- protocol.c | 2 +- serial.c | 3 ++- serial.h | 2 ++ 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/print.c b/print.c index 697ffb5..1487d73 100644 --- a/print.c +++ b/print.c @@ -40,25 +40,24 @@ void printPgmString(const char *s) serial_write(c); } +// Prints a single digit of any base up to 36. 0..9 prints as +// '0'..'9' while 10..35 prints as 'a'..'z' +void printDigit(uint8_t value) { + serial_write(value < 10 ? + '0' + value : + 'a' + value - 10); +} + void printIntegerInBase(unsigned long n, unsigned long base) { - unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. - unsigned long i = 0; - if (n == 0) { - serial_write('0'); + printDigit(0); return; } - while (n > 0) { - buf[i++] = n % base; + printDigit(n % base); n /= base; } - - for (; i > 0; i--) - serial_write(buf[i - 1] < 10 ? - '0' + buf[i - 1] : - 'A' + buf[i - 1] - 10); } void printInteger(long n) diff --git a/protocol.c b/protocol.c index a504d3a..b6ea2ba 100644 --- a/protocol.c +++ b/protocol.c @@ -72,7 +72,7 @@ uint8_t protocol_execute_line(char *line) { void protocol_process() { char c; - while((c = serial_read()) != 0xff) + while((c = serial_read()) != SERIAL_NO_DATA) { if((char_counter > 0) && ((c == '\n') || (c == '\r'))) { // Line is complete. Then execute! line[char_counter] = 0; // treminate string diff --git a/serial.c b/serial.c index b2786b9..2d7aaf7 100644 --- a/serial.c +++ b/serial.c @@ -23,6 +23,7 @@ #include #include +#include "serial.h" #ifdef __AVR_ATmega328P__ @@ -97,7 +98,7 @@ uint8_t serial_read() { if (rx_buffer_head != rx_buffer_tail) { // Return magic number if no data pending - return 0xff; + return SERIAL_NO_DATA; } else { uint8_t data = rx_buffer[rx_buffer_tail]; rx_buffer_tail = (rx_buffer_tail + 1) % RX_BUFFER_SIZE; diff --git a/serial.h b/serial.h index 4991d5c..910799b 100644 --- a/serial.h +++ b/serial.h @@ -24,6 +24,8 @@ #ifndef serial_h #define serial_h +#define SERIAL_NO_DATA 0xff + void serial_init(long baud); void serial_write(uint8_t data); From a7c13c497adbdfea8a1dbd87ee0d86276afceec9 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 3 Jun 2011 21:56:18 +0200 Subject: [PATCH 41/82] a new printFloat that probably works --- print.c | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/print.c b/print.c index 1487d73..f326d47 100644 --- a/print.c +++ b/print.c @@ -26,6 +26,8 @@ #include #include "serial.h" +#define DECIMAL_PLACES 4 + void printString(const char *s) { while (*s) @@ -70,15 +72,20 @@ void printInteger(long n) printIntegerInBase(n, 10); } -// TODO: This is nasty. I can't remember where I got this, but this monster -// will truncate leading zeroes from the fractional part so that -// 3.5, 3.05 and 3.000005 all will print as 3.5!!! Needs fixing fast. +// A very simple void printFloat(double n) { - double integer_part, fractional_part; + double integer_part, fractional_part, decimal_part; fractional_part = modf(n, &integer_part); printInteger(integer_part); serial_write('.'); - printInteger(round(fractional_part*1000)); + fractional_part *= 10; + int decimals = DECIMAL_PLACES; + while(decimals > 0) { + decimal_part = floor(fractional_part); + printDigit(decimal_part); + fractional_part -= decimal_part; + fractional_part *= 10; + } } From 903303579fae6e7ff5fc4188eef0d57e4c24d68e Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 3 Jun 2011 21:56:55 +0200 Subject: [PATCH 42/82] NOW printFloat probably works --- print.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/print.c b/print.c index f326d47..2375f37 100644 --- a/print.c +++ b/print.c @@ -81,7 +81,7 @@ void printFloat(double n) serial_write('.'); fractional_part *= 10; int decimals = DECIMAL_PLACES; - while(decimals > 0) { + while(decimals-- > 0) { decimal_part = floor(fractional_part); printDigit(decimal_part); fractional_part -= decimal_part; From 74db3e6c5b6b91c74a49f861a03729966da09263 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sat, 4 Jun 2011 22:17:51 +0200 Subject: [PATCH 43/82] undid changes to printIntegerInBase that resulted in the integers being printed in reverse... --- print.c | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/print.c b/print.c index 2375f37..23e8ae3 100644 --- a/print.c +++ b/print.c @@ -42,24 +42,25 @@ void printPgmString(const char *s) serial_write(c); } -// Prints a single digit of any base up to 36. 0..9 prints as -// '0'..'9' while 10..35 prints as 'a'..'z' -void printDigit(uint8_t value) { - serial_write(value < 10 ? - '0' + value : - 'a' + value - 10); -} - void printIntegerInBase(unsigned long n, unsigned long base) { + unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. + unsigned long i = 0; + if (n == 0) { - printDigit(0); + serial_write('0'); return; } + while (n > 0) { - printDigit(n % base); + buf[i++] = n % base; n /= base; } + + for (; i > 0; i--) + serial_write(buf[i - 1] < 10 ? + '0' + buf[i - 1] : + 'A' + buf[i - 1] - 10); } void printInteger(long n) @@ -75,7 +76,8 @@ void printInteger(long n) // A very simple void printFloat(double n) { - double integer_part, fractional_part, decimal_part; + double integer_part, fractional_part; + uint8_t decimal_part; fractional_part = modf(n, &integer_part); printInteger(integer_part); serial_write('.'); @@ -83,7 +85,7 @@ void printFloat(double n) int decimals = DECIMAL_PLACES; while(decimals-- > 0) { decimal_part = floor(fractional_part); - printDigit(decimal_part); + serial_write('0'+decimal_part); fractional_part -= decimal_part; fractional_part *= 10; } From 553c44a93f92e314a6c9dd35c933d5ae0f18b6a3 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sun, 5 Jun 2011 20:54:23 +0200 Subject: [PATCH 44/82] Fixed serial.c after tests on real hardware --- main.c | 4 ++-- serial.c | 13 ++++++++----- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/main.c b/main.c index 29d37da..b56bf6f 100644 --- a/main.c +++ b/main.c @@ -36,6 +36,8 @@ int main(void) { + sei(); + serial_init(BAUD_RATE); protocol_init(); settings_init(); @@ -44,8 +46,6 @@ int main(void) spindle_init(); gc_init(); limits_init(); - - sei(); for(;;){ sleep_mode(); // Wait for it ... diff --git a/serial.c b/serial.c index 2d7aaf7..ee63096 100644 --- a/serial.c +++ b/serial.c @@ -42,12 +42,16 @@ uint8_t tx_buffer[TX_BUFFER_SIZE]; uint8_t tx_buffer_head = 0; volatile uint8_t tx_buffer_tail = 0; +static void set_baud_rate(long baud) { + uint16_t UBRR0_value = ((F_CPU / 16 + baud / 2) / baud - 1); + UBRR0H = UBRR0_value >> 8; + UBRR0L = UBRR0_value; +} void serial_init(long baud) { - UBRR0H = ((F_CPU / 16 + baud / 2) / baud - 1) >> 8; - UBRR0L = ((F_CPU / 16 + baud / 2) / baud - 1); - + set_baud_rate(baud); + /* baud doubler off - Only needed on Uno XXX */ UCSR0A &= ~(1 << U2X0); @@ -96,8 +100,7 @@ SIGNAL(USART_UDRE_vect) { uint8_t serial_read() { - if (rx_buffer_head != rx_buffer_tail) { - // Return magic number if no data pending + if (rx_buffer_head == rx_buffer_tail) { return SERIAL_NO_DATA; } else { uint8_t data = rx_buffer[rx_buffer_tail]; From bd336867a64d9a22dbbee3fd4fae28c0b894260c Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sun, 5 Jun 2011 20:58:21 +0200 Subject: [PATCH 45/82] changed default fp decimal places to 3 --- print.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/print.c b/print.c index 23e8ae3..05524a3 100644 --- a/print.c +++ b/print.c @@ -26,7 +26,9 @@ #include #include "serial.h" -#define DECIMAL_PLACES 4 +#ifndef DECIMAL_PLACES +#define DECIMAL_PLACES 3 +#endif void printString(const char *s) { From 9e09a502e9d34413ba1480c30a0d5527fe8025ce Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sun, 5 Jun 2011 21:12:56 +0200 Subject: [PATCH 46/82] updated readme to reflect real state of 168-compatibility --- readme.textile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/readme.textile b/readme.textile index 2ad2df2..fab1580 100644 --- a/readme.textile +++ b/readme.textile @@ -8,7 +8,7 @@ It accepts standards-compliant G-code and has been tested with the output of sev Grbl includes full acceleration management with look ahead. That means the controller will look up to 20 motions into the future and plan its velocities ahead to deliver smooth acceleration and jerk-free cornering. -*Important note for Atmega 168 users:* Grbl used to be compatible with both the older Ardunios running atmega 168 and the newer with 328p. This had to go, as I was unable to fit the acceleration management into the 16k code space of the 168. If you want to run Grbl on an 168 I am still maintaining Grbl 0.51 "in the branch called 'v0_51'":https://github.com/simen/grbl/tree/v0_51. +*Important note for Atmega 168 users:* Grbl used to be compatible with both the older Ardunios running atmega 168 and the newer with 328p. The full version of Grbl now compiles without support for circles/arcs if you target 168. If you need arcs, but not acceleration-management I am still maintaining Grbl 0.51 "in the branch called 'v0_51'":https://github.com/simen/grbl/tree/v0_51. *Note for users upgrading from 0.51 to 0.6:* The new version has new and improved default pin-out. If nothing works when you upgrade, that is because the pulse trains are coming from the wrong pins. This is a simple matter of editing config.h – the whole legacy pin assignment is there for you to uncomment. From c3fda5ac211cff8b02d011a9ddde50ae8f29ed75 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sun, 5 Jun 2011 21:34:53 +0200 Subject: [PATCH 47/82] fixed a bug where the default step mask would not actually reflect the real pin out --- settings.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/settings.c b/settings.c index 3497304..a99fe25 100644 --- a/settings.c +++ b/settings.c @@ -52,7 +52,7 @@ typedef struct { #define DEFAULT_FEEDRATE 500.0 #define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/10.0) #define DEFAULT_MAX_JERK 300.0 -#define DEFAULT_STEPPING_INVERT_MASK ((1< Date: Mon, 6 Jun 2011 08:38:38 +0200 Subject: [PATCH 48/82] avoid line buffer overflow --- protocol.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/protocol.c b/protocol.c index b6ea2ba..1275f8d 100644 --- a/protocol.c +++ b/protocol.c @@ -78,7 +78,10 @@ void protocol_process() line[char_counter] = 0; // treminate string status_message(protocol_execute_line(line)); char_counter = 0; // reset line buffer index - } else if (c <= ' ') { // Throw away whitepace and control characters + } else if (c <= ' ') { + // Throw away whitepace and control characters + } else if (char_counter >= LINE_BUFFER_SIZE-1) { + // Throw away any characters beyond the end of the line buffer } else if (c >= 'a' && c <= 'z') { // Upcase lowercase line[char_counter++] = c-'a'+'A'; } else { From a9aa7d4d397c9e42c3187792029adacbb6965c8b Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Mon, 6 Jun 2011 20:00:57 +0200 Subject: [PATCH 49/82] need to bump the version number because of the serial refactoring --- settings.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/settings.h b/settings.h index 93feede..df9312a 100644 --- a/settings.h +++ b/settings.h @@ -25,7 +25,7 @@ #include #include -#define GRBL_VERSION "0.6b" +#define GRBL_VERSION "0.7b" // 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 From 804837a111e8fddb87074f12d8c39e1d5649d265 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sat, 9 Jul 2011 22:35:25 +0200 Subject: [PATCH 50/82] corrected an outdated comment --- planner.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planner.c b/planner.c index 28f4e2c..549a016 100644 --- a/planner.c +++ b/planner.c @@ -304,9 +304,9 @@ block_t *plan_get_current_block() { return(&block_buffer[block_buffer_tail]); } -// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in -// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration -// calculation the caller must also provide the physical length of the line in millimeters. +// 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 +// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) { // The target position of the tool in absolute steps From 5466bc0c33e9be6d94bff2485e6fb697413bdec6 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sun, 10 Jul 2011 21:54:03 +0200 Subject: [PATCH 51/82] fixed a nil dereferencing bug in planner_forward_pass_kernel thanks to jv4779 --- planner.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/planner.c b/planner.c index 549a016..5da1d3c 100644 --- a/planner.c +++ b/planner.c @@ -206,13 +206,15 @@ static void planner_forward_pass_kernel(block_t *previous, block_t *current, blo // If the previous block is an acceleration block, but it is not long enough to // complete the full speed change within the block, we need to adjust out entry // speed accordingly. Remember current->entry_factor equals the exit factor of - // the previous block. - if(previous->entry_factor < current->entry_factor) { - double max_entry_speed = max_allowable_speed(-settings.acceleration, - current->nominal_speed*previous->entry_factor, previous->millimeters); - double max_entry_factor = max_entry_speed/current->nominal_speed; - if (max_entry_factor < current->entry_factor) { - current->entry_factor = max_entry_factor; + // the previous block.¨ + if(previous) { + if(previous->entry_factor < current->entry_factor) { + double max_entry_speed = max_allowable_speed(-settings.acceleration, + current->nominal_speed*previous->entry_factor, previous->millimeters); + double max_entry_factor = max_entry_speed/current->nominal_speed; + if (max_entry_factor < current->entry_factor) { + current->entry_factor = max_entry_factor; + } } } } From d3f1f82e5f65e83198b3ef0f013ed7c77dbc120d Mon Sep 17 00:00:00 2001 From: Brian Boucheron Date: Sun, 17 Jul 2011 13:35:44 -0400 Subject: [PATCH 52/82] added error message for step pulses < 3 microseconds --- settings.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/settings.c b/settings.c index a99fe25..82544d3 100644 --- a/settings.c +++ b/settings.c @@ -145,7 +145,12 @@ void settings_store_setting(int parameter, double value) { return; } settings.steps_per_mm[parameter] = value; break; - case 3: settings.pulse_microseconds = round(value); break; + case 3: + if (value < 3) { + printPgmString(PSTR("Step pulse must be >= 3 microseconds\r\n")); + return; + } + settings.pulse_microseconds = round(value); break; case 4: settings.default_feed_rate = value; break; case 5: settings.default_seek_rate = value; break; case 6: settings.mm_per_arc_segment = value; break; From 517a0f659a06182c89cafe27ee371edccad777a4 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Tue, 2 Aug 2011 22:44:05 +0200 Subject: [PATCH 53/82] reordered a couple of functions for consistency --- planner.c | 77 ++++++++++++++++++++++++++++--------------------------- 1 file changed, 39 insertions(+), 38 deletions(-) diff --git a/planner.c b/planner.c index 5da1d3c..9856489 100644 --- a/planner.c +++ b/planner.c @@ -79,43 +79,7 @@ static double intersection_distance(double initial_rate, double final_rate, doub (4*acceleration) ); } - -/* - +--------+ <- nominal_rate - / \ - nominal_rate*entry_factor -> + \ - | + <- nominal_rate*exit_factor - +-------------+ - time --> -*/ - -// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. -// The factors represent a factor of braking and must be in the range 0.0-1.0. - -static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { - block->initial_rate = ceil(block->nominal_rate*entry_factor); - block->final_rate = ceil(block->nominal_rate*exit_factor); - int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; - int32_t accelerate_steps = - ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute)); - int32_t decelerate_steps = - floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute)); - - // Calculate the size of Plateau of Nominal Rate. - int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; - - // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will - // have to use intersection_distance() to calculate when to abort acceleration and start braking - // in order to reach the final_rate exactly at the end of this block. - if (plateau_steps < 0) { - accelerate_steps = ceil( - intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count)); - plateau_steps = 0; - } - - block->accelerate_until = accelerate_steps; - block->decelerate_after = accelerate_steps+plateau_steps; -} + // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // acceleration within the allotted distance. @@ -146,6 +110,7 @@ static double factor_for_safe_speed(block_t *block) { } } + // The kernel called by planner_recalculate() when scanning the plan from last to first entry. static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { if(!current) { return; } @@ -235,6 +200,42 @@ static void planner_forward_pass() { planner_forward_pass_kernel(block[1], block[2], NULL); } +/* + +--------+ <- nominal_rate + / \ + nominal_rate*entry_factor -> + \ + | + <- nominal_rate*exit_factor + +-------------+ + time --> +*/ + +// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. +// The factors represent a factor of braking and must be in the range 0.0-1.0. +static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { + block->initial_rate = ceil(block->nominal_rate*entry_factor); + block->final_rate = ceil(block->nominal_rate*exit_factor); + int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; + int32_t accelerate_steps = + ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute)); + int32_t decelerate_steps = + floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute)); + + // Calculate the size of Plateau of Nominal Rate. + int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; + + // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will + // have to use intersection_distance() to calculate when to abort acceleration and start braking + // in order to reach the final_rate exactly at the end of this block. + if (plateau_steps < 0) { + accelerate_steps = ceil( + intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count)); + plateau_steps = 0; + } + + block->accelerate_until = accelerate_steps; + block->decelerate_after = accelerate_steps+plateau_steps; +} + // Recalculates the trapezoid speed profiles for all blocks in the plan according to the // entry_factor for each junction. Must be called by planner_recalculate() after // updating the blocks. @@ -269,7 +270,7 @@ static void planner_recalculate_trapezoids() { // be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than // the set limit. Finally it will: // -// 3. Recalculate trapezoids for all blocks. +// 3. Recalculate trapezoids for all blocks using the recently updated factors static void planner_recalculate() { planner_reverse_pass(); From ea5b8942db2616e93fc0478922010c3bab7c0481 Mon Sep 17 00:00:00 2001 From: chamnit Date: Mon, 15 Aug 2011 17:06:50 -0600 Subject: [PATCH 54/82] Moved comment and block delete handling to be done in protocol.c rather than gcode.c. Prevents these from being held in memory. Also, fixes bug when comments and block delete character are mixed with g-code. --- protocol.c | 45 +++++++++++++++++++++++++++++++++------------ 1 file changed, 33 insertions(+), 12 deletions(-) diff --git a/protocol.c b/protocol.c index 1275f8d..ed509d4 100644 --- a/protocol.c +++ b/protocol.c @@ -72,20 +72,41 @@ uint8_t protocol_execute_line(char *line) { void protocol_process() { char c; + uint8_t iscomment = false; while((c = serial_read()) != SERIAL_NO_DATA) { - if((char_counter > 0) && ((c == '\n') || (c == '\r'))) { // Line is complete. Then execute! - line[char_counter] = 0; // treminate string - status_message(protocol_execute_line(line)); - char_counter = 0; // reset line buffer index - } else if (c <= ' ') { - // Throw away whitepace and control characters - } else if (char_counter >= LINE_BUFFER_SIZE-1) { - // Throw away any characters beyond the end of the line buffer - } else if (c >= 'a' && c <= 'z') { // Upcase lowercase - line[char_counter++] = c-'a'+'A'; - } else { - line[char_counter++] = c; + if (iscomment) { + // While in comment, skip all characters until ')', '\n', or '\r' + if ((c == ')') || (c == '\n') || (c == '\r')) { + iscomment = false; + } + } + if (!iscomment) { // Seperate if-statement to process '\n' or '\r' at end of comment + if ((c == '\n') || (c == '\r')) { // End of block reached + if (char_counter > 0) {// Line is complete. Then execute! + line[char_counter] = 0; // terminate string + status_message(protocol_execute_line(line)); + char_counter = 0; // reset line buffer index + } else { + // Empty or comment line. Skip block. + status_message(STATUS_OK); // Send status message for syncing purposes. + } + } else if (c <= ' ') { + // Throw away whitepace and control characters + } else if (c == '/') { + // Disable block delete and throw away character + // To enable block delete, uncomment following line + // iscomment = true; + } else if (c == '(') { + // Enable comments and ignore all characters until ')' or '\n' + iscomment = true; + } else if (char_counter >= LINE_BUFFER_SIZE-1) { + // Throw away any characters beyond the end of the line buffer + } else if (c >= 'a' && c <= 'z') { // Upcase lowercase + line[char_counter++] = c-'a'+'A'; + } else { + line[char_counter++] = c; + } } } } From fdc90f1821f1f5edb7756fcddce75b4b4fbf6bbf Mon Sep 17 00:00:00 2001 From: chamnit Date: Mon, 15 Aug 2011 17:10:08 -0600 Subject: [PATCH 55/82] Removed comment and block delete handling from gcode.c. Parser expects clean gcode. --- gcode.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/gcode.c b/gcode.c index be62ce8..62fec77 100644 --- a/gcode.c +++ b/gcode.c @@ -117,7 +117,8 @@ static double theta(double x, double y) #endif // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase -// characters and signed floating point values (no whitespace). +// characters and signed floating point values (no whitespace). Comments and block delete +// characters have been removed. uint8_t gc_execute_line(char *line) { uint8_t char_counter = 0; char letter; @@ -139,10 +140,6 @@ uint8_t gc_execute_line(char *line) { gc.status_code = STATUS_OK; - // Disregard comments and block delete - if (line[0] == '(') { return(gc.status_code); } - if (line[0] == '/') { char_counter++; } // ignore block delete - // Pass 1: Commands while(next_statement(&letter, &value, line, &char_counter)) { int_value = trunc(value); From 8b0556bcfdba918329d6a37a6ce057f1d31a4f86 Mon Sep 17 00:00:00 2001 From: Sonny J Date: Mon, 15 Aug 2011 19:14:25 -0600 Subject: [PATCH 56/82] Revert fdc90f1821f1f5edb7756fcddce75b4b4fbf6bbf^..HEAD --- gcode.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/gcode.c b/gcode.c index 62fec77..be62ce8 100644 --- a/gcode.c +++ b/gcode.c @@ -117,8 +117,7 @@ static double theta(double x, double y) #endif // 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. +// characters and signed floating point values (no whitespace). uint8_t gc_execute_line(char *line) { uint8_t char_counter = 0; char letter; @@ -140,6 +139,10 @@ uint8_t gc_execute_line(char *line) { gc.status_code = STATUS_OK; + // Disregard comments and block delete + if (line[0] == '(') { return(gc.status_code); } + if (line[0] == '/') { char_counter++; } // ignore block delete + // Pass 1: Commands while(next_statement(&letter, &value, line, &char_counter)) { int_value = trunc(value); From a2837943c014590704e57bca8597b9d8654f1b17 Mon Sep 17 00:00:00 2001 From: Sonny J Date: Mon, 15 Aug 2011 19:14:39 -0600 Subject: [PATCH 57/82] Revert "Moved comment and block delete handling to be done in protocol.c rather than gcode.c. Prevents these from being held in memory. Also, fixes bug when comments and block delete character are mixed with g-code." This reverts commit ea5b8942db2616e93fc0478922010c3bab7c0481. --- protocol.c | 45 ++++++++++++--------------------------------- 1 file changed, 12 insertions(+), 33 deletions(-) diff --git a/protocol.c b/protocol.c index ed509d4..1275f8d 100644 --- a/protocol.c +++ b/protocol.c @@ -72,41 +72,20 @@ uint8_t protocol_execute_line(char *line) { void protocol_process() { char c; - uint8_t iscomment = false; while((c = serial_read()) != SERIAL_NO_DATA) { - if (iscomment) { - // While in comment, skip all characters until ')', '\n', or '\r' - if ((c == ')') || (c == '\n') || (c == '\r')) { - iscomment = false; - } - } - if (!iscomment) { // Seperate if-statement to process '\n' or '\r' at end of comment - if ((c == '\n') || (c == '\r')) { // End of block reached - if (char_counter > 0) {// Line is complete. Then execute! - line[char_counter] = 0; // terminate string - status_message(protocol_execute_line(line)); - char_counter = 0; // reset line buffer index - } else { - // Empty or comment line. Skip block. - status_message(STATUS_OK); // Send status message for syncing purposes. - } - } else if (c <= ' ') { - // Throw away whitepace and control characters - } else if (c == '/') { - // Disable block delete and throw away character - // To enable block delete, uncomment following line - // iscomment = true; - } else if (c == '(') { - // Enable comments and ignore all characters until ')' or '\n' - iscomment = true; - } else if (char_counter >= LINE_BUFFER_SIZE-1) { - // Throw away any characters beyond the end of the line buffer - } else if (c >= 'a' && c <= 'z') { // Upcase lowercase - line[char_counter++] = c-'a'+'A'; - } else { - line[char_counter++] = c; - } + if((char_counter > 0) && ((c == '\n') || (c == '\r'))) { // Line is complete. Then execute! + line[char_counter] = 0; // treminate string + status_message(protocol_execute_line(line)); + char_counter = 0; // reset line buffer index + } else if (c <= ' ') { + // Throw away whitepace and control characters + } else if (char_counter >= LINE_BUFFER_SIZE-1) { + // Throw away any characters beyond the end of the line buffer + } else if (c >= 'a' && c <= 'z') { // Upcase lowercase + line[char_counter++] = c-'a'+'A'; + } else { + line[char_counter++] = c; } } } From 971e50aa9a0a29afc40beacc0ca3c23e590db65f Mon Sep 17 00:00:00 2001 From: Sonny J Date: Mon, 15 Aug 2011 19:15:43 -0600 Subject: [PATCH 58/82] Revert 517a0f659a06182c89cafe27ee371edccad777a4^..HEAD --- planner.c | 77 +++++++++++++++++++++++++++---------------------------- 1 file changed, 38 insertions(+), 39 deletions(-) diff --git a/planner.c b/planner.c index 9856489..5da1d3c 100644 --- a/planner.c +++ b/planner.c @@ -79,7 +79,43 @@ static double intersection_distance(double initial_rate, double final_rate, doub (4*acceleration) ); } - + +/* + +--------+ <- nominal_rate + / \ + nominal_rate*entry_factor -> + \ + | + <- nominal_rate*exit_factor + +-------------+ + time --> +*/ + +// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. +// The factors represent a factor of braking and must be in the range 0.0-1.0. + +static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { + block->initial_rate = ceil(block->nominal_rate*entry_factor); + block->final_rate = ceil(block->nominal_rate*exit_factor); + int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; + int32_t accelerate_steps = + ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute)); + int32_t decelerate_steps = + floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute)); + + // Calculate the size of Plateau of Nominal Rate. + int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; + + // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will + // have to use intersection_distance() to calculate when to abort acceleration and start braking + // in order to reach the final_rate exactly at the end of this block. + if (plateau_steps < 0) { + accelerate_steps = ceil( + intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count)); + plateau_steps = 0; + } + + block->accelerate_until = accelerate_steps; + block->decelerate_after = accelerate_steps+plateau_steps; +} // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // acceleration within the allotted distance. @@ -110,7 +146,6 @@ static double factor_for_safe_speed(block_t *block) { } } - // The kernel called by planner_recalculate() when scanning the plan from last to first entry. static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { if(!current) { return; } @@ -200,42 +235,6 @@ static void planner_forward_pass() { planner_forward_pass_kernel(block[1], block[2], NULL); } -/* - +--------+ <- nominal_rate - / \ - nominal_rate*entry_factor -> + \ - | + <- nominal_rate*exit_factor - +-------------+ - time --> -*/ - -// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. -// The factors represent a factor of braking and must be in the range 0.0-1.0. -static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { - block->initial_rate = ceil(block->nominal_rate*entry_factor); - block->final_rate = ceil(block->nominal_rate*exit_factor); - int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; - int32_t accelerate_steps = - ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute)); - int32_t decelerate_steps = - floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute)); - - // Calculate the size of Plateau of Nominal Rate. - int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; - - // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will - // have to use intersection_distance() to calculate when to abort acceleration and start braking - // in order to reach the final_rate exactly at the end of this block. - if (plateau_steps < 0) { - accelerate_steps = ceil( - intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count)); - plateau_steps = 0; - } - - block->accelerate_until = accelerate_steps; - block->decelerate_after = accelerate_steps+plateau_steps; -} - // Recalculates the trapezoid speed profiles for all blocks in the plan according to the // entry_factor for each junction. Must be called by planner_recalculate() after // updating the blocks. @@ -270,7 +269,7 @@ static void planner_recalculate_trapezoids() { // be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than // the set limit. Finally it will: // -// 3. Recalculate trapezoids for all blocks using the recently updated factors +// 3. Recalculate trapezoids for all blocks. static void planner_recalculate() { planner_reverse_pass(); From 896a6b9199395eb9dbfd42134d84ba40a332eb36 Mon Sep 17 00:00:00 2001 From: Sonny J Date: Mon, 15 Aug 2011 19:28:14 -0600 Subject: [PATCH 59/82] Moved comment and block delete handling into protocol.c from gcode.c. Fixes bug when comment and block delete are not isolated. Blank lines ignored. Comments, block delete characters, and blank lines are no longer passed to the gcode parser and should free up some memory by ignoring these characters. Gcode parser now expects clean gcode only. There was a bug if there were block deletes or comments not in the first character (i.e. spindle on/off for proofing geode without turning it on, or a NXX followed by a comment). This should fix it by bypassing the problem. Left a commented line for easily turning on and off block deletes for a later feature, if desired. --- gcode.c | 7 ++----- protocol.c | 46 +++++++++++++++++++++++++++++++++++----------- 2 files changed, 37 insertions(+), 16 deletions(-) diff --git a/gcode.c b/gcode.c index be62ce8..62fec77 100644 --- a/gcode.c +++ b/gcode.c @@ -117,7 +117,8 @@ static double theta(double x, double y) #endif // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase -// characters and signed floating point values (no whitespace). +// characters and signed floating point values (no whitespace). Comments and block delete +// characters have been removed. uint8_t gc_execute_line(char *line) { uint8_t char_counter = 0; char letter; @@ -139,10 +140,6 @@ uint8_t gc_execute_line(char *line) { gc.status_code = STATUS_OK; - // Disregard comments and block delete - if (line[0] == '(') { return(gc.status_code); } - if (line[0] == '/') { char_counter++; } // ignore block delete - // Pass 1: Commands while(next_statement(&letter, &value, line, &char_counter)) { int_value = trunc(value); diff --git a/protocol.c b/protocol.c index 1275f8d..2e2acd3 100644 --- a/protocol.c +++ b/protocol.c @@ -72,20 +72,44 @@ uint8_t protocol_execute_line(char *line) { void protocol_process() { char c; + uint8_t iscomment = false; while((c = serial_read()) != SERIAL_NO_DATA) { - if((char_counter > 0) && ((c == '\n') || (c == '\r'))) { // Line is complete. Then execute! - line[char_counter] = 0; // treminate string - status_message(protocol_execute_line(line)); - char_counter = 0; // reset line buffer index - } else if (c <= ' ') { - // Throw away whitepace and control characters - } else if (char_counter >= LINE_BUFFER_SIZE-1) { - // Throw away any characters beyond the end of the line buffer - } else if (c >= 'a' && c <= 'z') { // Upcase lowercase - line[char_counter++] = c-'a'+'A'; + if ((c == '\n') || (c == '\r')) { // End of block reached + if (char_counter > 0) {// Line is complete. Then execute! + line[char_counter] = 0; // terminate string + status_message(protocol_execute_line(line)); + } else { + // Empty or comment line. Skip block. + status_message(STATUS_OK); // Send status message for syncing purposes. + } + char_counter = 0; // Reset line buffer index + iscomment = false; // Reset comment flag } else { - line[char_counter++] = c; + if (iscomment) { + // Throw away all comment characters + if (c == ')') { + // End of comment. Resume line. + iscomment = false; + } + } else { + if (c <= ' ') { + // Throw away whitepace and control characters + } else if (c == '/') { + // Disable block delete and throw away character + // To enable block delete, uncomment following line. Will ignore until EOL. + // iscomment = true; + } else if (c == '(') { + // Enable comments flag and ignore all characters until ')' or EOL. + iscomment = true; + } else if (char_counter >= LINE_BUFFER_SIZE-1) { + // Throw away any characters beyond the end of the line buffer + } else if (c >= 'a' && c <= 'z') { // Upcase lowercase + line[char_counter++] = c-'a'+'A'; + } else { + line[char_counter++] = c; + } + } } } } From ed5e5d1181547bdc2c0be050a2bf24b846ed90b7 Mon Sep 17 00:00:00 2001 From: Sonny J Date: Mon, 15 Aug 2011 19:37:22 -0600 Subject: [PATCH 60/82] Revert ea5b8942db2616e93fc0478922010c3bab7c0481^..HEAD --- gcode.c | 7 +++-- planner.c | 77 +++++++++++++++++++++++++++--------------------------- protocol.c | 46 ++++++++------------------------ 3 files changed, 55 insertions(+), 75 deletions(-) diff --git a/gcode.c b/gcode.c index 62fec77..be62ce8 100644 --- a/gcode.c +++ b/gcode.c @@ -117,8 +117,7 @@ static double theta(double x, double y) #endif // 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. +// characters and signed floating point values (no whitespace). uint8_t gc_execute_line(char *line) { uint8_t char_counter = 0; char letter; @@ -140,6 +139,10 @@ uint8_t gc_execute_line(char *line) { gc.status_code = STATUS_OK; + // Disregard comments and block delete + if (line[0] == '(') { return(gc.status_code); } + if (line[0] == '/') { char_counter++; } // ignore block delete + // Pass 1: Commands while(next_statement(&letter, &value, line, &char_counter)) { int_value = trunc(value); diff --git a/planner.c b/planner.c index 5da1d3c..9856489 100644 --- a/planner.c +++ b/planner.c @@ -79,43 +79,7 @@ static double intersection_distance(double initial_rate, double final_rate, doub (4*acceleration) ); } - -/* - +--------+ <- nominal_rate - / \ - nominal_rate*entry_factor -> + \ - | + <- nominal_rate*exit_factor - +-------------+ - time --> -*/ - -// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. -// The factors represent a factor of braking and must be in the range 0.0-1.0. - -static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { - block->initial_rate = ceil(block->nominal_rate*entry_factor); - block->final_rate = ceil(block->nominal_rate*exit_factor); - int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; - int32_t accelerate_steps = - ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute)); - int32_t decelerate_steps = - floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute)); - - // Calculate the size of Plateau of Nominal Rate. - int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; - - // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will - // have to use intersection_distance() to calculate when to abort acceleration and start braking - // in order to reach the final_rate exactly at the end of this block. - if (plateau_steps < 0) { - accelerate_steps = ceil( - intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count)); - plateau_steps = 0; - } - - block->accelerate_until = accelerate_steps; - block->decelerate_after = accelerate_steps+plateau_steps; -} + // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // acceleration within the allotted distance. @@ -146,6 +110,7 @@ static double factor_for_safe_speed(block_t *block) { } } + // The kernel called by planner_recalculate() when scanning the plan from last to first entry. static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { if(!current) { return; } @@ -235,6 +200,42 @@ static void planner_forward_pass() { planner_forward_pass_kernel(block[1], block[2], NULL); } +/* + +--------+ <- nominal_rate + / \ + nominal_rate*entry_factor -> + \ + | + <- nominal_rate*exit_factor + +-------------+ + time --> +*/ + +// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. +// The factors represent a factor of braking and must be in the range 0.0-1.0. +static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { + block->initial_rate = ceil(block->nominal_rate*entry_factor); + block->final_rate = ceil(block->nominal_rate*exit_factor); + int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; + int32_t accelerate_steps = + ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute)); + int32_t decelerate_steps = + floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute)); + + // Calculate the size of Plateau of Nominal Rate. + int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; + + // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will + // have to use intersection_distance() to calculate when to abort acceleration and start braking + // in order to reach the final_rate exactly at the end of this block. + if (plateau_steps < 0) { + accelerate_steps = ceil( + intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count)); + plateau_steps = 0; + } + + block->accelerate_until = accelerate_steps; + block->decelerate_after = accelerate_steps+plateau_steps; +} + // Recalculates the trapezoid speed profiles for all blocks in the plan according to the // entry_factor for each junction. Must be called by planner_recalculate() after // updating the blocks. @@ -269,7 +270,7 @@ static void planner_recalculate_trapezoids() { // be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than // the set limit. Finally it will: // -// 3. Recalculate trapezoids for all blocks. +// 3. Recalculate trapezoids for all blocks using the recently updated factors static void planner_recalculate() { planner_reverse_pass(); diff --git a/protocol.c b/protocol.c index 2e2acd3..1275f8d 100644 --- a/protocol.c +++ b/protocol.c @@ -72,44 +72,20 @@ uint8_t protocol_execute_line(char *line) { void protocol_process() { char c; - uint8_t iscomment = false; while((c = serial_read()) != SERIAL_NO_DATA) { - if ((c == '\n') || (c == '\r')) { // End of block reached - if (char_counter > 0) {// Line is complete. Then execute! - line[char_counter] = 0; // terminate string - status_message(protocol_execute_line(line)); - } else { - // Empty or comment line. Skip block. - status_message(STATUS_OK); // Send status message for syncing purposes. - } - char_counter = 0; // Reset line buffer index - iscomment = false; // Reset comment flag + if((char_counter > 0) && ((c == '\n') || (c == '\r'))) { // Line is complete. Then execute! + line[char_counter] = 0; // treminate string + status_message(protocol_execute_line(line)); + char_counter = 0; // reset line buffer index + } else if (c <= ' ') { + // Throw away whitepace and control characters + } else if (char_counter >= LINE_BUFFER_SIZE-1) { + // Throw away any characters beyond the end of the line buffer + } else if (c >= 'a' && c <= 'z') { // Upcase lowercase + line[char_counter++] = c-'a'+'A'; } else { - if (iscomment) { - // Throw away all comment characters - if (c == ')') { - // End of comment. Resume line. - iscomment = false; - } - } else { - if (c <= ' ') { - // Throw away whitepace and control characters - } else if (c == '/') { - // Disable block delete and throw away character - // To enable block delete, uncomment following line. Will ignore until EOL. - // iscomment = true; - } else if (c == '(') { - // Enable comments flag and ignore all characters until ')' or EOL. - iscomment = true; - } else if (char_counter >= LINE_BUFFER_SIZE-1) { - // Throw away any characters beyond the end of the line buffer - } else if (c >= 'a' && c <= 'z') { // Upcase lowercase - line[char_counter++] = c-'a'+'A'; - } else { - line[char_counter++] = c; - } - } + line[char_counter++] = c; } } } From badb638df925924c05b3b5e49880590e53faa759 Mon Sep 17 00:00:00 2001 From: Sonny J Date: Mon, 15 Aug 2011 19:39:44 -0600 Subject: [PATCH 61/82] Moved comment and block delete handling into protocol.c from gcode.c. Fixes bug when comment and block delete are not isolated. Blank lines ignored. Comments, block delete characters, and blank lines are no longer passed to the gcode parser and should free up some memory by ignoring these characters. Gcode parser now expects clean gcode only. There was a bug if there were block deletes or comments not in the first character (i.e. spindle on/off for proofing geode without turning it on, or a NXX followed by a comment). This should fix it by bypassing the problem. Left a commented line for easily turning on and off block deletes for a later feature, if desired. --- gcode.c | 7 ++----- protocol.c | 46 +++++++++++++++++++++++++++++++++++----------- 2 files changed, 37 insertions(+), 16 deletions(-) diff --git a/gcode.c b/gcode.c index be62ce8..62fec77 100644 --- a/gcode.c +++ b/gcode.c @@ -117,7 +117,8 @@ static double theta(double x, double y) #endif // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase -// characters and signed floating point values (no whitespace). +// characters and signed floating point values (no whitespace). Comments and block delete +// characters have been removed. uint8_t gc_execute_line(char *line) { uint8_t char_counter = 0; char letter; @@ -139,10 +140,6 @@ uint8_t gc_execute_line(char *line) { gc.status_code = STATUS_OK; - // Disregard comments and block delete - if (line[0] == '(') { return(gc.status_code); } - if (line[0] == '/') { char_counter++; } // ignore block delete - // Pass 1: Commands while(next_statement(&letter, &value, line, &char_counter)) { int_value = trunc(value); diff --git a/protocol.c b/protocol.c index 1275f8d..2e2acd3 100644 --- a/protocol.c +++ b/protocol.c @@ -72,20 +72,44 @@ uint8_t protocol_execute_line(char *line) { void protocol_process() { char c; + uint8_t iscomment = false; while((c = serial_read()) != SERIAL_NO_DATA) { - if((char_counter > 0) && ((c == '\n') || (c == '\r'))) { // Line is complete. Then execute! - line[char_counter] = 0; // treminate string - status_message(protocol_execute_line(line)); - char_counter = 0; // reset line buffer index - } else if (c <= ' ') { - // Throw away whitepace and control characters - } else if (char_counter >= LINE_BUFFER_SIZE-1) { - // Throw away any characters beyond the end of the line buffer - } else if (c >= 'a' && c <= 'z') { // Upcase lowercase - line[char_counter++] = c-'a'+'A'; + if ((c == '\n') || (c == '\r')) { // End of block reached + if (char_counter > 0) {// Line is complete. Then execute! + line[char_counter] = 0; // terminate string + status_message(protocol_execute_line(line)); + } else { + // Empty or comment line. Skip block. + status_message(STATUS_OK); // Send status message for syncing purposes. + } + char_counter = 0; // Reset line buffer index + iscomment = false; // Reset comment flag } else { - line[char_counter++] = c; + if (iscomment) { + // Throw away all comment characters + if (c == ')') { + // End of comment. Resume line. + iscomment = false; + } + } else { + if (c <= ' ') { + // Throw away whitepace and control characters + } else if (c == '/') { + // Disable block delete and throw away character + // To enable block delete, uncomment following line. Will ignore until EOL. + // iscomment = true; + } else if (c == '(') { + // Enable comments flag and ignore all characters until ')' or EOL. + iscomment = true; + } else if (char_counter >= LINE_BUFFER_SIZE-1) { + // Throw away any characters beyond the end of the line buffer + } else if (c >= 'a' && c <= 'z') { // Upcase lowercase + line[char_counter++] = c-'a'+'A'; + } else { + line[char_counter++] = c; + } + } } } } From 5c2150daa9b6ca9c7f3be32f37907cf1f7b46f6f Mon Sep 17 00:00:00 2001 From: Sonny J Date: Sat, 3 Sep 2011 15:31:48 -0600 Subject: [PATCH 62/82] Significantly improved junction control and fixed computation bugs in planner - Junction jerk now re-defined as junction_deviation. The distance from the junction to the edge of a circle tangent to both previous and current path lines. The circle radii is used to compute the maximum junction velocity by centripetal acceleration. More robust and simplified way to compute jerk. - Fixed bugs related to entry and exit factors. They were computed based on the current nominal speeds but not when computing exit factors for neighboring blocks. Removed factors and replaced with entry speeds only. Factors now only computed for stepper trapezoid rate conversions. - Misc: Added min(), next_block_index, prev_block_index functions for clarity. --- nuts_bolts.h | 1 + planner.c | 217 +++++++++++++++++++++++++-------------------------- planner.h | 8 +- settings.c | 12 +-- settings.h | 2 +- 5 files changed, 119 insertions(+), 121 deletions(-) diff --git a/nuts_bolts.h b/nuts_bolts.h index e5a2ea8..dc19801 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -33,6 +33,7 @@ #define clear_vector(a) memset(a, 0, sizeof(a)) #define max(a,b) (((a) > (b)) ? (a) : (b)) +#define min(a,b) (((a) < (b)) ? (a) : (b)) // Read a floating point value from a string. Line points to the input buffer, char_counter // is the indexer pointing to the current character of the line, while double_ptr is diff --git a/planner.c b/planner.c index 9856489..0a4cdbe 100644 --- a/planner.c +++ b/planner.c @@ -3,7 +3,8 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - + Modifications Copyright (c) 2011 Sungeun 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 @@ -48,15 +49,28 @@ static uint8_t acceleration_manager_enabled; // Acceleration management active #define ONE_MINUTE_OF_MICROSECONDS 60000000.0 + +// Returns the index of the next block in the ring buffer +static int8_t next_block_index(int8_t block_index) { + return( (block_index + 1) % BLOCK_BUFFER_SIZE ); +} + + +// Returns the index of the previous block in the ring buffer +static int8_t prev_block_index(int8_t block_index) { + block_index--; + if (block_index < 0) { block_index = BLOCK_BUFFER_SIZE-1; } + return(block_index); +} + + // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // given acceleration: static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) { - return( - (target_rate*target_rate-initial_rate*initial_rate)/ - (2L*acceleration) - ); + return( (target_rate*target_rate-initial_rate*initial_rate)/(2L*acceleration) ); } + /* + <- some maximum rate we don't care about /|\ / | \ @@ -66,97 +80,53 @@ static double estimate_acceleration_distance(double initial_rate, double target_ ^ ^ | | intersection_distance distance */ - - // This function gives you the point at which you must start braking (at the rate of -acceleration) if // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after // a total travel of distance. This can be used to compute the intersection point between acceleration and // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) - static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) { - return( - (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ - (4*acceleration) - ); + return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration) ); } - + // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // acceleration within the allotted distance. static double max_allowable_speed(double acceleration, double target_velocity, double distance) { - return( - sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) - ); -} - -// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks. -// This method will calculate the junction jerk as the euclidean distance between the nominal -// velocities of the respective blocks. -static double junction_jerk(block_t *before, block_t *after) { - return(sqrt( - pow(before->speed_x-after->speed_x, 2)+ - pow(before->speed_y-after->speed_y, 2)+ - pow(before->speed_z-after->speed_z, 2)) - ); -} - -// Calculate a braking factor to reach baseline speed which is max_jerk/2, e.g. the -// speed under which you cannot exceed max_jerk no matter what you do. -static double factor_for_safe_speed(block_t *block) { - if(settings.max_jerk < block->nominal_speed) { - return(settings.max_jerk/block->nominal_speed); - } else { - return(1.0); - } + return( sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) ); } // The kernel called by planner_recalculate() when scanning the plan from last to first entry. static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { - if(!current) { return; } - - double entry_factor = 1.0; - double exit_factor; - if (next) { - exit_factor = next->entry_factor; - } else { - exit_factor = factor_for_safe_speed(current); - } - - // Calculate the entry_factor for the current block. + if (!current) { return; } + // Calculate the entry speed for the current block. if (previous) { - // Reduce speed so that junction_jerk is within the maximum allowed - double jerk = junction_jerk(previous, current); - if (jerk > settings.max_jerk) { - entry_factor = (settings.max_jerk/jerk); - } + double entry_speed = current->max_entry_speed; + double exit_speed; + if (next) { + exit_speed = next->entry_speed; + } else { + exit_speed = 0.0; + } // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly. - if (entry_factor > exit_factor) { - double max_entry_speed = max_allowable_speed(-settings.acceleration,current->nominal_speed*exit_factor, - current->millimeters); - double max_entry_factor = max_entry_speed/current->nominal_speed; - if (max_entry_factor < entry_factor) { - entry_factor = max_entry_factor; - } + if (entry_speed > exit_speed) { + entry_speed = + min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed); } + current->entry_speed = entry_speed; } else { - entry_factor = factor_for_safe_speed(current); + current->entry_speed = 0.0; } - - // Store result - current->entry_factor = entry_factor; } + // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This // implements the reverse pass. static void planner_reverse_pass() { auto int8_t block_index = block_buffer_head; block_t *block[3] = {NULL, NULL, NULL}; while(block_index != block_buffer_tail) { - block_index--; - if(block_index < 0) { - block_index = BLOCK_BUFFER_SIZE-1; - } + block_index = prev_block_index( block_index ); block[2]= block[1]; block[1]= block[0]; block[0] = &block_buffer[block_index]; @@ -165,25 +135,23 @@ static void planner_reverse_pass() { planner_reverse_pass_kernel(NULL, block[0], block[1]); } + // The kernel called by planner_recalculate() when scanning the plan from first to last entry. static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) { if(!current) { return; } // If the previous block is an acceleration block, but it is not long enough to - // complete the full speed change within the block, we need to adjust out entry - // speed accordingly. Remember current->entry_factor equals the exit factor of - // the previous block.¨ + // complete the full speed change within the block, we need to adjust the entry + // speed accordingly. if(previous) { - if(previous->entry_factor < current->entry_factor) { - double max_entry_speed = max_allowable_speed(-settings.acceleration, - current->nominal_speed*previous->entry_factor, previous->millimeters); - double max_entry_factor = max_entry_speed/current->nominal_speed; - if (max_entry_factor < current->entry_factor) { - current->entry_factor = max_entry_factor; - } + if (previous->entry_speed < current->entry_speed) { + current->entry_speed = + min( max_allowable_speed(-settings.acceleration,current->entry_speed,previous->millimeters), + min( current->max_entry_speed, current->entry_speed ) ); } } } + // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This // implements the forward pass. static void planner_forward_pass() { @@ -195,11 +163,12 @@ static void planner_forward_pass() { block[1] = block[2]; block[2] = &block_buffer[block_index]; planner_forward_pass_kernel(block[0],block[1],block[2]); - block_index = (block_index+1) % BLOCK_BUFFER_SIZE; + block_index = next_block_index( block_index ); } planner_forward_pass_kernel(block[1], block[2], NULL); } + /* +--------+ <- nominal_rate / \ @@ -208,10 +177,10 @@ static void planner_forward_pass() { +-------------+ time --> */ - // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. // The factors represent a factor of braking and must be in the range 0.0-1.0. static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { + block->initial_rate = ceil(block->nominal_rate*entry_factor); block->final_rate = ceil(block->nominal_rate*exit_factor); int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; @@ -236,6 +205,7 @@ static void calculate_trapezoid_for_block(block_t *block, double entry_factor, d block->decelerate_after = accelerate_steps+plateau_steps; } + // Recalculates the trapezoid speed profiles for all blocks in the plan according to the // entry_factor for each junction. Must be called by planner_recalculate() after // updating the blocks. @@ -243,34 +213,37 @@ static void planner_recalculate_trapezoids() { int8_t block_index = block_buffer_tail; block_t *current; block_t *next = NULL; - + while(block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; if (current) { - calculate_trapezoid_for_block(current, current->entry_factor, next->entry_factor); + // Compute entry and exit factors for trapezoid calculations + double entry_factor = current->entry_speed/current->nominal_speed; + double exit_factor = next->entry_speed/current->nominal_speed; + calculate_trapezoid_for_block(current, entry_factor, exit_factor); } - block_index = (block_index+1) % BLOCK_BUFFER_SIZE; + block_index = next_block_index( block_index ); } - calculate_trapezoid_for_block(next, next->entry_factor, factor_for_safe_speed(next)); + calculate_trapezoid_for_block(next, next->entry_speed, 0.0); } // 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_factor) +// 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 jerk is within the set limit +// a. The maximum junction speed is within the set limit // b. No speed reduction within one block requires faster deceleration than the one, true constant // acceleration. -// 2. Go over every block in chronological order and dial down junction speed reduction values if -// a. The speed increase within one block would require faster accelleration than the one, true +// 2. Go over every block in chronological order and dial down junction speed values if +// a. The speed increase within one block would require faster acceleration than the one, true // constant acceleration. // -// When these stages are complete all blocks have an entry_factor that will allow all speed changes to -// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than -// the set limit. Finally it will: +// When these stages are complete all blocks have an entry speed that will allow all speed changes to +// be performed using only the one, true constant acceleration, and where no junction speed is greater +// than the set limit. Finally it will: // -// 3. Recalculate trapezoids for all blocks using the recently updated factors +// 3. Recalculate trapezoids for all blocks using the recently updated junction speeds. static void planner_recalculate() { planner_reverse_pass(); @@ -298,7 +271,7 @@ int plan_is_acceleration_manager_enabled() { void plan_discard_current_block() { if (block_buffer_head != block_buffer_tail) { - block_buffer_tail = (block_buffer_tail + 1) % BLOCK_BUFFER_SIZE; + block_buffer_tail = next_block_index( block_buffer_tail ); } } @@ -307,6 +280,7 @@ block_t *plan_get_current_block() { return(&block_buffer[block_buffer_tail]); } + // 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 // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. @@ -320,10 +294,11 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); // Calculate the buffer head after we push this byte - int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE; - // If the buffer is full: good! That means we are well ahead of the robot. - // Rest here until there is room in the buffer. + int next_buffer_head = next_block_index( block_buffer_head ); + // If the buffer is full: good! That means we are well ahead of the robot. + // Rest here until there is room in the buffer. while(block_buffer_tail == next_buffer_head) { sleep_mode(); } + // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // Number of steps for each axis @@ -331,15 +306,16 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); block->steps_z = labs(target[Z_AXIS]-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; }; - double delta_x_mm = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS]; - double delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; - double delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; - block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm)); + block->delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS]; + block->delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; + block->delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; + block->millimeters = sqrt(square(block->delta_mm[X_AXIS]) + square(block->delta_mm[Y_AXIS]) + + square(block->delta_mm[Z_AXIS])); - uint32_t microseconds; if (!invert_feed_rate) { microseconds = lround((block->millimeters/feed_rate)*1000000); @@ -349,17 +325,16 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // Calculate speed in mm/minute for each axis double multiplier = 60.0*1000000.0/microseconds; - block->speed_x = delta_x_mm * multiplier; - block->speed_y = delta_y_mm * multiplier; - block->speed_z = delta_z_mm * multiplier; + block->speed_x = block->delta_mm[X_AXIS] * multiplier; + block->speed_y = block->delta_mm[Y_AXIS] * multiplier; + block->speed_z = block->delta_mm[Z_AXIS] * multiplier; block->nominal_speed = block->millimeters * multiplier; block->nominal_rate = ceil(block->step_event_count * multiplier); - block->entry_factor = 0.0; // This is a temporary fix to avoid a situation where very low nominal_speeds would be rounded // down to zero and cause a division by zero. TODO: Grbl deserves a less patchy fix for this problem if (block->nominal_speed < 60.0) { block->nominal_speed = 60.0; } - + // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line // average travel per step event changes. For a line along one axis the travel per step event // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both @@ -370,10 +345,32 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert block->rate_delta = ceil( ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick travel_per_step); // convert to: acceleration steps/min/acceleration_tick - if (acceleration_manager_enabled) { - // compute a preliminary conservative acceleration trapezoid - double safe_speed_factor = factor_for_safe_speed(block); - calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor); + + if (acceleration_manager_enabled) { + // Compute initial trapazoid and maximum entry speed at junction + double vmax_junction = 0.0; + // Skip first block, set default zero max junction speed. + if (block_buffer_head != block_buffer_tail) { + block_t *previous = &block_buffer[ prev_block_index(block_buffer_head) ]; + + // Compute cosine of angle between previous and current path + double cos_theta = ( -previous->delta_mm[X_AXIS] * block->delta_mm[X_AXIS] + + -previous->delta_mm[Y_AXIS] * block->delta_mm[Y_AXIS] + + -previous->delta_mm[Z_AXIS] * block->delta_mm[Z_AXIS] )/ + ( previous->millimeters * block->millimeters ); + + // Avoid divide by zero and set zero max junction velocity for highly acute angles. + if (cos_theta < 0.9) { + // Compute maximum junction velocity based on maximum acceleration and junction deviation + double sin_theta_d2 = sqrt((1-cos_theta)/2); // Trig half angle identity + vmax_junction = + sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1-sin_theta_d2)); + vmax_junction = max(0.0,min(vmax_junction, min(previous->nominal_speed,block->nominal_speed))); + } + } + block->max_entry_speed = vmax_junction; + block->entry_speed = 0.0; + calculate_trapezoid_for_block(block, block->entry_speed, 0.0); } else { block->initial_rate = block->nominal_rate; block->final_rate = block->nominal_rate; diff --git a/planner.h b/planner.h index c39e8fc..e99ce78 100644 --- a/planner.h +++ b/planner.h @@ -36,10 +36,10 @@ typedef struct { double speed_x, speed_y, speed_z; // Nominal mm/minute for each axis double nominal_speed; // The nominal speed for this block in mm/min double millimeters; // The total travel of this block in mm - double entry_factor; // The factor representing the change in speed at the start of this trapezoid. - // (The end of the curren speed trapezoid is defined by the entry_factor of the - // next block) - + double delta_mm[3]; // XYZ travel components of this block in mm + double entry_speed; // Entry speed at previous-current junction + double max_entry_speed; // Maximum allowable entry speed + // Settings for the trapezoid generator uint32_t initial_rate; // The jerk-adjusted step rate at start of block uint32_t final_rate; // The minimal rate at exit diff --git a/settings.c b/settings.c index 82544d3..791fca7 100644 --- a/settings.c +++ b/settings.c @@ -51,7 +51,7 @@ typedef struct { #define DEFAULT_RAPID_FEEDRATE 500.0 // in millimeters per minute #define DEFAULT_FEEDRATE 500.0 #define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/10.0) -#define DEFAULT_MAX_JERK 300.0 +#define DEFAULT_JUNCTION_DEVIATION 0.1 #define DEFAULT_STEPPING_INVERT_MASK ((1< Date: Sat, 3 Sep 2011 16:08:42 -0600 Subject: [PATCH 63/82] Add G02/03 arc conversion/pre-processor script and example streaming script Beta pre-processor script used to clean and streamline g-code for streaming and converts G02/03 arcs into linear segments. Allows for full acceleration support if the pre-processed g-code is then streamed to grill, sans G02/03 arcs. Added a simple example streaming script for Python users. --- script/grbl_preprocess.py | 224 ++++++++++++++++++++++++++++++++++++++ script/simple_stream.py | 33 ++++++ 2 files changed, 257 insertions(+) create mode 100755 script/grbl_preprocess.py create mode 100755 script/simple_stream.py diff --git a/script/grbl_preprocess.py b/script/grbl_preprocess.py new file mode 100755 index 0000000..4388b11 --- /dev/null +++ b/script/grbl_preprocess.py @@ -0,0 +1,224 @@ +#!/usr/bin/env python +"""\ +G-code preprocessor for grbl +- Converts G02/03 arcs to G01 linear interpolations +- Removes comments, block delete characters, and line numbers +- Removes spaces and capitalizes commands +- Minor input error checking +- OPTIONAL: Remove unsupported grbl G and M commands + +TODO: +- More robust error checking +- Improve interface to command line options +- Improve g-code parsing to NIST standards +- Fix problem with inverse feed rates +- Positioning updates may not be correct on grbl. Need to check. + +Based on GRBL 0.7b source code by Simen Svale Skogsrud + +Version: 20100825 +""" +import re +from math import * +from copy import * + +# -= SETTINGS =- +filein = 'test.gcode' # Input file name +fileout = 'grbl.gcode' # Output file name +ndigits_in = 4 # inch significant digits after '.' +ndigits_mm = 2 # mm significant digits after '.' +mm_per_arc_segment = 0.1 # mm per arc segment +inch2mm = 25.4 # inch to mm conversion scalar +verbose = False # Verbose flag to show all progress +remove_unsupported = True # Removal flag for all unsupported statements + +# Initialize parser state +gc = { 'current_xyz' : [0,0,0], + 'feed_rate' : 0, # F0 + 'motion_mode' : 'SEEK', # G00 + 'plane_axis' : [0,1,2], # G17 + 'inches_mode' : False, # G21 + 'inverse_feedrate_mode' : False, # G94 + 'absolute_mode' : True} # G90 + +def unit_conv(val) : # Converts value to mm + if gc['inches_mode'] : val *= inch2mm + return(val) + +def fout_conv(val) : # Returns converted value as rounded string for output file. + if gc['inches_mode'] : return( str(round(val/inch2mm,ndigits_in)) ) + else : return( str(round(val,ndigits_mm)) ) + +# Open g-code file +fin = open(filein,'r'); +fout = open(fileout,'w'); + +# Iterate through g-code file +l_count = 0 +for line in fin: + l_count += 1 # Iterate line counter + + # Strip comments/spaces/tabs/new line and capitalize. Comment MSG not supported. + block = re.sub('\s|\(.*?\)','',line).upper() + block = re.sub('\\\\','',block) # Strip \ block delete character + block = re.sub('%','',block) # Strip % program start/stop character + + if len(block) == 0 : # Ignore empty blocks + + print "Skipping: " + line.strip() + + else : # Process valid g-code clean block. Assumes no block delete characters or comments + + g_cmd = re.findall(r'[^0-9\.\-]+',block) # Extract block command characters + g_num = re.findall(r'[0-9\.\-]+',block) # Extract block numbers + + # G-code block error checks + # if len(g_cmd) != len(g_num) : print block; raise Exception('Invalid block. Unbalanced word and values.') + if 'N' in g_cmd : + if g_cmd[0]!='N': raise Exception('Line number must be first command in line.') + if g_cmd.count('N') > 1: raise Exception('More than one line number in block.') + g_cmd = g_cmd[1:] # Remove line number word + g_num = g_num[1:] + # Block item repeat checks? (0<=n'M'<5, G/M modal groups) + + # Initialize block state + blk = { 'next_action' : 'DEFAULT', + 'absolute_override' : False, + 'target_xyz' : deepcopy(gc['current_xyz']), + 'offset_ijk' : [0,0,0], + 'radius_mode' : False, + 'unsupported': [] } + + # Pass 1 + for cmd,num in zip(g_cmd,g_num) : + fnum = float(num) + inum = int(fnum) + if cmd is 'G' : + if inum is 0 : gc['motion_mode'] = 'SEEK' + elif inum is 1 : gc['motion_mode'] = 'LINEAR' + elif inum is 2 : gc['motion_mode'] = 'CW_ARC' + elif inum is 3 : gc['motion_mode'] = 'CCW_ARC' + elif inum is 4 : blk['next_action'] = 'DWELL' + elif inum is 17 : gc['plane_axis'] = [0,1,2] # Select XY Plane + elif inum is 18 : gc['plane_axis'] = [0,2,1] # Select XZ Plane + elif inum is 19 : gc['plane_axis'] = [1,2,0] # Select YZ Plane + elif inum is 20 : gc['inches_mode'] = True + elif inum is 21 : gc['inches_mode'] = False + elif inum in [28,30] : blk['next_action'] = 'GO_HOME' + elif inum is 53 : blk['absolute_override'] = True + elif inum is 80 : gc['motion_mode'] = 'MOTION_CANCEL' + elif inum is 90 : gc['absolute_mode'] = True + elif inum is 91 : gc['absolute_mode'] = False + elif inum is 92 : blk['next_action'] = 'SET_OFFSET' + elif inum is 93 : gc['inverse_feedrate_mode'] = True + elif inum is 94 : gc['inverse_feedrate_mode'] = False + else : + print 'Unsupported command ' + cmd + num + ' on line ' + str(l_count) + if remove_unsupported : blk['unsupported'].append(zip(g_cmd,g_num).index((cmd,num))) + elif cmd is 'M' : + if inum in [0,1] : pass # Program Pause + elif inum in [2,30,60] : pass # Program Completed + elif inum is 3 : pass # Spindle Direction 1 + elif inum is 4 : pass # Spindle Direction -1 + elif inum is 5 : pass # Spindle Direction 0 + else : + print 'Unsupported command ' + cmd + num + ' on line ' + str(l_count) + if remove_unsupported : blk['unsupported'].append(zip(g_cmd,g_num).index((cmd,num))) + elif cmd is 'T' : pass # Tool Number + + # Pass 2 + for cmd,num in zip(g_cmd,g_num) : + fnum = float(num) + if cmd is 'F' : gc['feed_rate'] = unit_conv(fnum) # Feed Rate + elif cmd in ['I','J','K'] : blk['offset_ijk'][ord(cmd)-ord('I')] = unit_conv(fnum) # Arc Center Offset + elif cmd is 'P' : p = fnum # Misc value parameter + elif cmd is 'R' : r = unit_conv(fnum); blk['radius_mode'] = True # Arc Radius Mode + elif cmd is 'S' : pass # Spindle Speed + elif cmd in ['X','Y','Z'] : # Target Coordinates + if (gc['absolute_mode'] | blk['absolute_override']) : + blk['target_xyz'][ord(cmd)-ord('X')] = unit_conv(fnum) + else : + blk['target_xyz'][ord(cmd)-ord('X')] += unit_conv(fnum) + + # Execute actions + if blk['next_action'] is 'GO_HOME' : + gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position + elif blk['next_action'] is 'SET_OFFSET' : + pass + elif blk['next_action'] is 'DWELL' : + if p < 0 : raise Exception('Dwell time negative.') + else : # 'DEFAULT' + if gc['motion_mode'] is 'SEEK' : + gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position + elif gc['motion_mode'] is 'LINEAR' : + gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position + elif gc['motion_mode'] in ['CW_ARC','CCW_ARC'] : + axis = gc['plane_axis'] + + # Convert radius mode to ijk mode + if blk['radius_mode'] : + x = blk['target_xyz'][axis[0]]-gc['current_xyz'][axis[0]] + y = blk['target_xyz'][axis[1]]-gc['current_xyz'][axis[1]] + if not (x==0 and y==0) : raise Exception('Same target and current XYZ not allowed in arc radius mode.') + h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y) + if isnan(h_x2_div_d) : raise Exception('Floating point error in arc conversion') + if gc['motion_mode'] is 'CCW_ARC' : h_x2_div_d = -h_x2_div_d + if r < 0 : h_x2_div_d = -h_x2_div_d + blk['offset_ijk'][axis[0]] = (x-(y*h_x2_div_d))/2; + blk['offset_ijk'][axis[1]] = (y+(x*h_x2_div_d))/2; + + # Compute arc center, radius, theta, and depth parameters + theta_start = atan2(-blk['offset_ijk'][axis[0]], -blk['offset_ijk'][axis[1]]) + theta_end = atan2(blk['target_xyz'][axis[0]] - blk['offset_ijk'][axis[0]] - gc['current_xyz'][axis[0]], \ + blk['target_xyz'][axis[1]] - blk['offset_ijk'][axis[1]] - gc['current_xyz'][axis[1]]) + if theta_end < theta_start : theta_end += 2*pi + radius = hypot(blk['offset_ijk'][axis[0]], blk['offset_ijk'][axis[1]]) + depth = blk['target_xyz'][axis[2]]-gc['current_xyz'][axis[2]] + center_x = gc['current_xyz'][axis[0]]-sin(theta_start)*radius + center_y = gc['current_xyz'][axis[1]]-cos(theta_start)*radius + + # Compute arc incremental linear segment parameters + angular_travel = theta_end-theta_start + if gc['motion_mode'] is 'CCW_ARC' : angular_travel = angular_travel-2*pi + millimeters_of_travel = hypot(angular_travel*radius, fabs(depth)) + if millimeters_of_travel is 0 : raise Exception('G02/03 arc travel is zero') + segments = int(round(millimeters_of_travel/mm_per_arc_segment)) + if segments is 0 : raise Exception('G02/03 zero length arc segment') +# ??? # if gc['inverse_feedrate_mode'] : gc['feed_rate'] *= segments + theta_per_segment = angular_travel/segments + depth_per_segment = depth/segments + + # Generate arc linear segments + if verbose: print 'Converting: '+ block + ' : ' + str(l_count) + fout.write('G01F'+fout_conv(gc['feed_rate'])) + if not gc['absolute_mode'] : fout.write('G90') + arc_target = [0,0,0] + for i in range(1,segments+1) : + if i < segments : + arc_target[axis[0]] = center_x + radius * sin(theta_start + i*theta_per_segment) + arc_target[axis[1]] = center_y + radius * cos(theta_start + i*theta_per_segment) + arc_target[axis[2]] = gc['current_xyz'][axis[2]] + i*depth_per_segment + else : + arc_target = deepcopy(blk['target_xyz']) # Last segment at target_xyz + # Write only changed variables. + if arc_target[0] != gc['current_xyz'][0] : fout.write('X'+fout_conv(arc_target[0])) + if arc_target[1] != gc['current_xyz'][1] : fout.write('Y'+fout_conv(arc_target[1])) + if arc_target[2] != gc['current_xyz'][2] : fout.write('Z'+fout_conv(arc_target[2])) + fout.write('\n') + gc['current_xyz'] = deepcopy(arc_target) # Update position + if not gc['absolute_mode'] : fout.write('G91\n') + + # Rebuild original gcode block sans line numbers, extra characters, and unsupported commands + if gc['motion_mode'] not in ['CW_ARC','CCW_ARC'] : + if remove_unsupported and len(blk['unsupported']) : + for i in blk['unsupported'][::-1] : del g_cmd[i]; del g_num[i] + out_block = "".join([i+j for (i,j) in zip(g_cmd,g_num)]) + if len(out_block) : + if verbose : print "Writing: " + out_block + ' : ' + str(l_count) + fout.write(out_block + '\n') + +print 'Done!' + +# Close files +fin.close() +fout.close() \ No newline at end of file diff --git a/script/simple_stream.py b/script/simple_stream.py new file mode 100755 index 0000000..8bd4b3b --- /dev/null +++ b/script/simple_stream.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python +"""\ +Simple g-code streaming script for grbl +""" + +import serial +import time + +# Open grbl serial port +s = serial.Serial('/dev/tty.usbmodem1811',9600) + +# Open g-code file +f = open('grbl.gcode','r'); + +# Wake up grbl +s.write("\r\n\r\n") +time.sleep(2) # Wait for grbl to initialize +s.flushInput() # Flush startup text in serial input + +# Stream g-code to grbl +for line in f: + l = line.strip() # Strip all EOL characters for consistency + print 'Sending: ' + l, + s.write(l + '\n') # Send g-code block to grbl + grbl_out = s.readline() # Wait for grbl response with carriage return + print ' : ' + grbl_out.strip() + +# Wait here until grbl is finished to close serial port and file. +raw_input(" Press to exit and disable grbl.") + +# Close file and serial port +f.close() +s.close() \ No newline at end of file From 5e2e935bda93d0923ffa385e9cafb7a751c65e34 Mon Sep 17 00:00:00 2001 From: Sonny J Date: Sat, 3 Sep 2011 23:22:27 -0600 Subject: [PATCH 64/82] Minor bug fixes in planner. --- planner.c | 40 ++++++++++++++++++---------------------- 1 file changed, 18 insertions(+), 22 deletions(-) diff --git a/planner.c b/planner.c index 0a4cdbe..8dbde44 100644 --- a/planner.c +++ b/planner.c @@ -99,24 +99,20 @@ static double max_allowable_speed(double acceleration, double target_velocity, d // The kernel called by planner_recalculate() when scanning the plan from last to first entry. static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { if (!current) { return; } - // Calculate the entry speed for the current block. - if (previous) { - double entry_speed = current->max_entry_speed; - double exit_speed; - if (next) { - exit_speed = next->entry_speed; - } else { - exit_speed = 0.0; - } - // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly. - if (entry_speed > exit_speed) { - entry_speed = - min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed); - } - current->entry_speed = entry_speed; + + double entry_speed = current->max_entry_speed; + double exit_speed; + if (next) { + exit_speed = next->entry_speed; } else { - current->entry_speed = 0.0; + exit_speed = 0.0; } + // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly. + if (entry_speed > exit_speed) { + entry_speed = + min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed); + } + current->entry_speed = entry_speed; } @@ -144,9 +140,8 @@ static void planner_forward_pass_kernel(block_t *previous, block_t *current, blo // speed accordingly. if(previous) { if (previous->entry_speed < current->entry_speed) { - current->entry_speed = - min( max_allowable_speed(-settings.acceleration,current->entry_speed,previous->millimeters), - min( current->max_entry_speed, current->entry_speed ) ); + current->entry_speed = min( min( current->entry_speed, current->max_entry_speed ), + max_allowable_speed(-settings.acceleration,previous->entry_speed,previous->millimeters) ); } } } @@ -359,18 +354,19 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert -previous->delta_mm[Z_AXIS] * block->delta_mm[Z_AXIS] )/ ( previous->millimeters * block->millimeters ); - // Avoid divide by zero and set zero max junction velocity for highly acute angles. - if (cos_theta < 0.9) { + // Avoid divide by zero for straight junctions around 180 degress + if (cos_theta > -0.95) { // Compute maximum junction velocity based on maximum acceleration and junction deviation double sin_theta_d2 = sqrt((1-cos_theta)/2); // Trig half angle identity vmax_junction = sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1-sin_theta_d2)); vmax_junction = max(0.0,min(vmax_junction, min(previous->nominal_speed,block->nominal_speed))); + } else { + vmax_junction = min(previous->nominal_speed,block->nominal_speed); } } block->max_entry_speed = vmax_junction; block->entry_speed = 0.0; - calculate_trapezoid_for_block(block, block->entry_speed, 0.0); } else { block->initial_rate = block->nominal_rate; block->final_rate = block->nominal_rate; From f1e5ff35ecf6dfd11c431e4e714c7bc6077ca52a Mon Sep 17 00:00:00 2001 From: Sonny J Date: Sun, 4 Sep 2011 11:19:08 -0600 Subject: [PATCH 65/82] More minor bug fixes in planner. Reverse planner was over-writing the initial/buffer tail entry speed, which reset the forward planner and caused it to lose track of its speed. Should now accelerate into short linear segments much nicer now. --- planner.c | 38 +++++++++++++++++++------------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/planner.c b/planner.c index 8dbde44..7eafc31 100644 --- a/planner.c +++ b/planner.c @@ -100,19 +100,21 @@ static double max_allowable_speed(double acceleration, double target_velocity, d static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { if (!current) { return; } - double entry_speed = current->max_entry_speed; - double exit_speed; - if (next) { - exit_speed = next->entry_speed; - } else { - exit_speed = 0.0; + if (previous) { // Prevent reverse planner from over-writing buffer_tail entry speed. + double entry_speed = current->max_entry_speed; // Re-write to ensure at max possible speed + double exit_speed; + if (next) { + exit_speed = next->entry_speed; + } else { + exit_speed = 0.0; // Assume last block has zero exit velocity + } + // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly. + if (entry_speed > exit_speed) { + entry_speed = + min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed); + } + current->entry_speed = entry_speed; } - // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly. - if (entry_speed > exit_speed) { - entry_speed = - min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed); - } - current->entry_speed = entry_speed; } @@ -354,19 +356,17 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert -previous->delta_mm[Z_AXIS] * block->delta_mm[Z_AXIS] )/ ( previous->millimeters * block->millimeters ); - // Avoid divide by zero for straight junctions around 180 degress + // Avoid divide by zero for straight junctions near 180 degrees. Limit to min nominal speeds. + vmax_junction = min(previous->nominal_speed,block->nominal_speed); if (cos_theta > -0.95) { // Compute maximum junction velocity based on maximum acceleration and junction deviation double sin_theta_d2 = sqrt((1-cos_theta)/2); // Trig half angle identity - vmax_junction = - sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1-sin_theta_d2)); - vmax_junction = max(0.0,min(vmax_junction, min(previous->nominal_speed,block->nominal_speed))); - } else { - vmax_junction = min(previous->nominal_speed,block->nominal_speed); + vmax_junction = max(0.0, min(vmax_junction, + sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1-sin_theta_d2)) ) ); } } block->max_entry_speed = vmax_junction; - block->entry_speed = 0.0; + block->entry_speed = vmax_junction; } else { block->initial_rate = block->nominal_rate; block->final_rate = block->nominal_rate; From d75ad82e4932aea166c189e4160e197e4710c191 Mon Sep 17 00:00:00 2001 From: Sonny J Date: Sun, 4 Sep 2011 18:53:25 -0600 Subject: [PATCH 66/82] Minor update for memory savings in ring buffer and fleshed out commenting. No changes in functionality. Path vectors moved from ring buffer to local planner static variables to save 3*(BUFFER_SIZE - 1) doubles in memory. Detailed comments. Really need to stop micro-updating. Should be the last until a planner optimization (ala Jens Geisler) has been completed. --- nuts_bolts.h | 1 + planner.c | 115 ++++++++++++++++++++++++++++++--------------------- planner.h | 1 - 3 files changed, 69 insertions(+), 48 deletions(-) diff --git a/nuts_bolts.h b/nuts_bolts.h index dc19801..0ed6f5c 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -32,6 +32,7 @@ #define Z_AXIS 2 #define clear_vector(a) memset(a, 0, sizeof(a)) +#define clear_vector_double(a) memset(a, 0.0, sizeof(a)) #define max(a,b) (((a) > (b)) ? (a) : (b)) #define min(a,b) (((a) < (b)) ? (a) : (b)) diff --git a/planner.c b/planner.c index 7eafc31..33b5931 100644 --- a/planner.c +++ b/planner.c @@ -42,8 +42,9 @@ static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion ins static volatile uint8_t block_buffer_head; // Index of the next block to be pushed static volatile uint8_t block_buffer_tail; // Index of the block to process now -// The current position of the tool in absolute steps -static int32_t position[3]; +static int32_t position[3]; // The current position of the tool in absolute steps +static double previous_unit_vec[3]; // Unit vector of previous path line segment +static double previous_nominal_speed; // Nominal speed of previous path line segment static uint8_t acceleration_manager_enabled; // Acceleration management active? @@ -67,7 +68,7 @@ static int8_t prev_block_index(int8_t block_index) { // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // given acceleration: static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) { - return( (target_rate*target_rate-initial_rate*initial_rate)/(2L*acceleration) ); + return( (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration) ); } @@ -100,21 +101,19 @@ static double max_allowable_speed(double acceleration, double target_velocity, d static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { if (!current) { return; } - if (previous) { // Prevent reverse planner from over-writing buffer_tail entry speed. - double entry_speed = current->max_entry_speed; // Re-write to ensure at max possible speed - double exit_speed; - if (next) { - exit_speed = next->entry_speed; - } else { - exit_speed = 0.0; // Assume last block has zero exit velocity - } - // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly. - if (entry_speed > exit_speed) { - entry_speed = - min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed); - } - current->entry_speed = entry_speed; + double entry_speed = current->max_entry_speed; // Re-write to ensure at max possible speed + double exit_speed; + if (next) { + exit_speed = next->entry_speed; + } else { + exit_speed = 0.0; // Assume last block has zero exit velocity } + // If the required deceleration across the block is too rapid, reduce the entry_speed accordingly. + if (entry_speed > exit_speed) { + entry_speed = + min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed); + } + current->entry_speed = entry_speed; } @@ -130,7 +129,7 @@ static void planner_reverse_pass() { block[0] = &block_buffer[block_index]; planner_reverse_pass_kernel(block[0], block[1], block[2]); } - planner_reverse_pass_kernel(NULL, block[0], block[1]); + // Skip buffer tail to prevent over-writing the initial entry speed. } @@ -204,7 +203,7 @@ static void calculate_trapezoid_for_block(block_t *block, double entry_factor, d // Recalculates the trapezoid speed profiles for all blocks in the plan according to the -// entry_factor for each junction. Must be called by planner_recalculate() after +// entry_speed for each junction. Must be called by planner_recalculate() after // updating the blocks. static void planner_recalculate_trapezoids() { int8_t block_index = block_buffer_tail; @@ -222,7 +221,7 @@ static void planner_recalculate_trapezoids() { } block_index = next_block_index( block_index ); } - calculate_trapezoid_for_block(next, next->entry_speed, 0.0); + calculate_trapezoid_for_block(next, next->entry_speed, 0.0); // Last block } // Recalculates the motion plan according to the following algorithm: @@ -253,6 +252,8 @@ void plan_init() { block_buffer_tail = 0; plan_set_acceleration_manager_enabled(true); clear_vector(position); + clear_vector_double(previous_unit_vec); + previous_nominal_speed = 0.0; } void plan_set_acceleration_manager_enabled(int enabled) { @@ -307,11 +308,13 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // Bail if this is a zero-length block if (block->step_event_count == 0) { return; }; - block->delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS]; - block->delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; - block->delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; - block->millimeters = sqrt(square(block->delta_mm[X_AXIS]) + square(block->delta_mm[Y_AXIS]) + - square(block->delta_mm[Z_AXIS])); + // Compute path vector in terms of quantized step target and current positions + double delta_mm[3]; + delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS]; + delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; + delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; + block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + + square(delta_mm[Z_AXIS])); uint32_t microseconds; if (!invert_feed_rate) { @@ -322,9 +325,9 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // Calculate speed in mm/minute for each axis double multiplier = 60.0*1000000.0/microseconds; - block->speed_x = block->delta_mm[X_AXIS] * multiplier; - block->speed_y = block->delta_mm[Y_AXIS] * multiplier; - block->speed_z = block->delta_mm[Z_AXIS] * multiplier; + block->speed_x = delta_mm[X_AXIS] * multiplier; + block->speed_y = delta_mm[Y_AXIS] * multiplier; + block->speed_z = delta_mm[Z_AXIS] * multiplier; block->nominal_speed = block->millimeters * multiplier; block->nominal_rate = ceil(block->step_event_count * multiplier); @@ -344,30 +347,47 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert travel_per_step); // convert to: acceleration steps/min/acceleration_tick if (acceleration_manager_enabled) { - // Compute initial trapazoid and maximum entry speed at junction - double vmax_junction = 0.0; - // Skip first block, set default zero max junction speed. - if (block_buffer_head != block_buffer_tail) { - block_t *previous = &block_buffer[ prev_block_index(block_buffer_head) ]; - + + // Compute path unit vector + double unit_vec[3]; + unit_vec[X_AXIS] = delta_mm[X_AXIS]/block->millimeters; + unit_vec[Y_AXIS] = delta_mm[Y_AXIS]/block->millimeters; + unit_vec[Z_AXIS] = delta_mm[Z_AXIS]/block->millimeters; + + // 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 edge of the circle. 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, which may be also viewed as path width or max_jerk. + double vmax_junction = 0.0; // Set default zero max junction speed + + // Use default for first block or when planner is reset by previous_nominal_speed = 0.0 + if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path - double cos_theta = ( -previous->delta_mm[X_AXIS] * block->delta_mm[X_AXIS] + - -previous->delta_mm[Y_AXIS] * block->delta_mm[Y_AXIS] + - -previous->delta_mm[Z_AXIS] * block->delta_mm[Z_AXIS] )/ - ( previous->millimeters * block->millimeters ); - - // Avoid divide by zero for straight junctions near 180 degrees. Limit to min nominal speeds. - vmax_junction = min(previous->nominal_speed,block->nominal_speed); - if (cos_theta > -0.95) { + double cos_theta = ( -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + + -previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + + -previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ); + + // Avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. + vmax_junction = min(previous_nominal_speed,block->nominal_speed); + if (cos_theta > -1.0) { // Compute maximum junction velocity based on maximum acceleration and junction deviation - double sin_theta_d2 = sqrt((1-cos_theta)/2); // Trig half angle identity + double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity vmax_junction = max(0.0, min(vmax_junction, - sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1-sin_theta_d2)) ) ); + sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ) ); } } + block->max_entry_speed = vmax_junction; block->entry_speed = vmax_junction; + + // Update previous path unit_vector and nominal speed + memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] + previous_nominal_speed = block->nominal_speed; + } else { + // Set at nominal rates only for disabled acceleration planner block->initial_rate = block->nominal_rate; block->final_rate = block->nominal_rate; block->accelerate_until = 0; @@ -383,16 +403,17 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // Move buffer head block_buffer_head = next_buffer_head; - // Update position + // Update position memcpy(position, target, sizeof(target)); // position[] = target[] - + if (acceleration_manager_enabled) { planner_recalculate(); } st_wake_up(); } -// Reset the planner position vector +// Reset the planner position vector and planner speed void plan_set_current_position(double x, double y, double z) { position[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); position[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); + previous_nominal_speed = 0.0; } diff --git a/planner.h b/planner.h index e99ce78..ba99fae 100644 --- a/planner.h +++ b/planner.h @@ -36,7 +36,6 @@ typedef struct { double speed_x, speed_y, speed_z; // Nominal mm/minute for each axis double nominal_speed; // The nominal speed for this block in mm/min double millimeters; // The total travel of this block in mm - double delta_mm[3]; // XYZ travel components of this block in mm double entry_speed; // Entry speed at previous-current junction double max_entry_speed; // Maximum allowable entry speed From ffcc3470a31411b63aa57d12cc58cc306a1fb660 Mon Sep 17 00:00:00 2001 From: Sonny J Date: Tue, 6 Sep 2011 19:39:14 -0600 Subject: [PATCH 67/82] Optimized planner re-write. Significantly faster. Full arc support enabled by rotation matrix approach. - Significant improvements in the planner. Removed or reordered repetitive and expensive calculations by order of importance: recalculating unchanged blocks, trig functions [sin(), cos(), tan()], sqrt(), divides, and multiplications. Blocks long enough for nominal speed to be guaranteed to be reached ignored by planner. Done by introducing two uint8_t flags per block. Reduced computational overhead by an order of magnitude. - Arc motion generation completely re-written and optimized. Now runs with acceleration planner. Removed all but one trig function (atan2) from initialization. Streamlined computations. Segment target locations generated by vector transformation and small angle approximation. Arc path correction implemented for accumulated error of approximation and single precision calculation of Arduino. Bug fix in message passing. --- gcode.c | 79 ++++---------- motion_control.c | 127 +++++++++++++++++------ motion_control.h | 16 +-- nuts_bolts.h | 1 + planner.c | 211 ++++++++++++++++++++++++-------------- planner.h | 11 +- protocol.c | 1 + script/grbl_preprocess.py | 3 +- settings.c | 1 + settings.h | 3 +- 10 files changed, 274 insertions(+), 179 deletions(-) diff --git a/gcode.c b/gcode.c index 62fec77..511bc59 100644 --- a/gcode.c +++ b/gcode.c @@ -3,7 +3,8 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - + Modifications Copyright (c) 2011 Sungeun (Sonny) 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 @@ -97,25 +98,6 @@ static float to_millimeters(double value) { return(gc.inches_mode ? (value * MM_PER_INCH) : value); } -#ifdef __AVR_ATmega328P__ -// Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis, -// positive to the right. -static double theta(double x, double y) -{ - double theta = atan(x/fabs(y)); - if (y>0) { - return(theta); - } else { - if (theta>0) - { - return(M_PI-theta); - } else { - return(-M_PI-theta); - } - } -} -#endif - // 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. @@ -125,7 +107,7 @@ uint8_t gc_execute_line(char *line) { double value; double unit_converted_value; double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified - int radius_mode = false; + uint8_t radius_mode = false; uint8_t absolute_override = false; /* 1 = absolute motion for this block only {G53} */ uint8_t next_action = NEXT_ACTION_DEFAULT; /* The action that will be taken by the parsed line */ @@ -331,50 +313,29 @@ uint8_t gc_execute_line(char *line) { // 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; } + 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] = (x-(y*h_x2_div_d))/2; - offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2; - } - - /* - This segment sets up an clockwise or counterclockwise arc from the current position to the target position around - the center designated by the offset vector. All theta-values measured in radians of deviance from the positive - y-axis. + 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)); - | <- theta == 0 - * * * - * * - * * - * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2) - * / - C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4)) + } else { // Offset mode specific computations + + r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc - */ - - // calculate the theta (angle) of the current point - double theta_start = theta(-offset[gc.plane_axis_0], -offset[gc.plane_axis_1]); - // calculate the theta (angle) of the target point - double theta_end = theta(target[gc.plane_axis_0] - offset[gc.plane_axis_0] - gc.position[gc.plane_axis_0], - target[gc.plane_axis_1] - offset[gc.plane_axis_1] - gc.position[gc.plane_axis_1]); - // ensure that the difference is positive so that we have clockwise travel - if (theta_end < theta_start) { theta_end += 2*M_PI; } - double angular_travel = theta_end-theta_start; - // Invert angular motion if the g-code wanted a counterclockwise arc - if (gc.motion_mode == MOTION_MODE_CCW_ARC) { - angular_travel = angular_travel-2*M_PI; } - // Find the radius - double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); - // Calculate the motion along the depth axis of the helix - double depth = target[gc.plane_axis_2]-gc.position[gc.plane_axis_2]; + + // Set clockwise/counter-clockwise sign for mc_arc computations + int8_t clockwise_sign = 1; + if (gc.motion_mode == MOTION_MODE_CW_ARC) { clockwise_sign = -1; } + // Trace the arc - mc_arc(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, + mc_arc(gc.position, target, 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, - gc.position); - // Finish off with a line to make sure we arrive exactly where we think we are - 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); + r, clockwise_sign); + break; #endif } diff --git a/motion_control.c b/motion_control.c index ef4a6b3..bf2ea44 100644 --- a/motion_control.c +++ b/motion_control.c @@ -3,7 +3,8 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - + Modifications Copyright (c) 2011 Sungeun (Sonny) 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 @@ -28,6 +29,8 @@ #include "stepper.h" #include "planner.h" +#define N_ARC_CORRECTION 25 // (0-255) Number of iterations before arc trajectory correction + void mc_dwell(uint32_t milliseconds) { @@ -35,51 +38,117 @@ void mc_dwell(uint32_t milliseconds) _delay_ms(milliseconds); } -// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc, -// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the -// circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining -// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion. -// position is a pointer to a vector representing the current position in millimeters. +// Execute an arc in offset mode format. position == current xyz, target == target xyz, +// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is +// the direction of helical travel, radius == circle radius, clockwise_sign == -1 or 1. Used +// for vector transformation direction. +// position, target, and offset are pointers to vectors from gcode.c #ifdef __AVR_ATmega328P__ // The arc is approximated by generating a huge number of tiny, linear segments. The length of each // segment is configured in settings.mm_per_arc_segment. -void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, - int axis_linear, double feed_rate, int invert_feed_rate, double *position) +void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, + uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t clockwise_sign) { - int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); - plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc - double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel)); +// int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); +// plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc + + double center_axis0 = position[axis_0] + offset[axis_0]; + double center_axis1 = position[axis_1] + offset[axis_1]; + double linear_travel = target[axis_linear] - position[axis_linear]; + double r_axis0 = -offset[axis_0]; // Radius vector from center to current location + double r_axis1 = -offset[axis_1]; + double rt_axis0 = target[axis_0] - center_axis0; + double rt_axis1 = target[axis_1] - center_axis1; + + // CCW angle between position and target from circle center. Only one atan2() trig computation required. + double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); + if (angular_travel < 0) { angular_travel += 2*M_PI; } + if (clockwise_sign < 0) { angular_travel = 2*M_PI-angular_travel; } + + double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel)); if (millimeters_of_travel == 0.0) { return; } - uint16_t segments = round(millimeters_of_travel/settings.mm_per_arc_segment); + uint16_t segments = floor(millimeters_of_travel/settings.mm_per_arc_segment); // Multiply inverse feed_rate to compensate for the fact that this movement is approximated // by a number of discrete segments. The inverse feed_rate should be correct for the sum of // all segments. if (invert_feed_rate) { feed_rate *= segments; } - // The angular motion for each segment + double theta_per_segment = angular_travel/segments; - // The linear motion for each segment double linear_per_segment = linear_travel/segments; - // Compute the center of this circle - double center_x = position[axis_1]-sin(theta)*radius; - double center_y = position[axis_2]-cos(theta)*radius; - // a vector to track the end point of each segment - double target[3]; - int i; + + /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, + and phi is the angle of rotation. Based on the solution approach by Jens Geisler. + r_T = [cos(phi) -sin(phi); + sin(phi) cos(phi] * r ; + + For arc generation, the center of the circle is the axis of rotation and the radius vector is + defined from the circle center to the initial position. Each line segment is formed by successive + vector rotations. This requires only two cos() and sin() computations to form the rotation + matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since + all double numbers are single precision on the Arduino. (True double precision will not have + round off issues for CNC applications.) Single precision error can accumulate to be greater than + tool precision in some cases. Therefore, arc path correction is implemented. + + Small angle approximation may be used to reduce computation overhead further. This approximation + holds for everything, but very small circles and large mm_per_arc_segment values. In other words, + theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large + to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for + numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an + issue for CNC machines with the single precision Arduino calculations. + + This approximation also allows mc_arc to immediately insert a line segment into the planner + without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied + a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. + This is important when there are successive arc motions. + */ + // Vector rotation matrix values + double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation + double sin_T = clockwise_sign*theta_per_segment; + + double trajectory[3]; + double sin_Ti; + double cos_Ti; + double r_axisi; + uint16_t i; + int8_t count = 0; + // Initialize the linear axis - target[axis_linear] = position[axis_linear]; - for (i=0; imax_entry_speed; // Re-write to ensure at max possible speed - double exit_speed; - if (next) { - exit_speed = next->entry_speed; - } else { - exit_speed = 0.0; // Assume last block has zero exit velocity + double entry_speed_sqr = current->max_entry_speed_sqr; // Reset and check to ensure max possible speed + + // If nominal length true, nominal speed is guaranteed to be reached. No need to re-compute. + // But, if forward planner changed entry speed, reset to max entry speed just to be sure. + if (!current->nominal_length_flag) { + if (next) { + // If the required deceleration across the block is too rapid, reduce entry_speed_sqr accordingly. + if (entry_speed_sqr > next->entry_speed_sqr) { + entry_speed_sqr = min( entry_speed_sqr, + max_allowable_speed_sqr(-settings.acceleration,next->entry_speed_sqr,current->millimeters)); + } + } else { + // Assume last block has zero exit velocity. + entry_speed_sqr = min( entry_speed_sqr, + max_allowable_speed_sqr(-settings.acceleration,0.0,current->millimeters)); + } + } + + // Check for junction speed change + if (current->entry_speed_sqr != entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + current->recalculate_flag = true; // Note: Newest block already set to true } - // If the required deceleration across the block is too rapid, reduce the entry_speed accordingly. - if (entry_speed > exit_speed) { - entry_speed = - min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed); - } - current->entry_speed = entry_speed; } @@ -136,13 +147,26 @@ static void planner_reverse_pass() { // The kernel called by planner_recalculate() when scanning the plan from first to last entry. static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) { if(!current) { return; } - // If the previous block is an acceleration block, but it is not long enough to - // complete the full speed change within the block, we need to adjust the entry - // speed accordingly. + if(previous) { - if (previous->entry_speed < current->entry_speed) { - current->entry_speed = min( min( current->entry_speed, current->max_entry_speed ), - max_allowable_speed(-settings.acceleration,previous->entry_speed,previous->millimeters) ); + + // If nominal length true, nominal speed is guaranteed to be reached. No need to recalculate. + if (!previous->nominal_length_flag) { + // If the previous block is an acceleration block, but it is not long enough to + // complete the full speed change within the block, we need to adjust the entry + // speed accordingly. + if (previous->entry_speed_sqr < current->entry_speed_sqr) { + double entry_speed_sqr = min( current->entry_speed_sqr, current->max_entry_speed_sqr ); + entry_speed_sqr = min( entry_speed_sqr, + max_allowable_speed_sqr(-settings.acceleration,previous->entry_speed_sqr,previous->millimeters) ); + + // Check for junction speed change + if (current->entry_speed_sqr != entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + current->recalculate_flag = true; + } + } + } } } @@ -165,7 +189,7 @@ static void planner_forward_pass() { } -/* +/* STEPPER RATE DEFINITION +--------+ <- nominal_rate / \ nominal_rate*entry_factor -> + \ @@ -175,6 +199,7 @@ static void planner_forward_pass() { */ // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. // The factors represent a factor of braking and must be in the range 0.0-1.0. +// This converts the planner parameters to the data required by the stepper controller. static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { block->initial_rate = ceil(block->nominal_rate*entry_factor); @@ -201,10 +226,19 @@ static void calculate_trapezoid_for_block(block_t *block, double entry_factor, d block->decelerate_after = accelerate_steps+plateau_steps; } - -// Recalculates the trapezoid speed profiles for all blocks in the plan according to the -// entry_speed for each junction. Must be called by planner_recalculate() after -// updating the blocks. +/* PLANNER SPEED DEFINITION + +--------+ <- current->nominal_speed + / \ + current->entry_speed -> + \ + | + <- next->entry_speed + +-------------+ + time --> +*/ +// Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the +// entry_speed for each junction and the entry_speed of the next junction. Must be called by +// planner_recalculate() after updating the blocks. Any recalulate flagged junction will +// compute the two adjacent trapezoids to the junction, since the junction speed corresponds +// to exit speed and entry speed of one another. static void planner_recalculate_trapezoids() { int8_t block_index = block_buffer_tail; block_t *current; @@ -214,21 +248,28 @@ static void planner_recalculate_trapezoids() { current = next; next = &block_buffer[block_index]; if (current) { - // Compute entry and exit factors for trapezoid calculations - double entry_factor = current->entry_speed/current->nominal_speed; - double exit_factor = next->entry_speed/current->nominal_speed; - calculate_trapezoid_for_block(current, entry_factor, exit_factor); + // Recalculate if current block entry or exit junction speed has changed. + if (current->recalculate_flag || next->recalculate_flag) { + // Compute entry and exit factors for trapezoid calculations. + // NOTE: sqrt(square velocities) now performed only when required in trapezoid calculation. + double entry_factor = sqrt( current->entry_speed_sqr ) / current->nominal_speed; + double exit_factor = sqrt( next->entry_speed_sqr ) / current->nominal_speed; + calculate_trapezoid_for_block(current, entry_factor, exit_factor); + current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + } } block_index = next_block_index( block_index ); } - calculate_trapezoid_for_block(next, next->entry_speed, 0.0); // Last block + // Last/newest block in buffer. Exit speed is zero. + calculate_trapezoid_for_block(next, sqrt( next->entry_speed_sqr ) / next->nominal_speed, 0.0); + next->recalculate_flag = false; } // Recalculates the motion plan according to the following algorithm: // // 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed) // so that: -// a. The maximum junction speed is within the set limit +// a. The junction speed is equal to or less than the maximum junction speed limit // b. No speed reduction within one block requires faster deceleration than the one, true constant // acceleration. // 2. Go over every block in chronological order and dial down junction speed values if @@ -237,9 +278,10 @@ static void planner_recalculate_trapezoids() { // // When these stages are complete all blocks have an entry speed that will allow all speed changes to // be performed using only the one, true constant acceleration, and where no junction speed is greater -// than the set limit. Finally it will: +// than the max limit. Finally it will: // -// 3. Recalculate trapezoids for all blocks using the recently updated junction speeds. +// 3. Recalculate trapezoids for all blocks using the recently updated junction speeds. Block trapezoids +// with no updated junction speeds will not be recalculated and assumed ok as is. static void planner_recalculate() { planner_reverse_pass(); @@ -256,7 +298,7 @@ void plan_init() { previous_nominal_speed = 0.0; } -void plan_set_acceleration_manager_enabled(int enabled) { +void plan_set_acceleration_manager_enabled(uint8_t enabled) { if ((!!acceleration_manager_enabled) != (!!enabled)) { st_synchronize(); acceleration_manager_enabled = !!enabled; @@ -282,8 +324,7 @@ block_t *plan_get_current_block() { // 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 // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. -void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) { - // The target position of the tool in absolute steps +void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) { // Calculate target position in absolute steps int32_t target[3]; @@ -299,6 +340,13 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; + + // Compute direction bits for this block + block->direction_bits = 0; + if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<steps_x = labs(target[X_AXIS]-position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); @@ -308,7 +356,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // Bail if this is a zero-length block if (block->step_event_count == 0) { return; }; - // Compute path vector in terms of quantized step target and current positions + // Compute path vector in terms of absolute step target and current positions double delta_mm[3]; delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS]; delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; @@ -341,53 +389,68 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2). // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed // specifically for each line to compensate for this phenomenon: - double travel_per_step = block->millimeters/block->step_event_count; - block->rate_delta = ceil( - ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick - travel_per_step); // convert to: acceleration steps/min/acceleration_tick + double step_per_travel = block->step_event_count/block->millimeters; // Compute inverse to remove divide + block->rate_delta = step_per_travel * ceil( // convert to: acceleration steps/min/acceleration_tick + settings.acceleration*60.0 / ACCELERATION_TICKS_PER_SECOND ); // acceleration mm/sec/sec per acceleration_tick + // Perform planner-enabled calculations if (acceleration_manager_enabled) { - + // Compute path unit vector - double unit_vec[3]; - unit_vec[X_AXIS] = delta_mm[X_AXIS]/block->millimeters; - unit_vec[Y_AXIS] = delta_mm[Y_AXIS]/block->millimeters; - unit_vec[Z_AXIS] = delta_mm[Z_AXIS]/block->millimeters; + double unit_vec[3]; + double inv_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides + unit_vec[X_AXIS] = delta_mm[X_AXIS]*inv_millimeters; + unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inv_millimeters; + unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inv_millimeters; // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + // Does not actually deviate from path, but used as a robust way to compute cornering speeds. // 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 edge of the circle. 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, which may be also viewed as path width or max_jerk. - double vmax_junction = 0.0; // Set default zero max junction speed - - // Use default for first block or when planner is reset by previous_nominal_speed = 0.0 + // 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. + // NOTE: sqrt() removed for speed optimization. Related calculations in terms of square velocity. + + double vmax_junction_sqr = 0.0; // Set default zero max junction speed + + // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { - // Compute cosine of angle between previous and current path - double cos_theta = ( -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + - -previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + - -previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ); + // 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. + double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; - // Avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. - vmax_junction = min(previous_nominal_speed,block->nominal_speed); - if (cos_theta > -1.0) { - // Compute maximum junction velocity based on maximum acceleration and junction deviation - double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity - vmax_junction = max(0.0, min(vmax_junction, - sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ) ); + // Skip and use default zero max junction speed for 0 degree acute junction. + if (cos_theta < 1.0) { + vmax_junction_sqr = square( min(previous_nominal_speed,block->nominal_speed) ); + // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. + if (cos_theta > -1.0) { + // Compute maximum junction velocity based on maximum acceleration and junction deviation + double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. + vmax_junction_sqr = min(vmax_junction_sqr, + settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2) ); + } } } - - block->max_entry_speed = vmax_junction; - block->entry_speed = vmax_junction; + block->max_entry_speed_sqr = vmax_junction_sqr; + block->entry_speed_sqr = vmax_junction_sqr; + + // Initialize planner efficiency flags + // Set flag if block will always reach nominal speed regardless of entry/exit speeds. + if (block->nominal_speed <= sqrt(max_allowable_speed_sqr(-settings.acceleration,0.0,0.5*block->millimeters)) ) + { block->nominal_length_flag = true; } + else { block->nominal_length_flag = false; } + block->recalculate_flag = true; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] previous_nominal_speed = block->nominal_speed; } else { - // Set at nominal rates only for disabled acceleration planner + // Acceleration planner disabled. Set minimum that is required. block->initial_rate = block->nominal_rate; block->final_rate = block->nominal_rate; block->accelerate_until = 0; @@ -395,12 +458,6 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert block->rate_delta = 0; } - // Compute direction bits for this block - block->direction_bits = 0; - if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1< Date: Tue, 13 Sep 2011 21:57:16 -0600 Subject: [PATCH 68/82] Further planner improvements and misc minor bug fixes. Memory savings and increased buffer size. - Update grbl version and settings version to automatically reset eeprom. FYI, this will reset your grbl settings. - Saved 3*BLOCK_BUFFER_SIZE doubles in static memory by removing obsolete variables: speed_x, speed_y, and speed_z. - Increased buffer size conservatively to 18 from 16. (Probably can do 20). - Removed expensive! modulo operator from block indexing function. Reduces significant computational overhead. - Re-organized some sqrt() calls to be more efficient during time critical planning cases, rather than non-time critical. - Minor bug fix in planner max junction velocity logic. - Simplified arc logic and removed need to multiply for CW or CCW direction. --- gcode.c | 8 +- motion_control.c | 10 +-- motion_control.h | 4 +- nuts_bolts.h | 2 +- planner.c | 162 +++++++++++++++++++------------------- planner.h | 7 +- protocol.c | 2 +- script/grbl_preprocess.py | 2 + settings.c | 4 +- settings.h | 6 +- 10 files changed, 103 insertions(+), 104 deletions(-) diff --git a/gcode.c b/gcode.c index 511bc59..29afe14 100644 --- a/gcode.c +++ b/gcode.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon + Copyright (c) 2011 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 @@ -328,13 +328,13 @@ uint8_t gc_execute_line(char *line) { } // Set clockwise/counter-clockwise sign for mc_arc computations - int8_t clockwise_sign = 1; - if (gc.motion_mode == MOTION_MODE_CW_ARC) { clockwise_sign = -1; } + int8_t isclockwise = false; + 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, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, - r, clockwise_sign); + r, isclockwise); break; #endif diff --git a/motion_control.c b/motion_control.c index bf2ea44..8b55779 100644 --- a/motion_control.c +++ b/motion_control.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon + Copyright (c) 2011 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 @@ -48,7 +48,7 @@ void mc_dwell(uint32_t milliseconds) // The arc is approximated by generating a huge number of tiny, linear segments. The length of each // segment is configured in settings.mm_per_arc_segment. void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, - uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t clockwise_sign) + uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t isclockwise) { // int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); // plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc @@ -64,7 +64,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui // CCW angle between position and target from circle center. Only one atan2() trig computation required. double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); if (angular_travel < 0) { angular_travel += 2*M_PI; } - if (clockwise_sign < 0) { angular_travel = 2*M_PI-angular_travel; } + if (isclockwise) { angular_travel -= 2*M_PI; } double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel)); if (millimeters_of_travel == 0.0) { return; } @@ -104,7 +104,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui */ // Vector rotation matrix values double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation - double sin_T = clockwise_sign*theta_per_segment; + double sin_T = theta_per_segment; double trajectory[3]; double sin_Ti; @@ -128,7 +128,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. // Compute exact location by applying transformation matrix from initial radius vector(=-offset). cos_Ti = cos(i*theta_per_segment); - sin_Ti = clockwise_sign*sin(i*theta_per_segment); + sin_Ti = sin(i*theta_per_segment); r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti; r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti; count = 0; diff --git a/motion_control.h b/motion_control.h index f763e6b..ba01315 100644 --- a/motion_control.h +++ b/motion_control.h @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon + Copyright (c) 2011 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 @@ -41,7 +41,7 @@ // the direction of helical travel, radius == circle radius, clockwise_sign == -1 or 1. Used // for vector transformation direction. void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, - uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t clockwise_sign); + uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t isclockwise); #endif // Dwell for a couple of time units diff --git a/nuts_bolts.h b/nuts_bolts.h index 5e2b04c..37f3aa6 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon + Copyright (c) 2011 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 diff --git a/planner.c b/planner.c index f5139ff..32f7a98 100644 --- a/planner.c +++ b/planner.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon + Copyright (c) 2011 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 @@ -33,7 +33,7 @@ // The number of linear motions that can be in the plan at any give time #ifdef __AVR_ATmega328P__ -#define BLOCK_BUFFER_SIZE 16 +#define BLOCK_BUFFER_SIZE 18 #else #define BLOCK_BUFFER_SIZE 5 #endif @@ -52,15 +52,18 @@ static uint8_t acceleration_manager_enabled; // Acceleration management active // Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. static int8_t next_block_index(int8_t block_index) { - return( (block_index + 1) % BLOCK_BUFFER_SIZE ); + 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 int8_t prev_block_index(int8_t block_index) { - block_index--; - if (block_index < 0) { block_index = BLOCK_BUFFER_SIZE-1; } + if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE-1; } + else { block_index--; } return(block_index); } @@ -90,41 +93,38 @@ static double intersection_distance(double initial_rate, double final_rate, doub } -// Calculates the square of the maximum allowable speed at this point when you must be able to reach -// target_velocity using the acceleration within the allotted distance. -// NOTE: sqrt() removed for speed optimization. Related calculations in terms of square velocity. -static double max_allowable_speed_sqr(double acceleration, double target_velocity_sqr, double distance) { - return( target_velocity_sqr-2*acceleration*60*60*distance ); +// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity +// using the acceleration within the allotted distance. +// NOTE: sqrt() reimplimented here from prior version due to improved planner logic. Increases speed +// in time critical computations, i.e. arcs or rapid short lines from curves. Guaranteed to not exceed +// BLOCK_BUFFER_SIZE calls per planner cycle. +static double max_allowable_speed(double acceleration, double target_velocity, double distance) { + return( sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) ); } // The kernel called by planner_recalculate() when scanning the plan from last to first entry. static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { - if (!current) { return; } + if (!current) { return; } // Cannot operate on nothing. - double entry_speed_sqr = current->max_entry_speed_sqr; // Reset and check to ensure max possible speed + if (next) { + // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. + // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and + // check for maximum allowable speed reductions to ensure maximum possible planned speed. + if (current->entry_speed != current->max_entry_speed) { + + // If nominal length true, max junction speed is guaranteed to be reached. Only compute + // for max allowable speed if block is decelerating and nominal length is false. + if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) { + current->entry_speed = min( current->max_entry_speed, + max_allowable_speed(-settings.acceleration,next->entry_speed,current->millimeters)); + } else { + current->entry_speed = current->max_entry_speed; + } + current->recalculate_flag = true; - // If nominal length true, nominal speed is guaranteed to be reached. No need to re-compute. - // But, if forward planner changed entry speed, reset to max entry speed just to be sure. - if (!current->nominal_length_flag) { - if (next) { - // If the required deceleration across the block is too rapid, reduce entry_speed_sqr accordingly. - if (entry_speed_sqr > next->entry_speed_sqr) { - entry_speed_sqr = min( entry_speed_sqr, - max_allowable_speed_sqr(-settings.acceleration,next->entry_speed_sqr,current->millimeters)); - } - } else { - // Assume last block has zero exit velocity. - entry_speed_sqr = min( entry_speed_sqr, - max_allowable_speed_sqr(-settings.acceleration,0.0,current->millimeters)); } - } - - // Check for junction speed change - if (current->entry_speed_sqr != entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - current->recalculate_flag = true; // Note: Newest block already set to true - } + } // Skip last block. Already initialized and set for recalculation. } @@ -140,34 +140,29 @@ static void planner_reverse_pass() { block[0] = &block_buffer[block_index]; planner_reverse_pass_kernel(block[0], block[1], block[2]); } - // Skip buffer tail to prevent over-writing the initial entry speed. + // Skip buffer tail/first block to prevent over-writing the initial entry speed. } // The kernel called by planner_recalculate() when scanning the plan from first to last entry. static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) { - if(!current) { return; } + if(!previous) { return; } // Begin planning after buffer_tail + + // If the previous block is an acceleration block, but it is not long enough to complete the + // full speed change within the block, we need to adjust the entry speed accordingly. Entry + // speeds have already been reset, maximized, and reverse planned by reverse planner. + // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. + if (!previous->nominal_length_flag) { + if (previous->entry_speed < current->entry_speed) { + double entry_speed = min( current->entry_speed, + max_allowable_speed(-settings.acceleration,previous->entry_speed,previous->millimeters) ); - if(previous) { - - // If nominal length true, nominal speed is guaranteed to be reached. No need to recalculate. - if (!previous->nominal_length_flag) { - // If the previous block is an acceleration block, but it is not long enough to - // complete the full speed change within the block, we need to adjust the entry - // speed accordingly. - if (previous->entry_speed_sqr < current->entry_speed_sqr) { - double entry_speed_sqr = min( current->entry_speed_sqr, current->max_entry_speed_sqr ); - entry_speed_sqr = min( entry_speed_sqr, - max_allowable_speed_sqr(-settings.acceleration,previous->entry_speed_sqr,previous->millimeters) ); - - // Check for junction speed change - if (current->entry_speed_sqr != entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - current->recalculate_flag = true; - } + // Check for junction speed change + if (current->entry_speed != entry_speed) { + current->entry_speed = entry_speed; + current->recalculate_flag = true; } - - } + } } } @@ -243,25 +238,23 @@ static void planner_recalculate_trapezoids() { int8_t block_index = block_buffer_tail; block_t *current; block_t *next = NULL; - + while(block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; if (current) { // Recalculate if current block entry or exit junction speed has changed. if (current->recalculate_flag || next->recalculate_flag) { - // Compute entry and exit factors for trapezoid calculations. - // NOTE: sqrt(square velocities) now performed only when required in trapezoid calculation. - double entry_factor = sqrt( current->entry_speed_sqr ) / current->nominal_speed; - double exit_factor = sqrt( next->entry_speed_sqr ) / current->nominal_speed; - calculate_trapezoid_for_block(current, entry_factor, exit_factor); + // NOTE: Entry and exit factors always > 0 by all previous logic operations. + calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed, + next->entry_speed/current->nominal_speed); current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed } } block_index = next_block_index( block_index ); } - // Last/newest block in buffer. Exit speed is zero. - calculate_trapezoid_for_block(next, sqrt( next->entry_speed_sqr ) / next->nominal_speed, 0.0); + // Last/newest block in buffer. Exit speed is zero. Always recalculated. + calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, 0.0); next->recalculate_flag = false; } @@ -373,9 +366,6 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in // Calculate speed in mm/minute for each axis double multiplier = 60.0*1000000.0/microseconds; - block->speed_x = delta_mm[X_AXIS] * multiplier; - block->speed_y = delta_mm[Y_AXIS] * multiplier; - block->speed_z = delta_mm[Z_AXIS] * multiplier; block->nominal_speed = block->millimeters * multiplier; block->nominal_rate = ceil(block->step_event_count * multiplier); @@ -404,16 +394,15 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inv_millimeters; // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. - // Does not actually deviate from path, but used as a robust way to compute cornering speeds. // 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. - // NOTE: sqrt() removed for speed optimization. Related calculations in terms of square velocity. - - double vmax_junction_sqr = 0.0; // Set default zero max junction speed + // 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. + double vmax_junction = 0.0; // Set default zero max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { @@ -423,25 +412,33 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; - // Skip and use default zero max junction speed for 0 degree acute junction. - if (cos_theta < 1.0) { - vmax_junction_sqr = square( min(previous_nominal_speed,block->nominal_speed) ); + // Skip and use default max junction speed for 0 degree acute junction. + if (cos_theta < 0.95) { + vmax_junction = min(previous_nominal_speed,block->nominal_speed); // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. - if (cos_theta > -1.0) { + if (cos_theta > -0.95) { // Compute maximum junction velocity based on maximum acceleration and junction deviation double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. - vmax_junction_sqr = min(vmax_junction_sqr, - settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2) ); + vmax_junction = min(vmax_junction, + sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } } - block->max_entry_speed_sqr = vmax_junction_sqr; - block->entry_speed_sqr = vmax_junction_sqr; + block->max_entry_speed = vmax_junction; + + // Initialize block entry speed. Compute based on deceleration to rest (zero speed). + double v_allowable = max_allowable_speed(-settings.acceleration,0.0,block->millimeters); + block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags - // Set flag if block will always reach nominal speed regardless of entry/exit speeds. - if (block->nominal_speed <= sqrt(max_allowable_speed_sqr(-settings.acceleration,0.0,0.5*block->millimeters)) ) - { block->nominal_length_flag = true; } + // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. + // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then + // the current block and next block junction speeds are guaranteed to always be at their maximum + // junction speeds in deceleration and acceleration, respectively. This is due to how the current + // block nominal speed limits both the current and next maximum junction speeds. Hence, in both + // the reverse and forward planners, the corresponding block junction speed will always be at the + // the maximum junction speed and may always be ignored for any speed reduction checks. + if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; } else { block->nominal_length_flag = false; } block->recalculate_flag = true; // Always calculate trapezoid for new block @@ -471,6 +468,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in void plan_set_current_position(double x, double y, double z) { position[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); position[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); - position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); - previous_nominal_speed = 0.0; + position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); + previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. + clear_vector_double(previous_unit_vec); } diff --git a/planner.h b/planner.h index 541efad..f995210 100644 --- a/planner.h +++ b/planner.h @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon + Copyright (c) 2011 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 @@ -34,10 +34,9 @@ typedef struct { uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute // Fields used by the motion planner to manage acceleration - double speed_x, speed_y, speed_z; // Nominal mm/minute for each axis double nominal_speed; // The nominal speed for this block in mm/min - double entry_speed_sqr; // Square of entry speed at previous-current junction in (mm/min)^2 - double max_entry_speed_sqr; // Square of maximum allowable entry speed in (mm/min)^2 + double entry_speed; // Entry speed at previous-current junction in mm/min + double max_entry_speed; // Maximum allowable junction entry speed in mm/min double millimeters; // The total travel of this block in mm uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction uint8_t nominal_length_flag; // Planner flag for nominal speed always reached diff --git a/protocol.c b/protocol.c index e6c4ea4..4835c90 100644 --- a/protocol.c +++ b/protocol.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon + Copyright (c) 2011 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 diff --git a/script/grbl_preprocess.py b/script/grbl_preprocess.py index ab6a853..bbb08c8 100755 --- a/script/grbl_preprocess.py +++ b/script/grbl_preprocess.py @@ -8,6 +8,8 @@ G-code preprocessor for grbl (BETA!) - OPTIONAL: Remove unsupported grbl G and M commands TODO: +- Number precision truncation +- Arc conversion option - More robust error checking - Improve interface to command line options - Improve g-code parsing to NIST standards diff --git a/settings.c b/settings.c index 5c9613d..b60f35e 100644 --- a/settings.c +++ b/settings.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon + Copyright (c) 2011 Sungeun 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 { #define DEFAULT_RAPID_FEEDRATE 500.0 // in millimeters per minute #define DEFAULT_FEEDRATE 500.0 #define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/10.0) -#define DEFAULT_JUNCTION_DEVIATION 0.1 +#define DEFAULT_JUNCTION_DEVIATION 0.05 #define DEFAULT_STEPPING_INVERT_MASK ((1< #include -#define GRBL_VERSION "0.7b" +#define GRBL_VERSION "0.7c" // 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 2 +#define SETTINGS_VERSION 3 // Current global settings (persisted in EEPROM from byte 1 onwards) typedef struct { From 110faae986eef6c4398c166a81c91cbd1d5a3bf4 Mon Sep 17 00:00:00 2001 From: Sonny J Date: Thu, 15 Sep 2011 20:32:15 -0600 Subject: [PATCH 69/82] More '%' modulo opertor removals and some housecleaning. - Serial functions contained quite a few modulo operations that would be executed with high frequency when streaming. AVR processors are very slow when operating these. In one test on the Arduino forums, it showed about a 15x slow down compared to a simple if-then statement. - Clarified some variable names and types and comments. --- gcode.c | 2 +- motion_control.c | 16 ++++++++-------- motion_control.h | 4 ++-- serial.c | 11 +++++++---- settings.c | 2 +- 5 files changed, 19 insertions(+), 16 deletions(-) diff --git a/gcode.c b/gcode.c index 29afe14..7127932 100644 --- a/gcode.c +++ b/gcode.c @@ -328,7 +328,7 @@ uint8_t gc_execute_line(char *line) { } // Set clockwise/counter-clockwise sign for mc_arc computations - int8_t isclockwise = false; + uint8_t isclockwise = false; if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; } // Trace the arc diff --git a/motion_control.c b/motion_control.c index 8b55779..aec1c1c 100644 --- a/motion_control.c +++ b/motion_control.c @@ -48,7 +48,7 @@ void mc_dwell(uint32_t milliseconds) // The arc is approximated by generating a huge number of tiny, linear segments. The length of each // segment is configured in settings.mm_per_arc_segment. void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, - uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t isclockwise) + uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise) { // int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); // plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc @@ -106,7 +106,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation double sin_T = theta_per_segment; - double trajectory[3]; + double arc_target[3]; double sin_Ti; double cos_Ti; double r_axisi; @@ -114,7 +114,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui int8_t count = 0; // Initialize the linear axis - trajectory[axis_linear] = position[axis_linear]; + arc_target[axis_linear] = position[axis_linear]; for (i = 1; i Date: Sun, 18 Sep 2011 05:36:55 -0600 Subject: [PATCH 70/82] Fixed minor bugs in planner. Increased max dwell time. Long slope bug stop-gap solution note. - Fixed the planner TODO regarding minimum nominal speeds. Re-arranged calculations to be both more efficient and guaranteed to be greater than zero. - Missed a parenthesis location on the rate_delta calculation. Should fix a nearly in-perceptible issue with incorrect acceleration ramping in diagonal directions. - Increased maximum dwell time from 6.5sec to an 18hour max. A crazy amount more, but that's how the math works out. - Converted the internal feedrate values to mm/min only, as it was switching between mm/min to mm/sec and back to mm/min. Also added a feedrate > 0 check in gcode.c. - Identified the long slope at the end of rapid de/ac-celerations noted by stephanix. Problem with the numerical integration truncation error between the exact solution of estimate_acceleration_distance and how grbl actually performs the acceleration ramps discretely. Increasing the ACCELERATION_TICKS_PER_SECOND in config.h helps fix this problem. Investigating further. --- config.h | 5 +++++ gcode.c | 11 ++++++----- motion_control.c | 16 ++++++++++++---- motion_control.h | 4 ++-- planner.c | 48 +++++++++++++++++++++--------------------------- 5 files changed, 46 insertions(+), 38 deletions(-) diff --git a/config.h b/config.h index e2811cd..952f7a2 100644 --- a/config.h +++ b/config.h @@ -55,6 +55,11 @@ // The temporal resolution of the acceleration management subsystem. Higher number // give smoother acceleration but may impact performance +// NOTE: Increasing this parameter will help remove the long slow motion bug at the end +// of very fast de/ac-celerations. This is due to the increased resolution of the +// acceleration steps that more accurately predicted by the planner exact integration +// of acceleration distance. An efficient solution to this bug is under investigation. +// In general, setting this parameter is high as your system will allow is suggested. #define ACCELERATION_TICKS_PER_SECOND 40L #endif diff --git a/gcode.c b/gcode.c index 7127932..1027a7f 100644 --- a/gcode.c +++ b/gcode.c @@ -88,8 +88,8 @@ static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) void gc_init() { memset(&gc, 0, sizeof(gc)); - gc.feed_rate = settings.default_feed_rate/60; - gc.seek_rate = settings.default_seek_rate/60; + gc.feed_rate = settings.default_feed_rate; + gc.seek_rate = settings.default_seek_rate; select_plane(X_AXIS, Y_AXIS, Z_AXIS); gc.absolute_mode = true; } @@ -180,13 +180,14 @@ uint8_t gc_execute_line(char *line) { unit_converted_value = to_millimeters(value); switch(letter) { case 'F': + if (unit_converted_value <= 0) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // Must be greater than zero if (gc.inverse_feed_rate_mode) { inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only } else { if (gc.motion_mode == MOTION_MODE_SEEK) { - gc.seek_rate = unit_converted_value/60; + gc.seek_rate = unit_converted_value; } else { - gc.feed_rate = unit_converted_value/60; // millimeters pr second + gc.feed_rate = unit_converted_value; // millimeters per minute } } break; @@ -213,7 +214,7 @@ uint8_t gc_execute_line(char *line) { // Perform any physical actions switch (next_action) { case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break; - case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break; + case NEXT_ACTION_DWELL: mc_dwell(p); break; case NEXT_ACTION_SET_COORDINATE_OFFSET: mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]); break; diff --git a/motion_control.c b/motion_control.c index aec1c1c..98931f8 100644 --- a/motion_control.c +++ b/motion_control.c @@ -29,13 +29,21 @@ #include "stepper.h" #include "planner.h" -#define N_ARC_CORRECTION 25 // (0-255) Number of iterations before arc trajectory correction +// Number of arc generation iterations with small angle approximation before exact arc +// trajectory correction. Value must be 1-255. +#define N_ARC_CORRECTION 25 -void mc_dwell(uint32_t milliseconds) +// Execute dwell in seconds. Maximum time delay is > 18 hours, more than enough for any application. +void mc_dwell(double seconds) { - st_synchronize(); - _delay_ms(milliseconds); + uint16_t i = floor(seconds); + st_synchronize(); + _delay_ms(floor(1000*(seconds-i))); // Delay millisecond remainder + while (i > 0) { + _delay_ms(1000); // Delay one second + i--; + } } // Execute an arc in offset mode format. position == current xyz, target == target xyz, diff --git a/motion_control.h b/motion_control.h index cfff84c..b91650d 100644 --- a/motion_control.h +++ b/motion_control.h @@ -44,8 +44,8 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise); #endif -// Dwell for a couple of time units -void mc_dwell(uint32_t milliseconds); +// Dwell for a specific number of seconds +void mc_dwell(double seconds); // Send the tool home (not implemented) void mc_go_home(); diff --git a/planner.c b/planner.c index 32f7a98..4be9c88 100644 --- a/planner.c +++ b/planner.c @@ -48,8 +48,6 @@ static double previous_nominal_speed; // Nominal speed of previous path line s static uint8_t acceleration_manager_enabled; // Acceleration management active? -#define ONE_MINUTE_OF_MICROSECONDS 60000000.0 - // Returns the index of the next block in the ring buffer // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. @@ -62,8 +60,8 @@ static int8_t next_block_index(int8_t block_index) { // Returns the index of the previous block in the ring buffer static int8_t prev_block_index(int8_t block_index) { - if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE-1; } - else { block_index--; } + if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } + block_index--; return(block_index); } @@ -197,9 +195,9 @@ static void planner_forward_pass() { // This converts the planner parameters to the data required by the stepper controller. static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { - block->initial_rate = ceil(block->nominal_rate*entry_factor); - block->final_rate = ceil(block->nominal_rate*exit_factor); - int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; + block->initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min) + block->final_rate = ceil(block->nominal_rate*exit_factor); // (step/min) + int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; // (step/min^2) int32_t accelerate_steps = ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute)); int32_t decelerate_steps = @@ -356,42 +354,38 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])); - - uint32_t microseconds; + double inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides + + // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks. + // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c + double inverse_minute; if (!invert_feed_rate) { - microseconds = lround((block->millimeters/feed_rate)*1000000); + inverse_minute = feed_rate * inverse_millimeters; } else { - microseconds = lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate); + inverse_minute = 1.0 / feed_rate; } + block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0 + block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0 - // Calculate speed in mm/minute for each axis - double multiplier = 60.0*1000000.0/microseconds; - block->nominal_speed = block->millimeters * multiplier; - block->nominal_rate = ceil(block->step_event_count * multiplier); - - // This is a temporary fix to avoid a situation where very low nominal_speeds would be rounded - // down to zero and cause a division by zero. TODO: Grbl deserves a less patchy fix for this problem - if (block->nominal_speed < 60.0) { block->nominal_speed = 60.0; } - // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line // average travel per step event changes. For a line along one axis the travel per step event // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2). // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed // specifically for each line to compensate for this phenomenon: - double step_per_travel = block->step_event_count/block->millimeters; // Compute inverse to remove divide - block->rate_delta = step_per_travel * ceil( // convert to: acceleration steps/min/acceleration_tick - settings.acceleration*60.0 / ACCELERATION_TICKS_PER_SECOND ); // acceleration mm/sec/sec per acceleration_tick + // Convert universal acceleration for direction-dependent stepper rate change parameter + block->rate_delta = ceil( block->step_event_count*inverse_millimeters * + settings.acceleration*60.0 / ACCELERATION_TICKS_PER_SECOND ); // (step/min/acceleration_tick) // Perform planner-enabled calculations if (acceleration_manager_enabled) { // Compute path unit vector double unit_vec[3]; - double inv_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides - unit_vec[X_AXIS] = delta_mm[X_AXIS]*inv_millimeters; - unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inv_millimeters; - unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inv_millimeters; + + unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; + unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; + unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; // 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 From 2be0d668722ed42af66a4bdab71cda672189ff73 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sat, 24 Sep 2011 07:46:41 -0600 Subject: [PATCH 71/82] Fixed long slope at deceleration issue. Moved things into config.h. New MINIMUM_PLANNER_SPEED parameter. - The long standing issue of a long slope at deceleration is likely fixed. The stepper program was not tracking and timing the end of acceleration and start of deceleration exactly and now is fixed to start and stop on time. Also, to ensure a better acceleration curve fit used by the planner, the stepper program delays the start of the accelerations by a half trapezoid tick to employ the midpoint rule. - Settings version 3 migration (not fully tested, but should work) - Added a MINIMUM_PLANNER_SPEED user-defined parameter to planner to let a user change this if problems arise for some reason. - Moved all user-definable #define parameters into config.h with clear comments on what they do and recommendations of how to change them. - Minor housekeeping. --- config.h | 38 +++++++++++--- gcode.c | 3 +- motion_control.c | 8 +-- planner.c | 19 ++++--- settings.c | 10 +++- stepper.c | 125 ++++++++++++++++++++++++++++++----------------- 6 files changed, 133 insertions(+), 70 deletions(-) diff --git a/config.h b/config.h index 952f7a2..89d9aea 100644 --- a/config.h +++ b/config.h @@ -3,6 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 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,6 +22,8 @@ #ifndef config_h #define config_h +// IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them. + #define BAUD_RATE 9600 // Updated default pin-assignments from 0.6 onwards @@ -55,12 +58,32 @@ // The temporal resolution of the acceleration management subsystem. Higher number // give smoother acceleration but may impact performance -// NOTE: Increasing this parameter will help remove the long slow motion bug at the end -// of very fast de/ac-celerations. This is due to the increased resolution of the -// acceleration steps that more accurately predicted by the planner exact integration -// of acceleration distance. An efficient solution to this bug is under investigation. -// In general, setting this parameter is high as your system will allow is suggested. -#define ACCELERATION_TICKS_PER_SECOND 40L +// NOTE: Increasing this parameter will help any resolution related issues, especially with machines +// requiring very high accelerations and/or very fast feedrates. In general, this will reduce the +// error between how the planner plans the motions and how the stepper program actually performs them. +// However, at some point, the resolution can be high enough, where the errors related to numerical +// round-off can be great enough to cause problems and/or it's too fast for the Arduino. The correct +// value for this parameter is machine dependent, so it's advised to set this only as high as needed. +// Approximate successful values can range from 30L to 100L or more. +#define ACCELERATION_TICKS_PER_SECOND 50L + +// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end +// of the buffer and all stops. This should not be much greater than zero and should only be changed +// if unwanted behavior is observed on a user's machine when running at very slow speeds. +#define MINIMUM_PLANNER_SPEED 0.0 // (mm/min) + +// Minimum stepper rate. Sets the absolute minimum stepper rate in the stepper program and never run +// slower than this value, except when sleeping. This parameter overrides the minimum planner speed. +// This is primarily used to guarantee that the end of a movement is always reached and not stop to +// never reach its target. This parameter should always be greater than zero. +#define MINIMUM_STEPS_PER_MINUTE 800 // (steps/min) - Integer value only + +// Number of arc generation iterations by small angle approximation before exact arc +// trajectory correction. Value must be 1-255. This parameter maybe decreased if there are issues +// with the accuracy of the arc generations. In general, the default value is more than enough for +// the intended CNC applications of grbl, and should be on the order or greater than the size of +// the buffer to help with the computational efficiency of generating arcs. +#define N_ARC_CORRECTION 25 #endif @@ -91,5 +114,4 @@ // // #define SPINDLE_DIRECTION_DDR DDRD // #define SPINDLE_DIRECTION_PORT PORTD -// #define SPINDLE_DIRECTION_BIT 7 - +// #define SPINDLE_DIRECTION_BIT 7 \ No newline at end of file diff --git a/gcode.c b/gcode.c index 1027a7f..1930389 100644 --- a/gcode.c +++ b/gcode.c @@ -388,5 +388,4 @@ static int next_statement(char *letter, double *double_ptr, char *line, uint8_t group 9 = {M48, M49} enable/disable feed and speed override switches group 12 = {G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3} coordinate system selection group 13 = {G61, G61.1, G64} path control mode -*/ - +*/ \ No newline at end of file diff --git a/motion_control.c b/motion_control.c index 98931f8..52a72c2 100644 --- a/motion_control.c +++ b/motion_control.c @@ -21,6 +21,7 @@ #include #include "settings.h" +#include "config.h" #include "motion_control.h" #include #include @@ -29,11 +30,6 @@ #include "stepper.h" #include "planner.h" -// Number of arc generation iterations with small angle approximation before exact arc -// trajectory correction. Value must be 1-255. -#define N_ARC_CORRECTION 25 - - // Execute dwell in seconds. Maximum time delay is > 18 hours, more than enough for any application. void mc_dwell(double seconds) { @@ -48,7 +44,7 @@ void mc_dwell(double seconds) // Execute an arc in offset mode format. position == current xyz, target == target xyz, // offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is -// the direction of helical travel, radius == circle radius, clockwise_sign == -1 or 1. Used +// the direction of helical travel, radius == circle radius, isclockwise boolean. Used // for vector transformation direction. // position, target, and offset are pointers to vectors from gcode.c diff --git a/planner.c b/planner.c index 4be9c88..b3abd6c 100644 --- a/planner.c +++ b/planner.c @@ -193,6 +193,7 @@ static void planner_forward_pass() { // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. // The factors represent a factor of braking and must be in the range 0.0-1.0. // This converts the planner parameters to the data required by the stepper controller. +// NOTE: Final rates must be computed in terms of their respective blocks. static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { block->initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min) @@ -212,6 +213,8 @@ static void calculate_trapezoid_for_block(block_t *block, double entry_factor, d if (plateau_steps < 0) { accelerate_steps = ceil( intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count)); + accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off + accelerate_steps = min(accelerate_steps,block->step_event_count); plateau_steps = 0; } @@ -251,8 +254,9 @@ static void planner_recalculate_trapezoids() { } block_index = next_block_index( block_index ); } - // Last/newest block in buffer. Exit speed is zero. Always recalculated. - calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, 0.0); + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, + MINIMUM_PLANNER_SPEED/next->nominal_speed); next->recalculate_flag = false; } @@ -273,6 +277,9 @@ static void planner_recalculate_trapezoids() { // // 3. Recalculate trapezoids for all blocks using the recently updated junction speeds. Block trapezoids // with no updated junction speeds will not be recalculated and assumed ok as is. +// +// All planner computations are performed with doubles (float on Arduinos) to minimize numerical round- +// off errors. Only when planned values are converted to stepper rate parameters, these are integers. static void planner_recalculate() { planner_reverse_pass(); @@ -396,7 +403,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in // 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. - double vmax_junction = 0.0; // Set default zero max junction speed + double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { @@ -420,8 +427,8 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in } block->max_entry_speed = vmax_junction; - // Initialize block entry speed. Compute based on deceleration to rest (zero speed). - double v_allowable = max_allowable_speed(-settings.acceleration,0.0,block->millimeters); + // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. + double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags @@ -465,4 +472,4 @@ void plan_set_current_position(double x, double y, double z) { position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. clear_vector_double(previous_unit_vec); -} +} \ No newline at end of file diff --git a/settings.c b/settings.c index bcf14cf..f85e3c3 100644 --- a/settings.c +++ b/settings.c @@ -80,7 +80,7 @@ void settings_dump() { printPgmString(PSTR(" (step port invert mask. binary = ")); printIntegerInBase(settings.invert_mask, 2); printPgmString(PSTR(")\r\n$8 = ")); printFloat(settings.acceleration); printPgmString(PSTR(" (acceleration in mm/sec^2)\r\n$9 = ")); printFloat(settings.junction_deviation); - printPgmString(PSTR(" (junction deviation for cornering in mm)")); + printPgmString(PSTR(" (cornering junction deviation in mm)")); printPgmString(PSTR("\r\n'$x=value' to set parameter or just '$' to dump current settings\r\n")); } @@ -125,12 +125,18 @@ int read_settings() { return(false); } } else if (version == 1) { - // Migrate from old settings version + // Migrate from settings version 1 if (!(memcpy_from_eeprom_with_checksum((char*)&settings, 1, sizeof(settings_v1_t)))) { return(false); } settings.acceleration = DEFAULT_ACCELERATION; settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION; + } else if (version == 2) { + // Migrate from settings version 2 + if (!(memcpy_from_eeprom_with_checksum((char*)&settings, 1, sizeof(settings_t)))) { + return(false); + } + settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION; } else { return(false); } diff --git a/stepper.c b/stepper.c index 164db81..ea6c17f 100644 --- a/stepper.c +++ b/stepper.c @@ -3,7 +3,8 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - + Modifications Copyright (c) 2011 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 @@ -40,8 +41,6 @@ #define TICKS_PER_MICROSECOND (F_CPU/1000000) #define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND) -#define MINIMUM_STEPS_PER_MINUTE 1200 // The stepper subsystem will never run slower than this, exept when sleeping - static block_t *current_block; // A pointer to the block currently being traced // Variables used by The Stepper Driver Interrupt @@ -95,41 +94,27 @@ static void st_go_idle() { // block begins. static void trapezoid_generator_reset() { trapezoid_adjusted_rate = current_block->initial_rate; - trapezoid_tick_cycle_counter = 0; // Always start a new trapezoid with a full acceleration tick - set_step_events_per_minute(trapezoid_adjusted_rate); + trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule. + set_step_events_per_minute(trapezoid_adjusted_rate); // Initialize cycles_per_step_event } -// This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event -// interrupt. It can be assumed that the trapezoid-generator-parameters and the -// current_block stays untouched by outside handlers for the duration of this function call. -static void trapezoid_generator_tick() { - if (current_block) { - if (step_events_completed < current_block->accelerate_until) { - trapezoid_adjusted_rate += current_block->rate_delta; - set_step_events_per_minute(trapezoid_adjusted_rate); - } else if (step_events_completed > current_block->decelerate_after) { - // NOTE: We will only reduce speed if the result will be > 0. This catches small - // rounding errors that might leave steps hanging after the last trapezoid tick. - if (trapezoid_adjusted_rate > current_block->rate_delta) { - trapezoid_adjusted_rate -= current_block->rate_delta; - } - if (trapezoid_adjusted_rate < current_block->final_rate) { - trapezoid_adjusted_rate = current_block->final_rate; - } - set_step_events_per_minute(trapezoid_adjusted_rate); - } else { - // Make sure we cruise at exactly nominal rate - if (trapezoid_adjusted_rate != current_block->nominal_rate) { - trapezoid_adjusted_rate = current_block->nominal_rate; - set_step_events_per_minute(trapezoid_adjusted_rate); - } - } +// This function determines an acceleration velocity change every CYCLES_PER_ACCELERATION_TICK by +// keeping track of the number of elapsed cycles during a de/ac-celeration. The code assumes that +// step_events occur significantly more often than the acceleration velocity iterations. +static uint8_t iterate_trapezoid_cycle_counter() { + trapezoid_tick_cycle_counter += cycles_per_step_event; + if(trapezoid_tick_cycle_counter > CYCLES_PER_ACCELERATION_TICK) { + trapezoid_tick_cycle_counter -= CYCLES_PER_ACCELERATION_TICK; + return(true); + } else { + return(false); } -} +} // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. -// It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse. +// 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. SIGNAL(TIMER1_COMPA_vect) { // TODO: Check if the busy-flag can be eliminated by just disabeling this interrupt while we are in it @@ -157,13 +142,14 @@ SIGNAL(TIMER1_COMPA_vect) counter_x = -(current_block->step_event_count >> 1); counter_y = counter_x; counter_z = counter_x; - step_events_completed = 0; + step_events_completed = 0; } else { st_go_idle(); } } if (current_block != NULL) { + // Execute step displacement profile by bresenham line algorithm out_bits = current_block->direction_bits; counter_x += current_block->steps_x; if (counter_x > 0) { @@ -180,26 +166,73 @@ SIGNAL(TIMER1_COMPA_vect) out_bits |= (1<step_event_count; } - // If current block is finished, reset pointer - step_events_completed += 1; - if (step_events_completed >= current_block->step_event_count) { + + step_events_completed += 1; // Iterate step events + + // While in block steps, check for de/ac-celeration events and execute them accordingly. + if (step_events_completed < current_block->step_event_count) { + + // Always check step event location to ensure de/ac-celerations are executed and terminated at + // exactly the right time. This helps prevent over/under-shooting the target position and speed. + // Trapezoid de/ac-celeration is approximated by discrete increases or decreases in velocity, + // defined by ACCELERATION_TICKS_PER_SECOND and block->rate_delta. The accelerations employ the + // midpoint rule to obtain an accurate representation of the exact acceleration curve. + + // NOTE: By increasing the ACCELERATION_TICKS_PER_SECOND in config.h, the resolution of the + // discrete velocity changes increase and accuracy can increase as well to a point. Numerical + // round-off errors can effect this, if set too high. This is important to note if a user has + // very high acceleration and/or feedrate requirements for their machine. + + if (step_events_completed < current_block->accelerate_until) { + // Iterate cycle counter and check if speeds need to be increased. + if ( iterate_trapezoid_cycle_counter() ) { + trapezoid_adjusted_rate += current_block->rate_delta; + if (trapezoid_adjusted_rate >= current_block->nominal_rate) { + // Reached nominal rate a little early. Cruise at nominal rate until decelerate_after. + trapezoid_adjusted_rate = current_block->nominal_rate; + } + set_step_events_per_minute(trapezoid_adjusted_rate); + } + } else if (step_events_completed > current_block->decelerate_after) { + // Iterate cycle counter and check if speeds need to be reduced. + if ( iterate_trapezoid_cycle_counter() ) { + // NOTE: We will only reduce speed if the result will be > 0. This catches small + // rounding errors that might leave steps hanging after the last trapezoid tick. + if (trapezoid_adjusted_rate > current_block->rate_delta) { + trapezoid_adjusted_rate -= current_block->rate_delta; + } + if (trapezoid_adjusted_rate < current_block->final_rate) { + // Reached final rate a little early. Cruise to end of block at final rate. + trapezoid_adjusted_rate = current_block->final_rate; + } + set_step_events_per_minute(trapezoid_adjusted_rate); + } + } else { + // No accelerations. Make sure we cruise exactly at nominal rate. + if (trapezoid_adjusted_rate != current_block->nominal_rate) { + trapezoid_adjusted_rate = current_block->nominal_rate; + set_step_events_per_minute(trapezoid_adjusted_rate); + } + // Check to reset trapezoid tick cycle counter to make sure that the deceleration is + // performed the same every time. Reset to CYCLES_PER_ACCELERATION_TICK/2 to follow the + // midpoint rule for an accurate approximation of the deceleration curve. + if (step_events_completed >= current_block-> decelerate_after) { + trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; + } + } + + } else { + // If current block is finished, reset pointer current_block = NULL; plan_discard_current_block(); } + } else { + // Still no block? Set the stepper pins to low before sleeping. out_bits = 0; } - out_bits ^= settings.invert_mask; - - // In average this generates a trapezoid_generator_tick every CYCLES_PER_ACCELERATION_TICK by keeping track - // of the number of elapsed cycles. The code assumes that step_events occur significantly more often than - // trapezoid_generator_ticks as they well should. - trapezoid_tick_cycle_counter += cycles_per_step_event; - if(trapezoid_tick_cycle_counter > CYCLES_PER_ACCELERATION_TICK) { - trapezoid_tick_cycle_counter -= CYCLES_PER_ACCELERATION_TICK; - trapezoid_generator_tick(); - } + out_bits ^= settings.invert_mask; // Apply stepper invert mask busy=false; } From 05ed6c122d16889a82ebaca6835a21676a6ed0e3 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sun, 25 Sep 2011 19:24:29 -0600 Subject: [PATCH 72/82] Updated some comments and fixed a bug in the new stepper logic. - The stepper logic was not initiating the decelerations for certain cases. Just re-arranged the logic to fix it. --- stepper.c | 53 ++++++++++++++++++++++++++--------------------------- 1 file changed, 26 insertions(+), 27 deletions(-) diff --git a/stepper.c b/stepper.c index ea6c17f..9786387 100644 --- a/stepper.c +++ b/stepper.c @@ -71,8 +71,8 @@ static uint32_t trapezoid_adjusted_rate; // The current rate of step_events // The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta // during the first block->accelerate_until step_events_completed, then keeps going at constant speed until // step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset. -// The slope of acceleration is always +/- block->rate_delta and is applied at a constant rate by trapezoid_generator_tick() -// that is called ACCELERATION_TICKS_PER_SECOND times per second. +// The slope of acceleration is always +/- block->rate_delta and is applied at a constant rate following the midpoint rule +// by the trapezoid generator, which is called ACCELERATION_TICKS_PER_SECOND times per second. static void set_step_events_per_minute(uint32_t steps_per_minute); @@ -171,12 +171,10 @@ SIGNAL(TIMER1_COMPA_vect) // While in block steps, check for de/ac-celeration events and execute them accordingly. if (step_events_completed < current_block->step_event_count) { - - // Always check step event location to ensure de/ac-celerations are executed and terminated at - // exactly the right time. This helps prevent over/under-shooting the target position and speed. - // Trapezoid de/ac-celeration is approximated by discrete increases or decreases in velocity, - // defined by ACCELERATION_TICKS_PER_SECOND and block->rate_delta. The accelerations employ the - // midpoint rule to obtain an accurate representation of the exact acceleration curve. + + // The trapezoid generator always checks step event location to ensure de/ac-celerations are + // executed and terminated at exactly the right time. This helps prevent over/under-shooting + // the target position and speed. // NOTE: By increasing the ACCELERATION_TICKS_PER_SECOND in config.h, the resolution of the // discrete velocity changes increase and accuracy can increase as well to a point. Numerical @@ -193,32 +191,33 @@ SIGNAL(TIMER1_COMPA_vect) } set_step_events_per_minute(trapezoid_adjusted_rate); } - } else if (step_events_completed > current_block->decelerate_after) { - // Iterate cycle counter and check if speeds need to be reduced. - if ( iterate_trapezoid_cycle_counter() ) { - // NOTE: We will only reduce speed if the result will be > 0. This catches small - // rounding errors that might leave steps hanging after the last trapezoid tick. - if (trapezoid_adjusted_rate > current_block->rate_delta) { - trapezoid_adjusted_rate -= current_block->rate_delta; + } else if (step_events_completed >= current_block->decelerate_after) { + // Reset trapezoid tick cycle counter to make sure that the deceleration is performed the + // same every time. Reset to CYCLES_PER_ACCELERATION_TICK/2 to follow the midpoint rule for + // an accurate approximation of the deceleration curve. + if (step_events_completed == current_block-> decelerate_after) { + trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; + } else { + // Iterate cycle counter and check if speeds need to be reduced. + if ( iterate_trapezoid_cycle_counter() ) { + // NOTE: We will only reduce speed if the result will be > 0. This catches small + // rounding errors that might leave steps hanging after the last trapezoid tick. + if (trapezoid_adjusted_rate > current_block->rate_delta) { + trapezoid_adjusted_rate -= current_block->rate_delta; + } + if (trapezoid_adjusted_rate < current_block->final_rate) { + // Reached final rate a little early. Cruise to end of block at final rate. + trapezoid_adjusted_rate = current_block->final_rate; + } + set_step_events_per_minute(trapezoid_adjusted_rate); } - if (trapezoid_adjusted_rate < current_block->final_rate) { - // Reached final rate a little early. Cruise to end of block at final rate. - trapezoid_adjusted_rate = current_block->final_rate; - } - set_step_events_per_minute(trapezoid_adjusted_rate); } } else { - // No accelerations. Make sure we cruise exactly at nominal rate. + // No accelerations. Make sure we cruise exactly at the nominal rate. if (trapezoid_adjusted_rate != current_block->nominal_rate) { trapezoid_adjusted_rate = current_block->nominal_rate; set_step_events_per_minute(trapezoid_adjusted_rate); } - // Check to reset trapezoid tick cycle counter to make sure that the deceleration is - // performed the same every time. Reset to CYCLES_PER_ACCELERATION_TICK/2 to follow the - // midpoint rule for an accurate approximation of the deceleration curve. - if (step_events_completed >= current_block-> decelerate_after) { - trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; - } } } else { From 59a84c4925c441e994583da46d9e9782331bf61d Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Thu, 29 Sep 2011 16:25:48 -0600 Subject: [PATCH 73/82] Added complete stop delay at the end of all motion. Moved grbl preprocessor script into a new repository. Added a very short (25 ms) user-definable delay before the steppers are disabled at the motors are disabled and grbl goes idle. This ensures any residual inertia at the end of the last motion does not cause the axes to drift and grbl to lose its position when manually entering g-code or when performing a tool change and starting the next operation. --- config.h | 12 +- script/grbl_preprocess.py | 227 -------------------------------------- stepper.c | 3 + 3 files changed, 13 insertions(+), 229 deletions(-) delete mode 100755 script/grbl_preprocess.py diff --git a/config.h b/config.h index 89d9aea..7ba20ec 100644 --- a/config.h +++ b/config.h @@ -56,8 +56,16 @@ #define SPINDLE_DIRECTION_PORT PORTB #define SPINDLE_DIRECTION_BIT 5 -// The temporal resolution of the acceleration management subsystem. Higher number -// give smoother acceleration but may impact performance +// This parameter sets the delay time before disabling the steppers after the final block of movement. +// A short delay ensures the steppers come to a complete stop and the residual inertial force in the +// CNC axes don't cause the axes to drift off position. This is particularly important when manually +// entering g-code into grbl, i.e. locating part zero or simple manual machining. If the axes drift, +// grbl has no way to know this has happened, since stepper motors are open-loop control. Depending +// on the machine, this parameter may need to be larger or smaller than the default time. +#define STEPPER_IDLE_LOCK_TIME 25 // (milliseconds) + +// The temporal resolution of the acceleration management subsystem. Higher number give smoother +// acceleration but may impact performance. // NOTE: Increasing this parameter will help any resolution related issues, especially with machines // requiring very high accelerations and/or very fast feedrates. In general, this will reduce the // error between how the planner plans the motions and how the stepper program actually performs them. diff --git a/script/grbl_preprocess.py b/script/grbl_preprocess.py deleted file mode 100755 index bbb08c8..0000000 --- a/script/grbl_preprocess.py +++ /dev/null @@ -1,227 +0,0 @@ -#!/usr/bin/env python -"""\ -G-code preprocessor for grbl (BETA!) -- Converts G02/03 arcs to G01 linear interpolations -- Removes comments, block delete characters, and line numbers -- Removes spaces and capitalizes commands -- Minor input error checking -- OPTIONAL: Remove unsupported grbl G and M commands - -TODO: -- Number precision truncation -- Arc conversion option -- More robust error checking -- Improve interface to command line options -- Improve g-code parsing to NIST standards -- Fix problem with inverse feed rates -- Positioning updates may not be correct on grbl. Need to check. - -Based on GRBL 0.7b source code by Simen Svale Skogsrud - -By: Sungeun (Sonny) Jeon -Version: 20100825 -""" -import re -from math import * -from copy import * - -# -= SETTINGS =- -filein = 'test.gcode' # Input file name -fileout = 'grbl.gcode' # Output file name -ndigits_in = 4 # inch significant digits after '.' -ndigits_mm = 2 # mm significant digits after '.' -mm_per_arc_segment = 0.1 # mm per arc segment -inch2mm = 25.4 # inch to mm conversion scalar -verbose = False # Verbose flag to show all progress -remove_unsupported = True # Removal flag for all unsupported statements - -# Initialize parser state -gc = { 'current_xyz' : [0,0,0], - 'feed_rate' : 0, # F0 - 'motion_mode' : 'SEEK', # G00 - 'plane_axis' : [0,1,2], # G17 - 'inches_mode' : False, # G21 - 'inverse_feedrate_mode' : False, # G94 - 'absolute_mode' : True} # G90 - -def unit_conv(val) : # Converts value to mm - if gc['inches_mode'] : val *= inch2mm - return(val) - -def fout_conv(val) : # Returns converted value as rounded string for output file. - if gc['inches_mode'] : return( str(round(val/inch2mm,ndigits_in)) ) - else : return( str(round(val,ndigits_mm)) ) - -# Open g-code file -fin = open(filein,'r'); -fout = open(fileout,'w'); - -# Iterate through g-code file -l_count = 0 -for line in fin: - l_count += 1 # Iterate line counter - - # Strip comments/spaces/tabs/new line and capitalize. Comment MSG not supported. - block = re.sub('\s|\(.*?\)','',line).upper() - block = re.sub('\\\\','',block) # Strip \ block delete character - block = re.sub('%','',block) # Strip % program start/stop character - - if len(block) == 0 : # Ignore empty blocks - - print "Skipping: " + line.strip() - - else : # Process valid g-code clean block. Assumes no block delete characters or comments - - g_cmd = re.findall(r'[^0-9\.\-]+',block) # Extract block command characters - g_num = re.findall(r'[0-9\.\-]+',block) # Extract block numbers - - # G-code block error checks - # if len(g_cmd) != len(g_num) : print block; raise Exception('Invalid block. Unbalanced word and values.') - if 'N' in g_cmd : - if g_cmd[0]!='N': raise Exception('Line number must be first command in line.') - if g_cmd.count('N') > 1: raise Exception('More than one line number in block.') - g_cmd = g_cmd[1:] # Remove line number word - g_num = g_num[1:] - # Block item repeat checks? (0<=n'M'<5, G/M modal groups) - - # Initialize block state - blk = { 'next_action' : 'DEFAULT', - 'absolute_override' : False, - 'target_xyz' : deepcopy(gc['current_xyz']), - 'offset_ijk' : [0,0,0], - 'radius_mode' : False, - 'unsupported': [] } - - # Pass 1 - for cmd,num in zip(g_cmd,g_num) : - fnum = float(num) - inum = int(fnum) - if cmd is 'G' : - if inum is 0 : gc['motion_mode'] = 'SEEK' - elif inum is 1 : gc['motion_mode'] = 'LINEAR' - elif inum is 2 : gc['motion_mode'] = 'CW_ARC' - elif inum is 3 : gc['motion_mode'] = 'CCW_ARC' - elif inum is 4 : blk['next_action'] = 'DWELL' - elif inum is 17 : gc['plane_axis'] = [0,1,2] # Select XY Plane - elif inum is 18 : gc['plane_axis'] = [0,2,1] # Select XZ Plane - elif inum is 19 : gc['plane_axis'] = [1,2,0] # Select YZ Plane - elif inum is 20 : gc['inches_mode'] = True - elif inum is 21 : gc['inches_mode'] = False - elif inum in [28,30] : blk['next_action'] = 'GO_HOME' - elif inum is 53 : blk['absolute_override'] = True - elif inum is 80 : gc['motion_mode'] = 'MOTION_CANCEL' - elif inum is 90 : gc['absolute_mode'] = True - elif inum is 91 : gc['absolute_mode'] = False - elif inum is 92 : blk['next_action'] = 'SET_OFFSET' - elif inum is 93 : gc['inverse_feedrate_mode'] = True - elif inum is 94 : gc['inverse_feedrate_mode'] = False - else : - print 'Unsupported command ' + cmd + num + ' on line ' + str(l_count) - if remove_unsupported : blk['unsupported'].append(zip(g_cmd,g_num).index((cmd,num))) - elif cmd is 'M' : - if inum in [0,1] : pass # Program Pause - elif inum in [2,30,60] : pass # Program Completed - elif inum is 3 : pass # Spindle Direction 1 - elif inum is 4 : pass # Spindle Direction -1 - elif inum is 5 : pass # Spindle Direction 0 - else : - print 'Unsupported command ' + cmd + num + ' on line ' + str(l_count) - if remove_unsupported : blk['unsupported'].append(zip(g_cmd,g_num).index((cmd,num))) - elif cmd is 'T' : pass # Tool Number - - # Pass 2 - for cmd,num in zip(g_cmd,g_num) : - fnum = float(num) - if cmd is 'F' : gc['feed_rate'] = unit_conv(fnum) # Feed Rate - elif cmd in ['I','J','K'] : blk['offset_ijk'][ord(cmd)-ord('I')] = unit_conv(fnum) # Arc Center Offset - elif cmd is 'P' : p = fnum # Misc value parameter - elif cmd is 'R' : r = unit_conv(fnum); blk['radius_mode'] = True # Arc Radius Mode - elif cmd is 'S' : pass # Spindle Speed - elif cmd in ['X','Y','Z'] : # Target Coordinates - if (gc['absolute_mode'] | blk['absolute_override']) : - blk['target_xyz'][ord(cmd)-ord('X')] = unit_conv(fnum) - else : - blk['target_xyz'][ord(cmd)-ord('X')] += unit_conv(fnum) - - # Execute actions - if blk['next_action'] is 'GO_HOME' : - gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position - elif blk['next_action'] is 'SET_OFFSET' : - pass - elif blk['next_action'] is 'DWELL' : - if p < 0 : raise Exception('Dwell time negative.') - else : # 'DEFAULT' - if gc['motion_mode'] is 'SEEK' : - gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position - elif gc['motion_mode'] is 'LINEAR' : - gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position - elif gc['motion_mode'] in ['CW_ARC','CCW_ARC'] : - axis = gc['plane_axis'] - - # Convert radius mode to ijk mode - if blk['radius_mode'] : - x = blk['target_xyz'][axis[0]]-gc['current_xyz'][axis[0]] - y = blk['target_xyz'][axis[1]]-gc['current_xyz'][axis[1]] - if not (x==0 and y==0) : raise Exception('Same target and current XYZ not allowed in arc radius mode.') - h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y) - if isnan(h_x2_div_d) : raise Exception('Floating point error in arc conversion') - if gc['motion_mode'] is 'CCW_ARC' : h_x2_div_d = -h_x2_div_d - if r < 0 : h_x2_div_d = -h_x2_div_d - blk['offset_ijk'][axis[0]] = (x-(y*h_x2_div_d))/2; - blk['offset_ijk'][axis[1]] = (y+(x*h_x2_div_d))/2; - - # Compute arc center, radius, theta, and depth parameters - theta_start = atan2(-blk['offset_ijk'][axis[0]], -blk['offset_ijk'][axis[1]]) - theta_end = atan2(blk['target_xyz'][axis[0]] - blk['offset_ijk'][axis[0]] - gc['current_xyz'][axis[0]], \ - blk['target_xyz'][axis[1]] - blk['offset_ijk'][axis[1]] - gc['current_xyz'][axis[1]]) - if theta_end < theta_start : theta_end += 2*pi - radius = hypot(blk['offset_ijk'][axis[0]], blk['offset_ijk'][axis[1]]) - depth = blk['target_xyz'][axis[2]]-gc['current_xyz'][axis[2]] - center_x = gc['current_xyz'][axis[0]]-sin(theta_start)*radius - center_y = gc['current_xyz'][axis[1]]-cos(theta_start)*radius - - # Compute arc incremental linear segment parameters - angular_travel = theta_end-theta_start - if gc['motion_mode'] is 'CCW_ARC' : angular_travel = angular_travel-2*pi - millimeters_of_travel = hypot(angular_travel*radius, fabs(depth)) - if millimeters_of_travel is 0 : raise Exception('G02/03 arc travel is zero') - segments = int(round(millimeters_of_travel/mm_per_arc_segment)) - if segments is 0 : raise Exception('G02/03 zero length arc segment') -# ??? # if gc['inverse_feedrate_mode'] : gc['feed_rate'] *= segments - theta_per_segment = angular_travel/segments - depth_per_segment = depth/segments - - # Generate arc linear segments - if verbose: print 'Converting: '+ block + ' : ' + str(l_count) - fout.write('G01F'+fout_conv(gc['feed_rate'])) - if not gc['absolute_mode'] : fout.write('G90') - arc_target = [0,0,0] - for i in range(1,segments+1) : - if i < segments : - arc_target[axis[0]] = center_x + radius * sin(theta_start + i*theta_per_segment) - arc_target[axis[1]] = center_y + radius * cos(theta_start + i*theta_per_segment) - arc_target[axis[2]] = gc['current_xyz'][axis[2]] + i*depth_per_segment - else : - arc_target = deepcopy(blk['target_xyz']) # Last segment at target_xyz - # Write only changed variables. - if arc_target[0] != gc['current_xyz'][0] : fout.write('X'+fout_conv(arc_target[0])) - if arc_target[1] != gc['current_xyz'][1] : fout.write('Y'+fout_conv(arc_target[1])) - if arc_target[2] != gc['current_xyz'][2] : fout.write('Z'+fout_conv(arc_target[2])) - fout.write('\n') - gc['current_xyz'] = deepcopy(arc_target) # Update position - if not gc['absolute_mode'] : fout.write('G91\n') - - # Rebuild original gcode block sans line numbers, extra characters, and unsupported commands - if gc['motion_mode'] not in ['CW_ARC','CCW_ARC'] : - if remove_unsupported and len(blk['unsupported']) : - for i in blk['unsupported'][::-1] : del g_cmd[i]; del g_num[i] - out_block = "".join([i+j for (i,j) in zip(g_cmd,g_num)]) - if len(out_block) : - if verbose : print "Writing: " + out_block + ' : ' + str(l_count) - fout.write(out_block + '\n') - -print 'Done!' - -# Close files -fin.close() -fout.close() \ No newline at end of file diff --git a/stepper.c b/stepper.c index 9786387..fdd73f9 100644 --- a/stepper.c +++ b/stepper.c @@ -84,6 +84,9 @@ void st_wake_up() { } static void st_go_idle() { + // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete + // stop and not drift from residual inertial forces at the end of the last movement. + _delay_ms(STEPPER_IDLE_LOCK_TIME); // Disable steppers by setting stepper disable STEPPERS_DISABLE_PORT |= (1< Date: Thu, 6 Oct 2011 23:14:21 -0600 Subject: [PATCH 74/82] Minor update to further eliminate the ole long slope deceleration issue. New update note! - Added another way to further ensure the long slope deceleration issue is eliminated. If the stepper rate change is too great near zero, the stepper rate is adjusted at half increments to the end of travel, creating a smooth transition. - If the new STEPPER_IDLE_LOCK_TIME is set as zero, this delay is not compiled at compile-time. - NOTE: The next update is likely going to be major, involving a full re-write of the stepper.c program to integrate a simple way to apply pauses, jogging, e-stop, and feedrate overrides. The interface should be flexible enough to be easily modified for use with either hardware switches or software commands. Coming soon. --- config.h | 17 +++++++++-------- stepper.c | 6 +++++- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/config.h b/config.h index 7ba20ec..17a566d 100644 --- a/config.h +++ b/config.h @@ -62,7 +62,8 @@ // entering g-code into grbl, i.e. locating part zero or simple manual machining. If the axes drift, // grbl has no way to know this has happened, since stepper motors are open-loop control. Depending // on the machine, this parameter may need to be larger or smaller than the default time. -#define STEPPER_IDLE_LOCK_TIME 25 // (milliseconds) +// NOTE: If defined 0, the delay will not be compiled. +#define STEPPER_IDLE_LOCK_TIME 25 // (milliseconds) - Integer >= 0 // The temporal resolution of the acceleration management subsystem. Higher number give smoother // acceleration but may impact performance. @@ -80,18 +81,18 @@ // if unwanted behavior is observed on a user's machine when running at very slow speeds. #define MINIMUM_PLANNER_SPEED 0.0 // (mm/min) -// Minimum stepper rate. Sets the absolute minimum stepper rate in the stepper program and never run +// Minimum stepper rate. Sets the absolute minimum stepper rate in the stepper program and never runs // slower than this value, except when sleeping. This parameter overrides the minimum planner speed. // This is primarily used to guarantee that the end of a movement is always reached and not stop to // never reach its target. This parameter should always be greater than zero. #define MINIMUM_STEPS_PER_MINUTE 800 // (steps/min) - Integer value only -// Number of arc generation iterations by small angle approximation before exact arc -// trajectory correction. Value must be 1-255. This parameter maybe decreased if there are issues -// with the accuracy of the arc generations. In general, the default value is more than enough for -// the intended CNC applications of grbl, and should be on the order or greater than the size of -// the buffer to help with the computational efficiency of generating arcs. -#define N_ARC_CORRECTION 25 +// Number of arc generation iterations by small angle approximation before exact arc trajectory +// correction. This parameter maybe decreased if there are issues with the accuracy of the arc +// generations. In general, the default value is more than enough for the intended CNC applications +// of grbl, and should be on the order or greater than the size of the buffer to help with the +// computational efficiency of generating arcs. +#define N_ARC_CORRECTION 25 // Integer (1-255) #endif diff --git a/stepper.c b/stepper.c index fdd73f9..f39be06 100644 --- a/stepper.c +++ b/stepper.c @@ -86,7 +86,9 @@ void st_wake_up() { static void st_go_idle() { // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete // stop and not drift from residual inertial forces at the end of the last movement. - _delay_ms(STEPPER_IDLE_LOCK_TIME); + #if STEPPER_IDLE_LOCK_TIME + _delay_ms(STEPPER_IDLE_LOCK_TIME); + #endif // Disable steppers by setting stepper disable STEPPERS_DISABLE_PORT |= (1< current_block->rate_delta) { trapezoid_adjusted_rate -= current_block->rate_delta; + } else { + trapezoid_adjusted_rate >>= 1; // Bit shift divide by 2 } if (trapezoid_adjusted_rate < current_block->final_rate) { // Reached final rate a little early. Cruise to end of block at final rate. From c98ff4cc2e8ed9461e65a37cba85c8470233ca4e Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Fri, 7 Oct 2011 15:51:40 -0600 Subject: [PATCH 75/82] Forgot something! Comments on what the last change does. --- stepper.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/stepper.c b/stepper.c index f39be06..a520038 100644 --- a/stepper.c +++ b/stepper.c @@ -207,7 +207,10 @@ SIGNAL(TIMER1_COMPA_vect) if ( iterate_trapezoid_cycle_counter() ) { // NOTE: We will only reduce speed if the result will be > 0. This catches small // rounding errors that might leave steps hanging after the last trapezoid tick. - if (trapezoid_adjusted_rate > current_block->rate_delta) { + // The if statement performs a bit shift multiply by 2 to gauge when to begin + // adjusting the rate by half increments. Prevents the long slope at the end of + // deceleration issue that occurs in certain cases. + if ((trapezoid_adjusted_rate << 1) > current_block->rate_delta) { trapezoid_adjusted_rate -= current_block->rate_delta; } else { trapezoid_adjusted_rate >>= 1; // Bit shift divide by 2 From 9141ad282540eaa50a41283685f901f29c24ddbd Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Tue, 11 Oct 2011 20:51:04 -0600 Subject: [PATCH 76/82] Third time's a charm! No more deceleration issues! Updated grbl version and settings. General cleanup. - Fleshed out the original idea to completely remove the long slope at the end of deceleration issue. This third time should absolutely eliminate it. - Changed the acceleration setting to kept as mm/min^2 internally, since this was creating unneccessary additional computation in the planner. Human readable value kept at mm/sec^2. - Updated grbl version 0.7d and settings version to 4. NOTE: Please check settings after update. These may have changed, but shouldn't. - Before updating the new features (pause, e-stop, federate override, etc), the edge branch will soon be merged with the master, barring any immediate issues that people may have, and the edge branch will be the testing ground for the new grbl version 0.8. --- config.h | 2 +- gcode.c | 6 +- motion_control.c | 8 +- motion_control_new.c | 207 +++++++++++++++++++++++++++++++++++++++++++ planner.c | 12 +-- planner.h | 2 +- settings.c | 19 ++-- settings.h | 4 +- stepper.c | 56 ++++++++---- stepper.h | 5 +- 10 files changed, 270 insertions(+), 51 deletions(-) create mode 100644 motion_control_new.c diff --git a/config.h b/config.h index 17a566d..db4d361 100644 --- a/config.h +++ b/config.h @@ -123,4 +123,4 @@ // // #define SPINDLE_DIRECTION_DDR DDRD // #define SPINDLE_DIRECTION_PORT PORTD -// #define SPINDLE_DIRECTION_BIT 7 \ No newline at end of file +// #define SPINDLE_DIRECTION_BIT 7 diff --git a/gcode.c b/gcode.c index 1930389..4b50f06 100644 --- a/gcode.c +++ b/gcode.c @@ -116,9 +116,6 @@ uint8_t gc_execute_line(char *line) { double p = 0, r = 0; int int_value; - - clear_vector(target); - clear_vector(offset); gc.status_code = STATUS_OK; @@ -171,6 +168,7 @@ uint8_t gc_execute_line(char *line) { if (gc.status_code) { return(gc.status_code); } char_counter = 0; + clear_vector(target); clear_vector(offset); memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position @@ -388,4 +386,4 @@ static int next_statement(char *letter, double *double_ptr, char *line, uint8_t group 9 = {M48, M49} enable/disable feed and speed override switches group 12 = {G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3} coordinate system selection group 13 = {G61, G61.1, G64} path control mode -*/ \ No newline at end of file +*/ diff --git a/motion_control.c b/motion_control.c index 52a72c2..c7e5670 100644 --- a/motion_control.c +++ b/motion_control.c @@ -42,12 +42,6 @@ void mc_dwell(double seconds) } } -// Execute an arc in offset mode format. position == current xyz, target == target xyz, -// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is -// the direction of helical travel, radius == circle radius, isclockwise boolean. Used -// for vector transformation direction. -// position, target, and offset are pointers to vectors from gcode.c - #ifdef __AVR_ATmega328P__ // The arc is approximated by generating a huge number of tiny, linear segments. The length of each // segment is configured in settings.mm_per_arc_segment. @@ -155,4 +149,4 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui void mc_go_home() { st_go_home(); -} \ No newline at end of file +} diff --git a/motion_control_new.c b/motion_control_new.c new file mode 100644 index 0000000..6ca0c9f --- /dev/null +++ b/motion_control_new.c @@ -0,0 +1,207 @@ +/* + motion_control.c - high level interface for issuing motion commands + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 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 . +*/ + +#include +#include "settings.h" +#include "config.h" +#include "motion_control.h" +#include +#include +#include +#include "nuts_bolts.h" +#include "stepper.h" +#include "planner.h" + +// Execute dwell in seconds. Maximum time delay is > 18 hours, more than enough for any application. +void mc_dwell(double seconds) +{ + uint16_t i = floor(seconds); + st_synchronize(); + _delay_ms(floor(1000*(seconds-i))); // Delay millisecond remainder + while (i > 0) { + _delay_ms(1000); // Delay one second + i--; + } +} + +// void mc_jog_enable() +// { +// // Planned sequence of events: +// // Send X,Y,Z motion, target step rate, direction +// // Rate_delta, step_xyz, counter_xyz should be all the same. +// // + +// Change of direction can cause some problems. Need to force a complete stop for any direction change. +// This likely needs to be done in stepper.c as a jog mode parameter. + +// !!! Need a way to get step locations realtime!!! +// Jog is a specialized case, where grbl is reset and there is no cycle start. +// If there is a real-time status elsewhere, this shouldn't be a problem. + +// st.direction_bits = current_block->direction_bits; +// st.target_rate; +// st.rate_delta; +// st.step_event_count; +// st.steps_x; +// st.steps_y; +// st.steps_z; +// st.counter_x = -(current_block->step_event_count >> 1); +// st.counter_y = st.counter_x; +// st.counter_z = st.counter_x; +// st.step_event_count = current_block->step_event_count; +// st.step_events_completed = 0; +// } + +// void mc_jog_disable() +// { +// // Calls stepper.c and disables jog mode to start deceleration. +// // Shouldn't have to anything else. Just initiate the stop, so if re-enabled, it can accelerate. +// } + +// void mc_feed_hold() +// { +// // Planned sequence of events: +// // Query stepper for interrupting cycle and hold until pause flag is set? +// // Query stepper intermittenly and check for !st.do_motion to indicate complete stop. +// // Retreive st.step_events_completed and recompute current location. +// // Truncate current block start to current location. +// // Re-plan buffer for start from zero velocity and truncated block length. +// // All necessary computations for a restart should be done by now. +// // Reset pause flag. +// // Only wait for a cycle start command from user interface. (TBD). +// // !!! Need to check how to circumvent the wait in the main program. May need to be in serial.c +// // as an interrupt process call. Can two interrupt programs exist at the same time?? +// } + +// Execute an arc in offset mode format. position == current xyz, target == target xyz, +// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is +// the direction of helical travel, radius == circle radius, isclockwise boolean. Used +// for vector transformation direction. +// position, target, and offset are pointers to vectors from gcode.c + +#ifdef __AVR_ATmega328P__ +// The arc is approximated by generating a huge number of tiny, linear segments. The length of each +// segment is configured in settings.mm_per_arc_segment. +void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, + uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise) +{ +// int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); +// plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc + + double center_axis0 = position[axis_0] + offset[axis_0]; + double center_axis1 = position[axis_1] + offset[axis_1]; + double linear_travel = target[axis_linear] - position[axis_linear]; + double r_axis0 = -offset[axis_0]; // Radius vector from center to current location + double r_axis1 = -offset[axis_1]; + double rt_axis0 = target[axis_0] - center_axis0; + double rt_axis1 = target[axis_1] - center_axis1; + + // CCW angle between position and target from circle center. Only one atan2() trig computation required. + double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); + if (angular_travel < 0) { angular_travel += 2*M_PI; } + if (isclockwise) { angular_travel -= 2*M_PI; } + + double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel)); + if (millimeters_of_travel == 0.0) { return; } + uint16_t segments = floor(millimeters_of_travel/settings.mm_per_arc_segment); + // Multiply inverse feed_rate to compensate for the fact that this movement is approximated + // by a number of discrete segments. The inverse feed_rate should be correct for the sum of + // all segments. + if (invert_feed_rate) { feed_rate *= segments; } + + double theta_per_segment = angular_travel/segments; + double linear_per_segment = linear_travel/segments; + + /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, + and phi is the angle of rotation. Based on the solution approach by Jens Geisler. + r_T = [cos(phi) -sin(phi); + sin(phi) cos(phi] * r ; + + For arc generation, the center of the circle is the axis of rotation and the radius vector is + defined from the circle center to the initial position. Each line segment is formed by successive + vector rotations. This requires only two cos() and sin() computations to form the rotation + matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since + all double numbers are single precision on the Arduino. (True double precision will not have + round off issues for CNC applications.) Single precision error can accumulate to be greater than + tool precision in some cases. Therefore, arc path correction is implemented. + + Small angle approximation may be used to reduce computation overhead further. This approximation + holds for everything, but very small circles and large mm_per_arc_segment values. In other words, + theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large + to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for + numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an + issue for CNC machines with the single precision Arduino calculations. + + This approximation also allows mc_arc to immediately insert a line segment into the planner + without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied + a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. + This is important when there are successive arc motions. + */ + // Vector rotation matrix values + double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation + double sin_T = theta_per_segment; + + double arc_target[3]; + double sin_Ti; + double cos_Ti; + double r_axisi; + uint16_t i; + int8_t count = 0; + + // Initialize the linear axis + arc_target[axis_linear] = position[axis_linear]; + + for (i = 1; iinitial_rate, block->nominal_rate, acceleration_per_minute)); int32_t decelerate_steps = floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute)); - + // Calculate the size of Plateau of Nominal Rate. int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; @@ -382,7 +382,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in // specifically for each line to compensate for this phenomenon: // Convert universal acceleration for direction-dependent stepper rate change parameter block->rate_delta = ceil( block->step_event_count*inverse_millimeters * - settings.acceleration*60.0 / ACCELERATION_TICKS_PER_SECOND ); // (step/min/acceleration_tick) + settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick) // Perform planner-enabled calculations if (acceleration_manager_enabled) { @@ -421,7 +421,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in // Compute maximum junction velocity based on maximum acceleration and junction deviation double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, - sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); + sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } } @@ -462,7 +462,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in memcpy(position, target, sizeof(target)); // position[] = target[] if (acceleration_manager_enabled) { planner_recalculate(); } - st_wake_up(); + st_cycle_start(); } // Reset the planner position vector and planner speed @@ -472,4 +472,4 @@ void plan_set_current_position(double x, double y, double z) { position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. clear_vector_double(previous_unit_vec); -} \ No newline at end of file +} diff --git a/planner.h b/planner.h index f995210..f91b7c2 100644 --- a/planner.h +++ b/planner.h @@ -74,4 +74,4 @@ int plan_is_acceleration_manager_enabled(); // Reset the position vector void plan_set_current_position(double x, double y, double z); -#endif \ No newline at end of file +#endif diff --git a/settings.c b/settings.c index f85e3c3..09514d6 100644 --- a/settings.c +++ b/settings.c @@ -49,10 +49,10 @@ typedef struct { #define DEFAULT_Z_STEPS_PER_MM (94.488188976378*MICROSTEPS) #define DEFAULT_STEP_PULSE_MICROSECONDS 30 #define DEFAULT_MM_PER_ARC_SEGMENT 0.1 -#define DEFAULT_RAPID_FEEDRATE 500.0 // in millimeters per minute +#define DEFAULT_RAPID_FEEDRATE 500.0 // mm/min #define DEFAULT_FEEDRATE 500.0 -#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/10.0) -#define DEFAULT_JUNCTION_DEVIATION 0.05 +#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE*60*60/10.0) // mm/min^2 +#define DEFAULT_JUNCTION_DEVIATION 0.05 // mm #define DEFAULT_STEPPING_INVERT_MASK ((1< #include -#define GRBL_VERSION "0.7c" +#define GRBL_VERSION "0.7d" // 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 3 +#define SETTINGS_VERSION 4 // Current global settings (persisted in EEPROM from byte 1 onwards) typedef struct { diff --git a/stepper.c b/stepper.c index a520038..c8ee56c 100644 --- a/stepper.c +++ b/stepper.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Modifications Copyright (c) 2011 Sungeun K. Jeon + Copyright (c) 2011 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 @@ -56,6 +56,8 @@ static uint32_t cycles_per_step_event; // The number of machine cycles be static uint32_t trapezoid_tick_cycle_counter; // The cycles since last trapezoid_tick. Used to generate ticks at a steady // pace without allocating a separate timer static uint32_t trapezoid_adjusted_rate; // The current rate of step_events according to the trapezoid generator +static uint32_t min_safe_rate; // Minimum safe rate for full deceleration rate reduction step. Otherwise halves step_rate. +static uint8_t cycle_start; // Cycle start flag to indicate program start and block processing. // __________________________ // /| |\ _________________ ^ @@ -76,14 +78,20 @@ static uint32_t trapezoid_adjusted_rate; // The current rate of step_events static void set_step_events_per_minute(uint32_t steps_per_minute); +// Stepper state initialization void st_wake_up() { + // Initialize stepper output bits + out_bits = (0) ^ (settings.invert_mask); // Enable steppers by resetting the stepper disable port STEPPERS_DISABLE_PORT &= ~(1<initial_rate; + trapezoid_adjusted_rate = current_block->initial_rate; + min_safe_rate = current_block->rate_delta + (current_block->rate_delta >> 1); // 1.5 x rate_delta trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule. set_step_events_per_minute(trapezoid_adjusted_rate); // Initialize cycles_per_step_event } @@ -116,23 +125,24 @@ static uint8_t iterate_trapezoid_cycle_counter() { } } -// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with +// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. // It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse. // The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts. SIGNAL(TIMER1_COMPA_vect) { - // TODO: Check if the busy-flag can be eliminated by just disabeling this interrupt while we are in it + // TODO: Check if the busy-flag can be eliminated by just disabling this interrupt while we are in it if(busy){ return; } // The busy-flag is used to avoid reentering this interrupt - // Set the direction pins a cuple of nanoseconds before we step the steppers + // Set the direction pins a couple of nanoseconds before we step the steppers STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK); // Then pulse the stepping pins STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits; // Reset step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after // exactly settings.pulse_microseconds microseconds. - TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND)/8); - +// TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND)/8); + TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Bit shift divide by 8. + busy = true; sei(); // Re enable interrupts (normally disabled while inside an interrupt handler) // ((We re-enable interrupts in order for SIG_OVERFLOW2 to be able to be triggered @@ -172,7 +182,7 @@ SIGNAL(TIMER1_COMPA_vect) counter_z -= current_block->step_event_count; } - step_events_completed += 1; // Iterate step events + step_events_completed++; // Iterate step events // While in block steps, check for de/ac-celeration events and execute them accordingly. if (step_events_completed < current_block->step_event_count) { @@ -205,12 +215,15 @@ SIGNAL(TIMER1_COMPA_vect) } else { // Iterate cycle counter and check if speeds need to be reduced. if ( iterate_trapezoid_cycle_counter() ) { - // NOTE: We will only reduce speed if the result will be > 0. This catches small - // rounding errors that might leave steps hanging after the last trapezoid tick. - // The if statement performs a bit shift multiply by 2 to gauge when to begin - // adjusting the rate by half increments. Prevents the long slope at the end of - // deceleration issue that occurs in certain cases. - if ((trapezoid_adjusted_rate << 1) > current_block->rate_delta) { + // NOTE: We will only do a full speed reduction if the result is more than the minimum safe + // rate, initialized in trapezoid reset as 1.5 x rate_delta. Otherwise, reduce the speed by + // half increments until finished. The half increments are guaranteed not to exceed the + // CNC acceleration limits, because they will never be greater than rate_delta. This catches + // small errors that might leave steps hanging after the last trapezoid tick or a very slow + // step rate at the end of a full stop deceleration in certain situations. The half rate + // reductions should only be called once or twice per block and create a nice smooth + // end deceleration. + if (trapezoid_adjusted_rate > min_safe_rate) { trapezoid_adjusted_rate -= current_block->rate_delta; } else { trapezoid_adjusted_rate >>= 1; // Bit shift divide by 2 @@ -235,11 +248,8 @@ SIGNAL(TIMER1_COMPA_vect) current_block = NULL; plan_discard_current_block(); } - - } else { - // Still no block? Set the stepper pins to low before sleeping. - out_bits = 0; - } + + } out_bits ^= settings.invert_mask; // Apply stepper invert mask busy=false; @@ -339,3 +349,11 @@ void st_go_home() limits_go_home(); plan_set_current_position(0,0,0); } + +// Planner external interface to start stepper interrupt and execute the blocks in queue. +void st_cycle_start() { + if (!cycle_start) { + cycle_start = true; + st_wake_up(); + } +} diff --git a/stepper.h b/stepper.h index 5d5758c..73d3e7c 100644 --- a/stepper.h +++ b/stepper.h @@ -38,8 +38,7 @@ void st_synchronize(); // Execute the homing cycle void st_go_home(); -// The stepper subsystem goes to sleep when it runs out of things to execute. Call this -// to notify the subsystem that it is time to go to work. -void st_wake_up(); +// Notify the stepper subsystem to start executing the g-code program in buffer. +void st_cycle_start(); #endif From 169c859b9ce314c81448d3d156f10232a29fafc1 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Tue, 11 Oct 2011 21:00:06 -0600 Subject: [PATCH 77/82] Delete a new work file shouldn't have been synced. --- motion_control_new.c | 207 ------------------------------------------- 1 file changed, 207 deletions(-) delete mode 100644 motion_control_new.c diff --git a/motion_control_new.c b/motion_control_new.c deleted file mode 100644 index 6ca0c9f..0000000 --- a/motion_control_new.c +++ /dev/null @@ -1,207 +0,0 @@ -/* - motion_control.c - high level interface for issuing motion commands - Part of Grbl - - Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011 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 . -*/ - -#include -#include "settings.h" -#include "config.h" -#include "motion_control.h" -#include -#include -#include -#include "nuts_bolts.h" -#include "stepper.h" -#include "planner.h" - -// Execute dwell in seconds. Maximum time delay is > 18 hours, more than enough for any application. -void mc_dwell(double seconds) -{ - uint16_t i = floor(seconds); - st_synchronize(); - _delay_ms(floor(1000*(seconds-i))); // Delay millisecond remainder - while (i > 0) { - _delay_ms(1000); // Delay one second - i--; - } -} - -// void mc_jog_enable() -// { -// // Planned sequence of events: -// // Send X,Y,Z motion, target step rate, direction -// // Rate_delta, step_xyz, counter_xyz should be all the same. -// // - -// Change of direction can cause some problems. Need to force a complete stop for any direction change. -// This likely needs to be done in stepper.c as a jog mode parameter. - -// !!! Need a way to get step locations realtime!!! -// Jog is a specialized case, where grbl is reset and there is no cycle start. -// If there is a real-time status elsewhere, this shouldn't be a problem. - -// st.direction_bits = current_block->direction_bits; -// st.target_rate; -// st.rate_delta; -// st.step_event_count; -// st.steps_x; -// st.steps_y; -// st.steps_z; -// st.counter_x = -(current_block->step_event_count >> 1); -// st.counter_y = st.counter_x; -// st.counter_z = st.counter_x; -// st.step_event_count = current_block->step_event_count; -// st.step_events_completed = 0; -// } - -// void mc_jog_disable() -// { -// // Calls stepper.c and disables jog mode to start deceleration. -// // Shouldn't have to anything else. Just initiate the stop, so if re-enabled, it can accelerate. -// } - -// void mc_feed_hold() -// { -// // Planned sequence of events: -// // Query stepper for interrupting cycle and hold until pause flag is set? -// // Query stepper intermittenly and check for !st.do_motion to indicate complete stop. -// // Retreive st.step_events_completed and recompute current location. -// // Truncate current block start to current location. -// // Re-plan buffer for start from zero velocity and truncated block length. -// // All necessary computations for a restart should be done by now. -// // Reset pause flag. -// // Only wait for a cycle start command from user interface. (TBD). -// // !!! Need to check how to circumvent the wait in the main program. May need to be in serial.c -// // as an interrupt process call. Can two interrupt programs exist at the same time?? -// } - -// Execute an arc in offset mode format. position == current xyz, target == target xyz, -// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is -// the direction of helical travel, radius == circle radius, isclockwise boolean. Used -// for vector transformation direction. -// position, target, and offset are pointers to vectors from gcode.c - -#ifdef __AVR_ATmega328P__ -// The arc is approximated by generating a huge number of tiny, linear segments. The length of each -// segment is configured in settings.mm_per_arc_segment. -void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, - uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise) -{ -// int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); -// plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc - - double center_axis0 = position[axis_0] + offset[axis_0]; - double center_axis1 = position[axis_1] + offset[axis_1]; - double linear_travel = target[axis_linear] - position[axis_linear]; - double r_axis0 = -offset[axis_0]; // Radius vector from center to current location - double r_axis1 = -offset[axis_1]; - double rt_axis0 = target[axis_0] - center_axis0; - double rt_axis1 = target[axis_1] - center_axis1; - - // CCW angle between position and target from circle center. Only one atan2() trig computation required. - double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); - if (angular_travel < 0) { angular_travel += 2*M_PI; } - if (isclockwise) { angular_travel -= 2*M_PI; } - - double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel)); - if (millimeters_of_travel == 0.0) { return; } - uint16_t segments = floor(millimeters_of_travel/settings.mm_per_arc_segment); - // Multiply inverse feed_rate to compensate for the fact that this movement is approximated - // by a number of discrete segments. The inverse feed_rate should be correct for the sum of - // all segments. - if (invert_feed_rate) { feed_rate *= segments; } - - double theta_per_segment = angular_travel/segments; - double linear_per_segment = linear_travel/segments; - - /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, - and phi is the angle of rotation. Based on the solution approach by Jens Geisler. - r_T = [cos(phi) -sin(phi); - sin(phi) cos(phi] * r ; - - For arc generation, the center of the circle is the axis of rotation and the radius vector is - defined from the circle center to the initial position. Each line segment is formed by successive - vector rotations. This requires only two cos() and sin() computations to form the rotation - matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since - all double numbers are single precision on the Arduino. (True double precision will not have - round off issues for CNC applications.) Single precision error can accumulate to be greater than - tool precision in some cases. Therefore, arc path correction is implemented. - - Small angle approximation may be used to reduce computation overhead further. This approximation - holds for everything, but very small circles and large mm_per_arc_segment values. In other words, - theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large - to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for - numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an - issue for CNC machines with the single precision Arduino calculations. - - This approximation also allows mc_arc to immediately insert a line segment into the planner - without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied - a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. - This is important when there are successive arc motions. - */ - // Vector rotation matrix values - double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation - double sin_T = theta_per_segment; - - double arc_target[3]; - double sin_Ti; - double cos_Ti; - double r_axisi; - uint16_t i; - int8_t count = 0; - - // Initialize the linear axis - arc_target[axis_linear] = position[axis_linear]; - - for (i = 1; i Date: Fri, 11 Nov 2011 13:36:42 -0700 Subject: [PATCH 78/82] Corrected clearing of target and position variable for the go home routine. Thanks Jens! --- gcode.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gcode.c b/gcode.c index 4b50f06..7fb5b30 100644 --- a/gcode.c +++ b/gcode.c @@ -211,7 +211,7 @@ uint8_t gc_execute_line(char *line) { // Perform any physical actions switch (next_action) { - case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break; + case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(target); break; case NEXT_ACTION_DWELL: mc_dwell(p); break; case NEXT_ACTION_SET_COORDINATE_OFFSET: mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]); From 292fcca67faf6f781db46e8578e3314271026b17 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sat, 19 Nov 2011 10:08:41 -0700 Subject: [PATCH 79/82] Re-ordered stepper idle function to first disable interrupt. --- stepper.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stepper.c b/stepper.c index c8ee56c..b810259 100644 --- a/stepper.c +++ b/stepper.c @@ -92,6 +92,8 @@ void st_wake_up() { static void st_go_idle() { // Cycle finished. Set flag to false. cycle_start = false; + // Disable stepper driver interrupt + TIMSK1 &= ~(1< Date: Sat, 19 Nov 2011 14:10:55 -0700 Subject: [PATCH 80/82] Updated README and reordered branch versions. --- readme.textile | 16 ++++++++--- stepper.c | 73 +++++++++++++++++++++++++------------------------- 2 files changed, 48 insertions(+), 41 deletions(-) diff --git a/readme.textile b/readme.textile index fab1580..c4d2b97 100644 --- a/readme.textile +++ b/readme.textile @@ -4,12 +4,20 @@ Grbl is a no-compromise, high performance, low cost alternative to parallel-port The controller is written in highly optimized C utilizing every clever feature of the AVR-chips to achieve precise timing and asynchronous operation. It is able to maintain more than 30kHz of stable, jitter free control pulses. -It accepts standards-compliant G-code and has been tested with the output of several CAM tools with no problems. Arcs, circles and helical motion are fully supported - but no support for tool offsets, functions or variables as these are apocryphal and fell into disuse after humans left G-code authoring to machines some time in the 80s. +It accepts standards-compliant G-code and has been tested with the output of several CAM tools with no problems. Arcs, circles and helical motion are fully supported, as well as, other basic functional g-code commands. Functions and variables are not currently supported, but may be included in future releases in a form of a pre-processor. -Grbl includes full acceleration management with look ahead. That means the controller will look up to 20 motions into the future and plan its velocities ahead to deliver smooth acceleration and jerk-free cornering. +Grbl includes full acceleration management with look ahead. That means the controller will look up to 16 to 20 motions into the future and plan its velocities ahead to deliver smooth acceleration and jerk-free cornering. -*Important note for Atmega 168 users:* Grbl used to be compatible with both the older Ardunios running atmega 168 and the newer with 328p. The full version of Grbl now compiles without support for circles/arcs if you target 168. If you need arcs, but not acceleration-management I am still maintaining Grbl 0.51 "in the branch called 'v0_51'":https://github.com/simen/grbl/tree/v0_51. +*Changelog for v0.7 from v0.6:* + - Significantly improved and optimized planner re-factoring. + - New robust cornering algorithm, enabling smoother and faster motions. + - Arc acceleration planning enabled by efficient vector transformation implementation. + - Stepper subsystem re-factoring to help remove some motion issues from pre-v0.7 builds. + - Increased dwell times. + - G92 Coordinate offset support. + - (Beta) Limit switch and homing cycle support. + - Many other bug fixes and efficiency improvements. -*Note for users upgrading from 0.51 to 0.6:* The new version has new and improved default pin-out. If nothing works when you upgrade, that is because the pulse trains are coming from the wrong pins. This is a simple matter of editing config.h – the whole legacy pin assignment is there for you to uncomment. +*Important note for Atmega 168 users:* Going forward, support for Atmega 168 will be dropped due to its limited memory and speed. However, legacy Grbl v0.51 "in the branch called 'v0_51' is still available for use. _The project was initially inspired by the Arduino GCode Interpreter by Mike Ellery_ diff --git a/stepper.c b/stepper.c index b810259..d8110e5 100644 --- a/stepper.c +++ b/stepper.c @@ -49,7 +49,7 @@ static int32_t counter_x, // Counter variables for the bresenham line trac counter_y, counter_z; static uint32_t step_events_completed; // The number of step events executed in the current block -static volatile int busy; // true when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler. +static volatile uint8_t busy; // true when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler. // Variables used by the trapezoid generation static uint32_t cycles_per_step_event; // The number of machine cycles between each step event @@ -79,7 +79,8 @@ static uint8_t cycle_start; // Cycle start flag to indicate program start an static void set_step_events_per_minute(uint32_t steps_per_minute); // Stepper state initialization -void st_wake_up() { +void st_wake_up() +{ // Initialize stepper output bits out_bits = (0) ^ (settings.invert_mask); // Enable steppers by resetting the stepper disable port @@ -89,7 +90,8 @@ void st_wake_up() { } // Stepper shutdown -static void st_go_idle() { +void st_go_idle() +{ // Cycle finished. Set flag to false. cycle_start = false; // Disable stepper driver interrupt @@ -105,7 +107,8 @@ static void st_go_idle() { // Initializes the trapezoid generator from the current block. Called whenever a new // block begins. -static void trapezoid_generator_reset() { +static void trapezoid_generator_reset() +{ trapezoid_adjusted_rate = current_block->initial_rate; min_safe_rate = current_block->rate_delta + (current_block->rate_delta >> 1); // 1.5 x rate_delta trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule. @@ -115,7 +118,8 @@ static void trapezoid_generator_reset() { // This function determines an acceleration velocity change every CYCLES_PER_ACCELERATION_TICK by // keeping track of the number of elapsed cycles during a de/ac-celeration. The code assumes that // step_events occur significantly more often than the acceleration velocity iterations. -static uint8_t iterate_trapezoid_cycle_counter() { +static uint8_t iterate_trapezoid_cycle_counter() +{ trapezoid_tick_cycle_counter += cycles_per_step_event; if(trapezoid_tick_cycle_counter > CYCLES_PER_ACCELERATION_TICK) { trapezoid_tick_cycle_counter -= CYCLES_PER_ACCELERATION_TICK; @@ -186,16 +190,13 @@ SIGNAL(TIMER1_COMPA_vect) // While in block steps, check for de/ac-celeration events and execute them accordingly. if (step_events_completed < current_block->step_event_count) { - // The trapezoid generator always checks step event location to ensure de/ac-celerations are // executed and terminated at exactly the right time. This helps prevent over/under-shooting // the target position and speed. - // NOTE: By increasing the ACCELERATION_TICKS_PER_SECOND in config.h, the resolution of the // discrete velocity changes increase and accuracy can increase as well to a point. Numerical // round-off errors can effect this, if set too high. This is important to note if a user has // very high acceleration and/or feedrate requirements for their machine. - if (step_events_completed < current_block->accelerate_until) { // Iterate cycle counter and check if speeds need to be increased. if ( iterate_trapezoid_cycle_counter() ) { @@ -248,9 +249,7 @@ SIGNAL(TIMER1_COMPA_vect) current_block = NULL; plan_discard_current_block(); } - } - out_bits ^= settings.invert_mask; // Apply stepper invert mask busy=false; } @@ -306,33 +305,33 @@ static uint32_t config_step_timer(uint32_t cycles) uint16_t ceiling; uint16_t prescaler; uint32_t actual_cycles; - if (cycles <= 0xffffL) { - ceiling = cycles; - prescaler = 0; // prescaler: 0 - actual_cycles = ceiling; - } else if (cycles <= 0x7ffffL) { - ceiling = cycles >> 3; - prescaler = 1; // prescaler: 8 - actual_cycles = ceiling * 8L; - } else if (cycles <= 0x3fffffL) { - ceiling = cycles >> 6; - prescaler = 2; // prescaler: 64 - actual_cycles = ceiling * 64L; - } else if (cycles <= 0xffffffL) { - ceiling = (cycles >> 8); - prescaler = 3; // prescaler: 256 - actual_cycles = ceiling * 256L; - } else if (cycles <= 0x3ffffffL) { - ceiling = (cycles >> 10); - prescaler = 4; // prescaler: 1024 - actual_cycles = ceiling * 1024L; - } else { - // Okay, that was slower than we actually go. Just set the slowest speed - ceiling = 0xffff; - prescaler = 4; - actual_cycles = 0xffff * 1024; - } - // Set prescaler + if (cycles <= 0xffffL) { + ceiling = cycles; + prescaler = 0; // prescaler: 0 + actual_cycles = ceiling; + } else if (cycles <= 0x7ffffL) { + ceiling = cycles >> 3; + prescaler = 1; // prescaler: 8 + actual_cycles = ceiling * 8L; + } else if (cycles <= 0x3fffffL) { + ceiling = cycles >> 6; + prescaler = 2; // prescaler: 64 + actual_cycles = ceiling * 64L; + } else if (cycles <= 0xffffffL) { + ceiling = (cycles >> 8); + prescaler = 3; // prescaler: 256 + actual_cycles = ceiling * 256L; + } else if (cycles <= 0x3ffffffL) { + ceiling = (cycles >> 10); + prescaler = 4; // prescaler: 1024 + actual_cycles = ceiling * 1024L; + } else { + // Okay, that was slower than we actually go. Just set the slowest speed + ceiling = 0xffff; + prescaler = 4; + actual_cycles = 0xffff * 1024; + } + // Set prescaler TCCR1B = (TCCR1B & ~(0x07< Date: Sat, 10 Dec 2011 11:18:24 -0700 Subject: [PATCH 81/82] Various minor updates and variable definition corrections. Removed deprecated acceleration manager. - Removed deprecated acceleration manager (non-functional since v0.7b) - Updated variable types and function headers. - Updated stepper interrupt to ISR() from SIGNAL()+sei(). - General code cleanup. --- main.c | 6 +- motion_control.c | 5 -- nuts_bolts.c | 20 +++++ nuts_bolts.h | 2 +- planner.c | 194 ++++++++++++++++++++++------------------------- planner.h | 19 ++--- protocol.c | 20 +++-- protocol.h | 1 + serial.c | 52 ++++++------- stepper.c | 96 +++++++++++------------ 10 files changed, 210 insertions(+), 205 deletions(-) diff --git a/main.c b/main.c index b56bf6f..829ec5a 100644 --- a/main.c +++ b/main.c @@ -36,7 +36,7 @@ int main(void) { - sei(); + sei(); // Enable interrupts serial_init(BAUD_RATE); protocol_init(); @@ -47,8 +47,8 @@ int main(void) gc_init(); limits_init(); - for(;;){ - sleep_mode(); // Wait for it ... + while (1) { +// sleep_mode(); // Wait for it ... protocol_process(); // ... process the serial protocol } return 0; /* never reached */ diff --git a/motion_control.c b/motion_control.c index c7e5670..06b7868 100644 --- a/motion_control.c +++ b/motion_control.c @@ -48,9 +48,6 @@ void mc_dwell(double seconds) void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise) { -// int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); -// plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc - double center_axis0 = position[axis_0] + offset[axis_0]; double center_axis1 = position[axis_1] + offset[axis_1]; double linear_travel = target[axis_linear] - position[axis_linear]; @@ -141,8 +138,6 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui } // Ensure last segment arrives at target location. plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); - -// plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled); } #endif diff --git a/nuts_bolts.c b/nuts_bolts.c index 0a8bf5c..8a6960c 100644 --- a/nuts_bolts.c +++ b/nuts_bolts.c @@ -1,3 +1,23 @@ +/* + nuts_bolts.c - Shared functions + Part of Grbl + + 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 "nuts_bolts.h" #include #include diff --git a/nuts_bolts.h b/nuts_bolts.h index 37f3aa6..54b3cb8 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -1,5 +1,5 @@ /* - motion_control.h - cartesian robot controller. + nuts_bolts.h - Header file for shared definitions, variables, and functions Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud diff --git a/planner.c b/planner.c index 6dea6b2..44913c8 100644 --- a/planner.c +++ b/planner.c @@ -46,12 +46,11 @@ static int32_t position[3]; // The current position of the tool in a static double previous_unit_vec[3]; // Unit vector of previous path line segment static double previous_nominal_speed; // Nominal speed of previous path line segment -static uint8_t acceleration_manager_enabled; // Acceleration management active? - // Returns the index of the next block in the ring buffer // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. -static int8_t next_block_index(int8_t block_index) { +static uint8_t next_block_index(uint8_t block_index) +{ block_index++; if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } return(block_index); @@ -59,7 +58,8 @@ static int8_t next_block_index(int8_t block_index) { // Returns the index of the previous block in the ring buffer -static int8_t prev_block_index(int8_t block_index) { +static uint8_t prev_block_index(uint8_t block_index) +{ if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } block_index--; return(block_index); @@ -68,7 +68,8 @@ static int8_t prev_block_index(int8_t block_index) { // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // given acceleration: -static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) { +static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) +{ return( (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration) ); } @@ -86,7 +87,8 @@ static double estimate_acceleration_distance(double initial_rate, double target_ // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after // a total travel of distance. This can be used to compute the intersection point between acceleration and // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) -static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) { +static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) +{ return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration) ); } @@ -96,13 +98,15 @@ static double intersection_distance(double initial_rate, double final_rate, doub // NOTE: sqrt() reimplimented here from prior version due to improved planner logic. Increases speed // in time critical computations, i.e. arcs or rapid short lines from curves. Guaranteed to not exceed // BLOCK_BUFFER_SIZE calls per planner cycle. -static double max_allowable_speed(double acceleration, double target_velocity, double distance) { +static double max_allowable_speed(double acceleration, double target_velocity, double distance) +{ return( sqrt(target_velocity*target_velocity-2*acceleration*distance) ); } // The kernel called by planner_recalculate() when scanning the plan from last to first entry. -static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { +static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) +{ if (!current) { return; } // Cannot operate on nothing. if (next) { @@ -128,8 +132,9 @@ static void planner_reverse_pass_kernel(block_t *previous, block_t *current, blo // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This // implements the reverse pass. -static void planner_reverse_pass() { - auto int8_t block_index = block_buffer_head; +static void planner_reverse_pass() +{ + uint8_t block_index = block_buffer_head; block_t *block[3] = {NULL, NULL, NULL}; while(block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); @@ -143,7 +148,8 @@ static void planner_reverse_pass() { // The kernel called by planner_recalculate() when scanning the plan from first to last entry. -static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) { +static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) +{ if(!previous) { return; } // Begin planning after buffer_tail // If the previous block is an acceleration block, but it is not long enough to complete the @@ -167,8 +173,9 @@ static void planner_forward_pass_kernel(block_t *previous, block_t *current, blo // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This // implements the forward pass. -static void planner_forward_pass() { - int8_t block_index = block_buffer_tail; +static void planner_forward_pass() +{ + uint8_t block_index = block_buffer_tail; block_t *block[3] = {NULL, NULL, NULL}; while(block_index != block_buffer_head) { @@ -194,8 +201,8 @@ static void planner_forward_pass() { // The factors represent a factor of braking and must be in the range 0.0-1.0. // This converts the planner parameters to the data required by the stepper controller. // NOTE: Final rates must be computed in terms of their respective blocks. -static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { - +static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) +{ block->initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min) block->final_rate = ceil(block->nominal_rate*exit_factor); // (step/min) int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; // (step/min^2) @@ -235,8 +242,9 @@ static void calculate_trapezoid_for_block(block_t *block, double entry_factor, d // planner_recalculate() after updating the blocks. Any recalulate flagged junction will // compute the two adjacent trapezoids to the junction, since the junction speed corresponds // to exit speed and entry speed of one another. -static void planner_recalculate_trapezoids() { - int8_t block_index = block_buffer_tail; +static void planner_recalculate_trapezoids() +{ + uint8_t block_index = block_buffer_tail; block_t *current; block_t *next = NULL; @@ -281,49 +289,41 @@ static void planner_recalculate_trapezoids() { // All planner computations are performed with doubles (float on Arduinos) to minimize numerical round- // off errors. Only when planned values are converted to stepper rate parameters, these are integers. -static void planner_recalculate() { +static void planner_recalculate() +{ planner_reverse_pass(); planner_forward_pass(); planner_recalculate_trapezoids(); } -void plan_init() { +void plan_init() +{ block_buffer_head = 0; block_buffer_tail = 0; - plan_set_acceleration_manager_enabled(true); clear_vector(position); clear_vector_double(previous_unit_vec); previous_nominal_speed = 0.0; } -void plan_set_acceleration_manager_enabled(uint8_t enabled) { - if ((!!acceleration_manager_enabled) != (!!enabled)) { - st_synchronize(); - acceleration_manager_enabled = !!enabled; - } -} - -int plan_is_acceleration_manager_enabled() { - return(acceleration_manager_enabled); -} - -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 ); } } -block_t *plan_get_current_block() { +block_t *plan_get_current_block() +{ if (block_buffer_head == block_buffer_tail) { return(NULL); } return(&block_buffer[block_buffer_tail]); } // 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 +// 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(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) { - +void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) +{ // Calculate target position in absolute steps int32_t target[3]; target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); @@ -331,7 +331,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); // Calculate the buffer head after we push this byte - int next_buffer_head = next_block_index( block_buffer_head ); + uint8_t next_buffer_head = next_block_index( block_buffer_head ); // If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer. while(block_buffer_tail == next_buffer_head) { sleep_mode(); } @@ -384,84 +384,72 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in block->rate_delta = ceil( block->step_event_count*inverse_millimeters * settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick) - // Perform planner-enabled calculations - if (acceleration_manager_enabled) { - - // Compute path unit vector - double unit_vec[3]; + // Compute path unit vector + double unit_vec[3]; - unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; - unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; - unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; - - // 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. - double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed + unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; + unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; + unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; - // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. - if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 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. - double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; - - // Skip and use default max junction speed for 0 degree acute junction. - if (cos_theta < 0.95) { - vmax_junction = min(previous_nominal_speed,block->nominal_speed); - // 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 - double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. - vmax_junction = min(vmax_junction, - sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); - } + // 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. + double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed + + // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. + if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 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. + double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; + + // Skip and use default max junction speed for 0 degree acute junction. + if (cos_theta < 0.95) { + vmax_junction = min(previous_nominal_speed,block->nominal_speed); + // 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 + double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. + vmax_junction = min(vmax_junction, + sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } - block->max_entry_speed = vmax_junction; - - // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. - double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); - block->entry_speed = min(vmax_junction, v_allowable); - - // Initialize planner efficiency flags - // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. - // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then - // the current block and next block junction speeds are guaranteed to always be at their maximum - // junction speeds in deceleration and acceleration, respectively. This is due to how the current - // block nominal speed limits both the current and next maximum junction speeds. Hence, in both - // the reverse and forward planners, the corresponding block junction speed will always be at the - // the maximum junction speed and may always be ignored for any speed reduction checks. - if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; } - else { block->nominal_length_flag = false; } - block->recalculate_flag = true; // Always calculate trapezoid for new block + } + block->max_entry_speed = vmax_junction; - // Update previous path unit_vector and nominal speed - memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] - previous_nominal_speed = block->nominal_speed; + // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. + double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); + block->entry_speed = min(vmax_junction, v_allowable); - } else { - // Acceleration planner disabled. Set minimum that is required. - block->initial_rate = block->nominal_rate; - block->final_rate = block->nominal_rate; - block->accelerate_until = 0; - block->decelerate_after = block->step_event_count; - block->rate_delta = 0; - } + // Initialize planner efficiency flags + // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. + // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then + // the current block and next block junction speeds are guaranteed to always be at their maximum + // junction speeds in deceleration and acceleration, respectively. This is due to how the current + // block nominal speed limits both the current and next maximum junction speeds. Hence, in both + // the reverse and forward planners, the corresponding block junction speed will always be at the + // the maximum junction speed and may always be ignored for any speed reduction checks. + if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; } + else { block->nominal_length_flag = false; } + block->recalculate_flag = true; // Always calculate trapezoid for new block + + // Update previous path unit_vector and nominal speed + memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] + previous_nominal_speed = block->nominal_speed; // Move buffer head block_buffer_head = next_buffer_head; // Update position memcpy(position, target, sizeof(target)); // position[] = target[] - if (acceleration_manager_enabled) { planner_recalculate(); } + planner_recalculate(); st_cycle_start(); } diff --git a/planner.h b/planner.h index f91b7c2..f8f59e8 100644 --- a/planner.h +++ b/planner.h @@ -27,12 +27,12 @@ // 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 - uint32_t steps_x, steps_y, steps_z; // Step count along each axis 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 - uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute - + // Fields used by the motion planner to manage acceleration double nominal_speed; // The nominal speed for this block in mm/min double entry_speed; // Entry speed at previous-current junction in mm/min @@ -42,12 +42,13 @@ typedef struct { uint8_t nominal_length_flag; // Planner flag for nominal speed always reached // Settings for the trapezoid generator - uint32_t initial_rate; // The jerk-adjusted step rate at start of block - uint32_t final_rate; // The minimal rate at exit + uint32_t initial_rate; // The step rate at start of block + uint32_t final_rate; // The step rate at end of block int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) uint32_t accelerate_until; // The index of the step event on which to stop acceleration uint32_t decelerate_after; // The index of the step event on which to start decelerating - + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + } block_t; // Initialize the motion plan subsystem @@ -65,12 +66,6 @@ void plan_discard_current_block(); // Gets the current block. Returns NULL if buffer empty block_t *plan_get_current_block(); -// Enables or disables acceleration-management for upcoming blocks -void plan_set_acceleration_manager_enabled(uint8_t enabled); - -// Is acceleration-management currently enabled? -int plan_is_acceleration_manager_enabled(); - // Reset the position vector void plan_set_current_position(double x, double y, double z); diff --git a/protocol.c b/protocol.c index 4835c90..4010253 100644 --- a/protocol.c +++ b/protocol.c @@ -31,10 +31,12 @@ #include #define LINE_BUFFER_SIZE 50 -static char line[LINE_BUFFER_SIZE]; -static uint8_t char_counter; +static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. +static uint8_t char_counter; // Last character counter in line variable. +static uint8_t iscomment; // Comment/block delete flag for processor to ignore comment characters. -static void status_message(int status_code) { +static void status_message(int status_code) +{ if (status_code == 0) { printPgmString(PSTR("ok\r\n")); } else { @@ -57,12 +59,15 @@ static void status_message(int status_code) { void protocol_init() { + char_counter = 0; // Reset line input + iscomment = false; printPgmString(PSTR("\r\nGrbl " GRBL_VERSION)); printPgmString(PSTR("\r\n")); } // Executes one line of input according to protocol -uint8_t protocol_execute_line(char *line) { +uint8_t protocol_execute_line(char *line) +{ if(line[0] == '$') { return(settings_execute_line(line)); // Delegate lines starting with '$' to the settings module } else { @@ -70,15 +75,16 @@ uint8_t protocol_execute_line(char *line) { } } + +// Process one line of incoming serial data. Remove unneeded characters and capitalize. void protocol_process() { char c; - uint8_t iscomment = false; while((c = serial_read()) != SERIAL_NO_DATA) { - if ((c == '\n') || (c == '\r')) { // End of block reached + if ((c == '\n') || (c == '\r')) { // End of line reached if (char_counter > 0) {// Line is complete. Then execute! - line[char_counter] = 0; // terminate string + line[char_counter] = 0; // Terminate string status_message(protocol_execute_line(line)); } else { // Empty or comment line. Skip block. diff --git a/protocol.h b/protocol.h index 3ad6597..4490a4b 100644 --- a/protocol.h +++ b/protocol.h @@ -3,6 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 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 diff --git a/serial.c b/serial.c index 7310e3f..04483bb 100644 --- a/serial.c +++ b/serial.c @@ -44,25 +44,25 @@ volatile uint8_t tx_buffer_tail = 0; static void set_baud_rate(long baud) { uint16_t UBRR0_value = ((F_CPU / 16 + baud / 2) / baud - 1); - UBRR0H = UBRR0_value >> 8; - UBRR0L = UBRR0_value; + UBRR0H = UBRR0_value >> 8; + UBRR0L = UBRR0_value; } void serial_init(long baud) { set_baud_rate(baud); - /* baud doubler off - Only needed on Uno XXX */ + /* baud doubler off - Only needed on Uno XXX */ UCSR0A &= ~(1 << U2X0); - // enable rx and tx + // enable rx and tx UCSR0B |= 1<> 3); // Bit shift divide by 8. - - busy = true; - sei(); // Re enable interrupts (normally disabled while inside an interrupt handler) - // ((We re-enable interrupts in order for SIG_OVERFLOW2 to be able to be triggered - // at exactly the right time even if we occasionally spend a lot of time inside this handler.)) + TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // If there is no current block, attempt to pop one from the buffer if (current_block == NULL) { - // Anything in the buffer? + // Anything in the buffer? If so, initialize next motion. current_block = plan_get_current_block(); if (current_block != NULL) { trapezoid_generator_reset(); @@ -256,37 +252,39 @@ SIGNAL(TIMER1_COMPA_vect) // This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets // the motor port after a short period (settings.pulse_microseconds) completing one step cycle. -SIGNAL(TIMER2_OVF_vect) +ISR(TIMER2_OVF_vect) { - // reset stepping pins (leave the direction pins) + // Reset stepping pins (leave the direction pins) STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); } // Initialize and start the stepper motor subsystem void st_init() { - // Configure directions of interface pins - STEPPING_DDR |= STEPPING_MASK; + // Configure directions of interface pins + STEPPING_DDR |= STEPPING_MASK; STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask; STEPPERS_DISABLE_DDR |= 1<> 3; - prescaler = 1; // prescaler: 8 - actual_cycles = ceiling * 8L; + ceiling = cycles >> 3; + prescaler = 1; // prescaler: 8 + actual_cycles = ceiling * 8L; } else if (cycles <= 0x3fffffL) { - ceiling = cycles >> 6; - prescaler = 2; // prescaler: 64 - actual_cycles = ceiling * 64L; + ceiling = cycles >> 6; + prescaler = 2; // prescaler: 64 + actual_cycles = ceiling * 64L; } else if (cycles <= 0xffffffL) { - ceiling = (cycles >> 8); - prescaler = 3; // prescaler: 256 - actual_cycles = ceiling * 256L; + ceiling = (cycles >> 8); + prescaler = 3; // prescaler: 256 + actual_cycles = ceiling * 256L; } else if (cycles <= 0x3ffffffL) { - ceiling = (cycles >> 10); - prescaler = 4; // prescaler: 1024 - actual_cycles = ceiling * 1024L; + ceiling = (cycles >> 10); + prescaler = 4; // prescaler: 1024 + actual_cycles = ceiling * 1024L; } else { // Okay, that was slower than we actually go. Just set the slowest speed - ceiling = 0xffff; - prescaler = 4; - actual_cycles = 0xffff * 1024; + ceiling = 0xffff; + prescaler = 4; + actual_cycles = 0xffff * 1024; } // Set prescaler TCCR1B = (TCCR1B & ~(0x07< Date: Sun, 15 Jan 2012 19:05:06 -0700 Subject: [PATCH 82/82] Propagated premature step end bug fix from the edge branch. Updated printFloat() function. - Will not be uploading a hex build of this, unless asked. --- print.c | 35 ++++++++++++++++++++++------------- stepper.c | 31 +++++++++++++++++++------------ 2 files changed, 41 insertions(+), 25 deletions(-) diff --git a/print.c b/print.c index 05524a3..f8e7a0c 100644 --- a/print.c +++ b/print.c @@ -3,6 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 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 @@ -28,20 +29,21 @@ #ifndef DECIMAL_PLACES #define DECIMAL_PLACES 3 +#define DECIMAL_MULTIPLIER 10*10*10 #endif void printString(const char *s) { - while (*s) - serial_write(*s++); + while (*s) + serial_write(*s++); } // Print a string stored in PGM-memory void printPgmString(const char *s) { char c; - while ((c = pgm_read_byte_near(s++))) - serial_write(c); + while ((c = pgm_read_byte_near(s++))) + serial_write(c); } void printIntegerInBase(unsigned long n, unsigned long base) @@ -78,18 +80,25 @@ void printInteger(long n) // A very simple void printFloat(double n) { - double integer_part, fractional_part; - uint8_t decimal_part; - fractional_part = modf(n, &integer_part); - printInteger(integer_part); + if (n < 0) { + serial_write('-'); + n = -n; + } + n += 0.5/DECIMAL_MULTIPLIER; // Add rounding factor + + long integer_part; + integer_part = (int)n; + printIntegerInBase(integer_part,10); + serial_write('.'); - fractional_part *= 10; + + n -= integer_part; int decimals = DECIMAL_PLACES; + uint8_t decimal_part; while(decimals-- > 0) { - decimal_part = floor(fractional_part); + n *= 10; + decimal_part = (int) n; serial_write('0'+decimal_part); - fractional_part -= decimal_part; - fractional_part *= 10; + n -= decimal_part; } } - diff --git a/stepper.c b/stepper.c index 3f2899c..418355a 100644 --- a/stepper.c +++ b/stepper.c @@ -133,21 +133,25 @@ static uint8_t iterate_trapezoid_cycle_counter() // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. // It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse. // The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts. -// NOTE: ISR_NOBLOCK allows SIG_OVERFLOW2 to trigger on-time regardless of time in this handler. This is -// the compiler optimizable equivalent of the old SIGNAL() and sei() method. -ISR(TIMER1_COMPA_vect,ISR_NOBLOCK) +ISR(TIMER1_COMPA_vect) { if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt - busy = true; // Set the direction pins a couple of nanoseconds before we step the steppers STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK); // Then pulse the stepping pins STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits; - // Reset step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after - // exactly settings.pulse_microseconds microseconds. - TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); - + // Enable step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after + // exactly settings.pulse_microseconds microseconds, independent of the main Timer1 prescaler. + TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Reload timer counter + TCCR2B = (1<