From ea5b8942db2616e93fc0478922010c3bab7c0481 Mon Sep 17 00:00:00 2001 From: chamnit Date: Mon, 15 Aug 2011 17:06:50 -0600 Subject: [PATCH 01/32] 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 02/32] 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 03/32] 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 04/32] 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 05/32] 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 06/32] 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 07/32] 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 08/32] 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 09/32] 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 10/32] 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 11/32] 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 12/32] 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 13/32] 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 14/32] 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 15/32] 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 16/32] 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 17/32] 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 18/32] 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 19/32] 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 20/32] 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 21/32] 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 22/32] 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 23/32] 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 24/32] 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 25/32] 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 26/32] 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: Thu, 8 Dec 2011 18:47:48 -0700 Subject: [PATCH 27/32] Initial v0.8 ALPHA commit. Features multi-tasking run-time command execution (feed hold, cycle start, reset, status query). Extensive re-structuring of code for future features. - ALPHA status. - Multitasking ability with run-time command executions for real-time control and feedback. - Decelerating feed hold and resume during operation. - System abort/reset, which immediately kills all movement and re-initializes grbl. - Re-structured grbl to easily allow for new features: Status reporting, jogging, backlash compensation. (To be completed in the following releases.) - Resized TX/RX serial buffers (32/128 bytes) - Increased planner buffer size to 20 blocks. - Updated documentation. --- config.h | 50 ++---- doc/commands.txt | 21 +++ doc/planner-maths.txt | 30 ---- doc/structure.txt | 14 +- gcode.c | 4 - limits.c | 6 +- main.c | 64 +++++-- motion_control.c | 84 +++++++-- motion_control.h | 9 +- nuts_bolts.c | 20 +++ nuts_bolts.h | 19 +- planner.c | 287 +++++++++++++++++------------- planner.h | 33 ++-- protocol.c | 116 ++++++++++-- protocol.h | 4 + readme.textile | 27 ++- serial.c | 92 ++++++---- serial.h | 4 + settings.c | 4 +- settings.h | 2 +- spindle_control.c | 7 +- stepper.c | 404 +++++++++++++++++++++++++----------------- stepper.h | 17 +- 23 files changed, 841 insertions(+), 477 deletions(-) create mode 100644 doc/commands.txt delete mode 100644 doc/planner-maths.txt diff --git a/config.h b/config.h index db4d361..56f4b9d 100644 --- a/config.h +++ b/config.h @@ -26,9 +26,7 @@ #define BAUD_RATE 9600 -// Updated default pin-assignments from 0.6 onwards -// (see bottom of file for a copy of the old config) - +// Define pin-assignments #define STEPPERS_DISABLE_DDR DDRB #define STEPPERS_DISABLE_PORT PORTB #define STEPPERS_DISABLE_BIT 0 @@ -56,6 +54,16 @@ #define SPINDLE_DIRECTION_PORT PORTB #define SPINDLE_DIRECTION_BIT 5 +// Define runtime command special characters. These characters are 'picked-off' directly from the +// serial read data stream and are not passed to the grbl line execution parser. Select characters +// that do not and must not exist in the streamed g-code program. ASCII control characters may be +// used, if they are available per user setup. +// TODO: Solidify these default characters. Temporary for now. +#define CMD_STATUS_REPORT '?' +#define CMD_FEED_HOLD '!' +#define CMD_CYCLE_START '~' +#define CMD_RESET 0x18 // ctrl-x + // 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 @@ -94,33 +102,11 @@ // computational efficiency of generating arcs. #define N_ARC_CORRECTION 25 // Integer (1-255) +// Time delay increments performed during a dwell. The default value is set at 50ms, which provides +// a maximum time delay of roughly 55 minutes, more than enough for most any application. Increasing +// this delay will increase the maximum dwell time linearly, but also reduces the responsiveness of +// run-time command executions, like status reports, since these are performed between each dwell +// time step. Also, keep in mind that the Arduino delay timer is not very accurate for long delays. +#define DWELL_TIME_STEP 50 // Integer (milliseconds) + #endif - -// Pin-assignments from Grbl 0.5 - -// #define STEPPERS_DISABLE_DDR DDRD -// #define STEPPERS_DISABLE_PORT PORTD -// #define STEPPERS_DISABLE_BIT 2 -// -// #define STEPPING_DDR DDRC -// #define STEPPING_PORT PORTC -// #define X_STEP_BIT 0 -// #define Y_STEP_BIT 1 -// #define Z_STEP_BIT 2 -// #define X_DIRECTION_BIT 3 -// #define Y_DIRECTION_BIT 4 -// #define Z_DIRECTION_BIT 5 -// -// #define LIMIT_DDR DDRD -// #define LIMIT_PORT PORTD -// #define X_LIMIT_BIT 3 -// #define Y_LIMIT_BIT 4 -// #define Z_LIMIT_BIT 5 -// -// #define SPINDLE_ENABLE_DDR DDRD -// #define SPINDLE_ENABLE_PORT PORTD -// #define SPINDLE_ENABLE_BIT 6 -// -// #define SPINDLE_DIRECTION_DDR DDRD -// #define SPINDLE_DIRECTION_PORT PORTD -// #define SPINDLE_DIRECTION_BIT 7 diff --git a/doc/commands.txt b/doc/commands.txt new file mode 100644 index 0000000..aacf76a --- /dev/null +++ b/doc/commands.txt @@ -0,0 +1,21 @@ +Runtime commands for Grbl +========================= + +In normal operation, grbl accepts g-code blocks followed by a carriage return. Each block is then parsed, processed, and placed into a ring buffer with computed acceleration profiles. Grbl will respond with an 'ok' or 'error:XXX' for each block received. + +As of v0.8, grbl features multi-tasking events, which allow for immediate execution of run-time commands regardless of what grbl is doing. With this functionality, direct control of grbl may be possible, such as a controlled decelerating feed hold, resume, and system abort/reset. In addition, this provides the ability to report the real-time status of the CNC machine's current location and feed rates. + +How it works: The run-time commands are defined as special characters, which are picked off the serial read buffer at an interrupt level. The serial interrupt then sets a run-time command system flag for the main program to execute when ready. The main program consists of run-time command check points placed strategically in various points in the program, where grbl maybe idle waiting for room in a buffer or the execution time from the last check point may exceed a fraction of a second. + +How to interface: From a terminal connection, run-time commands may be sent at anytime via keystrokes. When streaming g-code, the user interface should be able to send these special characters independently of the stream. Grbl will not write these run-time command special characters to the buffer, so they may be placed anywhere in the stream at anytime, as long as there is room in the buffer. Also, ensure that the g-code program being streamed to grbl does not contain any of these special characters in the program. These characters may be defined per user requirements in the 'config.h' file. + +Run-time commands: + +- Feed Hold: This initiates an immediate controlled deceleration of the streaming g-code program to a stop. The deceleration, limited by the machine acceleration settings, ensures no steps are lost and positioning is maintained. Grbl may still receive and buffer g-code blocks as the feed hold is being executed. Once the feed hold completes, grbl will replan the buffer and resume upon a 'cycle start' command. + +- Cycle Start: (a.k.a. Resume) For now, cycle start only resumes the g-code program after a feed hold. In later releases, this may also function as a way to initiate the steppers manually when a user would like to fill the planner buffer completely before starting the cycle. + +- Reset: This issues an immediate shutdown of the stepper motors and a system abort. The main program will exit back to the main loop and re-initialize grbl. + +- Status Report: (TODO) In future releases, this will provide real-time positioning, feed rate, and block processed data, as well as other important data to the user. This also may be considered a 'poor-man's' DRO (digital read-out), where grbl thinks it is, rather than a direct and absolute measurement. + diff --git a/doc/planner-maths.txt b/doc/planner-maths.txt deleted file mode 100644 index 49f371f..0000000 --- a/doc/planner-maths.txt +++ /dev/null @@ -1,30 +0,0 @@ -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 4992f80..cf176a6 100644 --- a/doc/structure.txt +++ b/doc/structure.txt @@ -4,7 +4,8 @@ The general structure of Grbl The main processing stack: 'protocol' : Accepts command lines from the serial port and passes them to 'gcode' for execution. - Provides status responses for each command. + Provides status responses for each command. Also manages run-time commands set by + the serial interrupt. 'gcode' : Recieves gcode from 'protocol', parses it according to the current state of the parser and issues commands via '..._control' modules @@ -14,8 +15,8 @@ The main processing stack: 'motion_control' : Accepts motion commands from 'gcode' and passes them to the 'planner'. This module represents the public interface of the planner/stepper duo. -'planner' : Recieves linear motion commands from 'motion_control' and adds them to the plan of - prepared motions. It takes care of continously optimizing the acceleration profile +'planner' : Receives linear motion commands from 'motion_control' and adds them to the plan of + prepared motions. It takes care of continuously optimizing the acceleration profile as motions are added. 'stepper' : Executes the motions by stepping the steppers according to the plan. @@ -27,15 +28,16 @@ Supporting files: 'config.h' : Compile time user settings -'settings' : Maintains the run time settings record in eeprom and makes it availible +'settings' : Maintains the run time settings record in eeprom and makes it available to all modules. 'eeprom' : A library from Atmel that provides methods for reading and writing the eeprom with a small addition from us that read and write binary streams with check sums used to verify validity of the settings record. -'nuts_bolts.h' : A tiny collection of useful constants and macros used everywhere +'nuts_bolts.h' : A collection of global variable definitions, useful constants, and macros used everywhere -'serial' : Low level serial communications +'serial' : Low level serial communications and picks off run-time commands real-time for asynchronous + control. 'print' : Functions to print strings of different formats (using serial) \ No newline at end of file diff --git a/gcode.c b/gcode.c index 7fb5b30..1936015 100644 --- a/gcode.c +++ b/gcode.c @@ -127,10 +127,8 @@ 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; @@ -226,7 +224,6 @@ 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) { /* @@ -336,7 +333,6 @@ uint8_t gc_execute_line(char *line) { r, isclockwise); break; -#endif } } diff --git a/limits.c b/limits.c index 6444c8d..ec0b432 100644 --- a/limits.c +++ b/limits.c @@ -24,6 +24,10 @@ #include "settings.h" #include "nuts_bolts.h" #include "config.h" +#include "motion_control.h" +#include "planner.h" + +// TODO: Deprecated. Need to update for new version. void limits_init() { LIMIT_DDR &= ~(LIMIT_MASK); @@ -87,7 +91,7 @@ static void leave_limit_switch(bool x, bool y, bool z) { } void limits_go_home() { - st_synchronize(); + plan_synchronize(); // Store the current limit switch state uint8_t original_limit_state = LIMIT_PIN; approach_limit_switch(false, false, true); // First home the z axis diff --git a/main.c b/main.c index b56bf6f..a1b0b1d 100644 --- a/main.c +++ b/main.c @@ -3,7 +3,9 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - + Copyright (c) 2011 Sungeun K. Jeon + Copyright (c) 2011 Jens Geisler + Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or @@ -21,9 +23,11 @@ #include #include #include +#include #include #include "config.h" #include "planner.h" +#include "nuts_bolts.h" #include "stepper.h" #include "spindle_control.h" #include "motion_control.h" @@ -34,22 +38,56 @@ #include "settings.h" #include "serial.h" +#include "print.h" + +// Declare system global variables +uint8_t sys_abort; // Global system abort flag +volatile uint8_t sys_state; // Global system state variable + int main(void) { - sei(); - - serial_init(BAUD_RATE); - protocol_init(); - settings_init(); - plan_init(); - st_init(); - spindle_init(); - gc_init(); - limits_init(); + // Initialize system + sei(); // Enable interrupts + serial_init(BAUD_RATE); // Setup serial baud rate and interrupts + st_init(); // Setup stepper pins and interrupt timers + sys_abort = true; // Set abort to complete initialization - for(;;){ - sleep_mode(); // Wait for it ... + while(1) { + + // Upon a system abort, the main program will return to this loop. Once here, it is safe to + // re-initialize the system. Upon startup, the system will automatically reset to finish the + // initialization process. + if (sys_abort) { + // Execute system reset + sys_state = 0; // Reset system state + sys_abort = false; // Release system abort + + // Reset system. + serial_reset_read_buffer(); // Clear serial read buffer + settings_init(); // Load grbl settings from EEPROM + protocol_init(); // Clear incoming line data + plan_init(); // Clear block buffer and planner variables + gc_init(); // Set g-code parser to default state + spindle_init(); + limits_init(); + + // TODO: For now, the stepper subsystem tracks the absolute stepper position from the point + // of power up or hard reset. This reset is a soft reset, where the information of the current + // position is not lost after a system abort. This is not guaranteed to be correct, since + // during an abort, the steppers can lose steps in the immediate stop. However, if a feed + // hold is performed before a system abort, this position should be correct. In the next few + // updates, this soft reset feature will be fleshed out along with the status reporting and + // jogging features. + st_reset(); // Clear stepper subsystem variables. Machine position variable is not reset. + + // Print grbl initialization message + printPgmString(PSTR("\r\nGrbl " GRBL_VERSION)); + printPgmString(PSTR("\r\n'$' to dump current settings\r\n")); + } + + protocol_execute_runtime(); protocol_process(); // ... process the serial protocol + } return 0; /* never reached */ } diff --git a/motion_control.c b/motion_control.c index c7e5670..a5985c4 100644 --- a/motion_control.c +++ b/motion_control.c @@ -4,6 +4,7 @@ Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2011 Sungeun K. Jeon + Copyright (c) 2011 Jens Geisler Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -29,28 +30,55 @@ #include "nuts_bolts.h" #include "stepper.h" #include "planner.h" +#include "limits.h" +#include "protocol.h" -// Execute dwell in seconds. Maximum time delay is > 18 hours, more than enough for any application. -void mc_dwell(double seconds) +#include "print.h" + +// 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: This is the primary gateway to the grbl planner. All line motions, including arc line +// segments, must pass through this routine before being passed to the planner. The seperation of +// mc_line and plan_buffer_line is done primarily to make backlash compensation integration simple +// and direct. +// TODO: Check for a better way to avoid having to push the arguments twice for non-backlash cases. +// However, this keeps the memory requirements lower since it doesn't have to call and hold two +// plan_buffer_lines in memory. Grbl only has to retain the original line input variables during a +// backlash segment(s). +void mc_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) { - 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--; - } + // TODO: Backlash compensation may be installed here. Only need direction info to track when + // to insert a backlash line motion(s) before the intended line motion. Requires its own + // plan_check_full_buffer() and check for system abort loop. + + // If the buffer is full: good! That means we are well ahead of the robot. + // Remain in this loop until there is room in the buffer. + do { + protocol_execute_runtime(); // Check for any run-time commands + if (sys_abort) { return; } // Bail, if system abort. + } while ( plan_check_full_buffer() ); + plan_buffer_line(x, y, z, feed_rate, invert_feed_rate); + + // Auto-cycle start. + // TODO: Determine a more efficient and robust way of implementing the auto-starting the cycle. + // For example, only auto-starting when the buffer is full; if there was only one g-code command + // sent during manual operation; or if there is buffer starvation, making sure it minimizes any + // dwelling/motion hiccups. Additionally, these situations must not auto-start during a feed hold. + // Only the cycle start runtime command should be able to restart the cycle after a feed hold. + st_cycle_start(); } -#ifdef __AVR_ATmega328P__ + +// 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. // 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]; @@ -136,17 +164,35 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui arc_target[axis_0] = center_axis0 + r_axis0; arc_target[axis_1] = center_axis1 + r_axis1; arc_target[axis_linear] += linear_per_segment; - plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], feed_rate, invert_feed_rate); + mc_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], feed_rate, invert_feed_rate); + // Bail mid-circle on system abort. Runtime command check already performed by mc_line. + if (sys_abort) { return; } } // Ensure last segment arrives at target location. - 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); + mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); } -#endif + +// Execute dwell in seconds. +void mc_dwell(double seconds) +{ + uint16_t i = floor(1000/DWELL_TIME_STEP*seconds); + plan_synchronize(); + _delay_ms(floor(1000*seconds-i*DWELL_TIME_STEP)); // Delay millisecond remainder + while (i > 0) { + // NOTE: Check and execute runtime commands during dwell every <= DWELL_TIME_STEP milliseconds. + protocol_execute_runtime(); + if (sys_abort) { return; } + _delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment + i--; + } +} + + +// TODO: Update limits and homing cycle subprograms for better integration with new features. void mc_go_home() { - st_go_home(); + limits_go_home(); + plan_set_current_position(0,0,0); } diff --git a/motion_control.h b/motion_control.h index b91650d..003893a 100644 --- a/motion_control.h +++ b/motion_control.h @@ -25,24 +25,21 @@ #include #include "planner.h" -// NOTE: Although the following functions structurally belongs in this module, there is nothing to do but +// NOTE: Although the following function structurally belongs in this module, there is nothing to do but // to forward the request to the planner. +#define mc_set_current_position(x, y, z) plan_set_current_position(x, y, z) // 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) +void mc_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate); -#define mc_set_current_position(x, y, z) plan_set_current_position(x, y, z) - -#ifdef __AVR_ATmega328P__ // 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. 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); -#endif // Dwell for a specific number of seconds void mc_dwell(double seconds); 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..665de25 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 @@ -37,6 +37,23 @@ #define max(a,b) (((a) > (b)) ? (a) : (b)) #define min(a,b) (((a) < (b)) ? (a) : (b)) +// Define system state bit map. Used internally by runtime protocol as runtime command flags. +// NOTE: The system state is an unsigned 8-bit volatile variable and has a 8 flag limit. The default +// flags are always false, so the runtime protocol only needs to check for a non-zero state value to +// know when there is a runtime command to execute. +#define BIT_STATUS_REPORT 1 // bit 00000001 +#define BIT_CYCLE_START 2 // bit 00000010 +#define BIT_FEED_HOLD 4 // bit 00000100 +#define BIT_RESET 8 // bit 00001000 +#define BIT_REPLAN_CYCLE 16 // bit 00010000 +// #define 32 // bit 00100000 +// #define 64 // bit 01000000 +// #define 128 // bit 10000000 + +// Define global system variables +extern uint8_t sys_abort; // Global system abort flag +extern volatile uint8_t sys_state; // Global system state variable + // 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 diff --git a/planner.c b/planner.c index 6dea6b2..aeba0cd 100644 --- a/planner.c +++ b/planner.c @@ -4,6 +4,7 @@ Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2011 Sungeun K. Jeon + Copyright (c) 2011 Jens Geisler Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -30,28 +31,26 @@ #include "stepper.h" #include "settings.h" #include "config.h" +#include "protocol.h" // The number of linear motions that can be in the plan at any give time -#ifdef __AVR_ATmega328P__ -#define BLOCK_BUFFER_SIZE 18 -#else -#define BLOCK_BUFFER_SIZE 5 -#endif +#define BLOCK_BUFFER_SIZE 20 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 uint8_t next_buffer_head; // Index of the next buffer head -static int32_t position[3]; // The current position of the tool in absolute steps +static int32_t position[3]; // The planner position of the tool in absolute steps +// static int32_t coord_offset[3]; // Current coordinate offset from machine zero 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? - // 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,63 +289,71 @@ 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() { - block_buffer_head = 0; - block_buffer_tail = 0; - plan_set_acceleration_manager_enabled(true); +void plan_reset_buffer() +{ + block_buffer_tail = block_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); +} + +void plan_init() +{ + plan_reset_buffer(); clear_vector(position); +// clear_vector(coord_offset); 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]); } +// Returns the availability status of the block ring buffer. True, if full. +uint8_t plan_check_full_buffer() +{ + if (block_buffer_tail == next_buffer_head) { return(true); } + return(false); +} + +// Block until all buffered steps are executed. +void plan_synchronize() +{ + while(plan_get_current_block()) { + protocol_execute_runtime(); // Check and execute run-time commands + if (sys_abort) { return; } // Check for system abort + } +} // Add a new linear movement to the buffer. 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) { - +// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. +void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) +{ + // Prepare to set up new block + block_t *block = &block_buffer[block_buffer_head]; + // Calculate target position in absolute steps int32_t target[3]; target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); - - // Calculate the buffer head after we push this byte - 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]; // Compute direction bits for this block block->direction_bits = 0; @@ -384,92 +400,113 @@ 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 - - // 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 { - // 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; } + 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 + + // 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; + + // Update buffer head and next buffer head indices + block_buffer_head = next_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); - // Move buffer head - block_buffer_head = next_buffer_head; // Update position memcpy(position, target, sizeof(target)); // position[] = target[] - if (acceleration_manager_enabled) { planner_recalculate(); } - st_cycle_start(); + planner_recalculate(); } // Reset the planner position vector and planner speed -void plan_set_current_position(double x, double y, double z) { +void plan_set_current_position(double x, double y, double z) +{ + // Track the position offset from the initial position + // TODO: Need to make sure coord_offset is robust and/or needed. Can be used for a soft reset, + // where the machine position is retained after a system abort/reset. However, this is not + // correlated to the actual machine position after a soft reset and may not be needed. This could + // be left to a user interface to maintain. +// coord_offset[X_AXIS] += position[X_AXIS]; +// coord_offset[Y_AXIS] += position[Y_AXIS]; +// coord_offset[Z_AXIS] += position[Z_AXIS]; 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]); + position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); +// coord_offset[X_AXIS] -= position[X_AXIS]; +// coord_offset[Y_AXIS] -= position[Y_AXIS]; +// coord_offset[Z_AXIS] -= position[Z_AXIS]; previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. clear_vector_double(previous_unit_vec); } + +// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. +// Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped. +void plan_cycle_reinitialize(int32_t step_events_remaining) +{ + block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block + + // Only remaining millimeters and step_event_count need to be updated for planner recalculate. + // Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to + // ensure the original planned motion is resumed exactly. + block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count; + block->step_event_count = step_events_remaining; + + // Re-plan from a complete stop. Reset planner entry speeds and flags. + block->entry_speed = 0.0; + block->max_entry_speed = 0.0; + block->nominal_length_flag = false; + block->recalculate_flag = true; + planner_recalculate(); +} diff --git a/planner.h b/planner.h index f91b7c2..fc692b5 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,13 +66,19 @@ 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); +void plan_set_current_position(double x, double y, double z); + +// Reinitialize plan with a partially completed block +void plan_cycle_reinitialize(int32_t step_events_remaining); + +// Reset buffer +void plan_reset_buffer(); + +// Returns the status of the block ring buffer. True, if buffer is full. +uint8_t plan_check_full_buffer(); + +// Block until all buffered steps are executed +void plan_synchronize(); #endif diff --git a/protocol.c b/protocol.c index 4835c90..da31da8 100644 --- a/protocol.c +++ b/protocol.c @@ -29,12 +29,17 @@ #include #include "nuts_bolts.h" #include +#include "stepper.h" +#include "planner.h" + #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 { @@ -55,30 +60,115 @@ static void status_message(int status_code) { } } -void protocol_init() + +void protocol_status_report() { - printPgmString(PSTR("\r\nGrbl " GRBL_VERSION)); - printPgmString(PSTR("\r\n")); + // TODO: Status report data is written to the user here. This function should be able to grab a + // real-time snapshot of the stepper subprogram and the actual location of the CNC machine. At a + // minimum, status report should return real-time location information. Other important information + // may be distance to go on block, processed block id, and feed rate. A secondary, non-critical + // status report may include g-code state, i.e. inch mode, plane mode, absolute mode, etc. + // The report generated must be as short as possible, yet still provide the user easily readable + // information, i.e. 'x0.23 y120.4 z2.4'. This is necessary as it minimizes the computational + // overhead and allows grbl to keep running smoothly, especially with g-code programs with fast, + // short line segments and interface setups that require real-time status reports (10-20Hz). + printString("Query Received.\r\n"); // Notify that it's working. } + +void protocol_init() +{ + char_counter = 0; // Reset line input + iscomment = false; +} + + +// Executes run-time commands, when required. This is called from various check points in the main +// program, primarily where there may be a while loop waiting for a buffer to clear space or any +// point where the execution time from the last check point may be more than a fraction of a second. +// This is a way to execute runtime commands asynchronously (aka multitasking) with grbl's g-code +// parsing and planning functions. +// NOTE: The sys_state variable flags are set by the serial read subprogram, except where noted. +void protocol_execute_runtime() +{ + if (sys_state) { // Enter only if any bit flag is enabled + + // System abort. Steppers have already been force stopped. + if (sys_state & BIT_RESET) { + sys_abort = true; + return; // Nothing else to do but exit. + } + + // Execute and serial print status + if (sys_state & BIT_STATUS_REPORT) { + protocol_status_report(); + sys_state ^= BIT_STATUS_REPORT; // Toggle off + } + + // Initiate stepper feed hold + if (sys_state & BIT_FEED_HOLD) { + st_feed_hold(); + sys_state ^= BIT_FEED_HOLD; // Toggle off + } + + // Re-plans the buffer after a feed hold completes + // NOTE: BIT_REPLAN_CYCLE is set by the stepper subsystem when the feed hold is complete. + if (sys_state & BIT_REPLAN_CYCLE) { + st_cycle_reinitialize(); + sys_state ^= BIT_REPLAN_CYCLE; // Toggle off + } + + if (sys_state & BIT_CYCLE_START) { + st_cycle_start(); // Issue cycle start command to stepper subsystem + sys_state ^= BIT_CYCLE_START; // Toggle off + } + } +} + + // 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 if { + // + // JOG MODE + // + // TODO: Here jogging can be placed for execution as a seperate subprogram. It does not need to be + // susceptible to other runtime commands except for e-stop. The jogging function is intended to + // be a basic toggle on/off with controlled acceleration and deceleration to prevent skipped + // steps. The user would supply the desired feedrate, axis to move, and direction. Toggle on would + // start motion and toggle off would initiate a deceleration to stop. One could 'feather' the + // motion by repeatedly toggling to slow the motion to the desired location. Location data would + // need to be updated real-time and supplied to the user through status queries. + // More controlled exact motions can be taken care of by inputting G0 or G1 commands, which are + // handled by the planner. It would be possible for the jog subprogram to insert blocks into the + // block buffer without having the planner plan them. It would need to manage de/ac-celerations + // on its own carefully. This approach could be effective and possibly size/memory efficient. + } else { return(gc_execute_line(line)); // Everything else is gcode } } + +// 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 + uint8_t c; + while((c = serial_read()) != SERIAL_NO_DATA) { + if ((c == '\n') || (c == '\r')) { // End of line reached + + // Runtime command check point before executing line. Prevent any furthur line executions. + // NOTE: If there is no line, this function should quickly return to the main program when + // the buffer empties of non-executable data. + protocol_execute_runtime(); + if (sys_abort) { return; } // Bail to main program upon system abort + 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..1ac66ae 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 @@ -36,4 +37,7 @@ void protocol_process(); // Executes one line of input according to protocol uint8_t protocol_execute_line(char *line); +// Checks and executes a runtime command at various stop points in main program +void protocol_execute_runtime(); + #endif diff --git a/readme.textile b/readme.textile index fab1580..4f0bb98 100644 --- a/readme.textile +++ b/readme.textile @@ -4,12 +4,33 @@ 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. -*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.8 from v0.7:* + - *ALPHA STATUS*: Major structural overhaul to allow for multi-tasking events and new feature sets + - New run-time command control: Feed hold (pause), Cycle start (resume), Reset (abort), Status report (TBD) + - Controlled feed hold with deceleration to ensure no skipped steps and loss of location. + - After feed hold, cycle accelerations are re-planned and may be resumed. + - System reset re-initializes grbl without resetting the Arduino. + - Updated dwell function. + - Restructured planner and stepper modules to become independent and ready for future features. + - Planned features: Jog mode, status reporting, backlash compensation, improved flow control, planner optimizations + - Reduce serial read buffer to 127 characters and increased write buffer to 31 characters (-1 ring buffer). + - Increased planner buffer size to 20 blocks. + - Misc bug fixes and removed deprecated acceleration enabled code. -*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. +*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. + +*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/serial.c b/serial.c index 7310e3f..8aa9419 100644 --- a/serial.c +++ b/serial.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 @@ -24,15 +25,13 @@ #include #include #include "serial.h" +#include "config.h" +#include "stepper.h" +#include "nuts_bolts.h" +#include "protocol.h" - -#ifdef __AVR_ATmega328P__ -#define RX_BUFFER_SIZE 256 -#else -#define RX_BUFFER_SIZE 64 -#endif - -#define TX_BUFFER_SIZE 16 +#define RX_BUFFER_SIZE 128 +#define TX_BUFFER_SIZE 32 uint8_t rx_buffer[RX_BUFFER_SIZE]; uint8_t rx_buffer_head = 0; @@ -44,25 +43,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< #include -#define GRBL_VERSION "0.7d" +#define GRBL_VERSION "0.8a" // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom diff --git a/spindle_control.c b/spindle_control.c index 0321c89..cd99c9a 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -20,11 +20,14 @@ #include "spindle_control.h" #include "settings.h" +#include "motion_control.h" #include "config.h" -#include "stepper.h" +#include "planner.h" #include +// TODO: Deprecated. Need to update for new version. + static int current_direction; void spindle_init() @@ -35,7 +38,7 @@ void spindle_init() void spindle_run(int direction, uint32_t rpm) { if (direction != current_direction) { - st_synchronize(); + plan_synchronize(); if(direction) { if(direction > 0) { SPINDLE_DIRECTION_PORT &= ~(1<> 3); // Enable steppers by resetting the stepper disable port STEPPERS_DISABLE_PORT &= ~(1<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 -} - // 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; +static uint8_t iterate_trapezoid_cycle_counter() +{ + st.trapezoid_tick_cycle_counter += st.cycles_per_step_event; + if(st.trapezoid_tick_cycle_counter > CYCLES_PER_ACCELERATION_TICK) { + st.trapezoid_tick_cycle_counter -= CYCLES_PER_ACCELERATION_TICK; return(true); } else { return(false); @@ -129,36 +137,48 @@ 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. -SIGNAL(TIMER1_COMPA_vect) +// NOTE: ISR_NOBLOCK allows SIG_OVERFLOW2 to trigger on-time regardless of time in this handler. + +// TODO: ISR_NOBLOCK is the same as the old SIGNAL with sei() method, but is optimizable by the compiler. On +// an oscilloscope there is a weird hitch in the step pulse during high load operation. Very infrequent, but +// when this does happen most of the time the pulse falling edge is randomly delayed by 20%-50% of the total +// intended pulse time, but sometimes it pulses less than 3usec. The former likely caused by the serial +// interrupt doing its thing, not that big of a deal, but the latter cause is unknown and worrisome. Need +// to track down what is causing this problem. Functionally, this shouldn't cause any noticeable issues +// as long as stepper drivers have a pulse minimum of 1usec or so (Pololu and any Allegro IC are ok). + +ISR(TIMER1_COMPA_vect,ISR_NOBLOCK) { - // 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 + busy = true; - if(busy){ return; } // The busy-flag is used to avoid reentering this interrupt // Set the direction pins a couple of nanoseconds before we step the steppers 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) >> 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 = step_pulse_time; // 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(); - counter_x = -(current_block->step_event_count >> 1); - counter_y = counter_x; - counter_z = counter_x; - step_events_completed = 0; + if (!st.feed_hold) { // During feed hold, do not update rate and trap counter. Keep decelerating. + st.trapezoid_adjusted_rate = current_block->initial_rate; + set_step_events_per_minute(st.trapezoid_adjusted_rate); // Initialize cycles_per_step_event + st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule. + } + st.min_safe_rate = current_block->rate_delta + (current_block->rate_delta >> 1); // 1.5 x rate_delta + st.counter_x = -(current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + st.event_count = current_block->step_event_count; + st.step_events_completed = 0; } else { + st.cycle_start = false; + st.feed_hold = false; st_go_idle(); } } @@ -166,139 +186,167 @@ SIGNAL(TIMER1_COMPA_vect) 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) { + st.counter_x += current_block->steps_x; + if (st.counter_x > 0) { out_bits |= (1<step_event_count; + st.counter_x -= st.event_count; + if (out_bits & (1<steps_y; - if (counter_y > 0) { + st.counter_y += current_block->steps_y; + if (st.counter_y > 0) { out_bits |= (1<step_event_count; + st.counter_y -= st.event_count; + if (out_bits & (1<steps_z; - if (counter_z > 0) { + st.counter_z += current_block->steps_z; + if (st.counter_z > 0) { out_bits |= (1<step_event_count; + st.counter_z -= st.event_count; + if (out_bits & (1<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() ) { - 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) { - // 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 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 - } - 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 (st.step_events_completed < current_block->step_event_count) { + if (st.feed_hold) { + // Check for and execute feed hold by enforcing a steady deceleration from the moment of + // execution. The rate of deceleration is limited by rate_delta and will never decelerate + // faster or slower than in normal operation. If the distance required for the feed hold + // deceleration spans more than one block, the initial rate of the following blocks are not + // updated and deceleration is continued according to their corresponding rate_delta. + // NOTE: The trapezoid tick cycle counter is not updated intentionally. This ensures that + // the deceleration is smooth regardless of where the feed hold is initiated and if the + // deceleration distance spans multiple blocks. + if ( iterate_trapezoid_cycle_counter() ) { + // If deceleration complete, set system flags and shutdown steppers. + if (st.trapezoid_adjusted_rate <= current_block->rate_delta) { + // Just go idle. Do not NULL current block. The bresenham algorithm variables must + // remain intact to ensure the stepper path is exactly the same. + st.cycle_start = false; + st_go_idle(); + sys_state |= BIT_REPLAN_CYCLE; // Flag main program that feed hold is complete. + } else { + st.trapezoid_adjusted_rate -= current_block->rate_delta; + set_step_events_per_minute(st.trapezoid_adjusted_rate); + } } + } else { - // 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); + // The trapezoid generator always checks step event location to ensure de/ac-celerations are + // executed and terminated at exactly the right time. This helps prevent over/under-shooting + // the target position and speed. + // NOTE: By increasing the ACCELERATION_TICKS_PER_SECOND in config.h, the resolution of the + // discrete velocity changes increase and accuracy can increase as well to a point. Numerical + // round-off errors can effect this, if set too high. This is important to note if a user has + // very high acceleration and/or feedrate requirements for their machine. + if (st.step_events_completed < current_block->accelerate_until) { + // Iterate cycle counter and check if speeds need to be increased. + if ( iterate_trapezoid_cycle_counter() ) { + st.trapezoid_adjusted_rate += current_block->rate_delta; + if (st.trapezoid_adjusted_rate >= current_block->nominal_rate) { + // Reached nominal rate a little early. Cruise at nominal rate until decelerate_after. + st.trapezoid_adjusted_rate = current_block->nominal_rate; + } + set_step_events_per_minute(st.trapezoid_adjusted_rate); + } + } else if (st.step_events_completed >= current_block->decelerate_after) { + // Reset trapezoid tick cycle counter to make sure that the deceleration is performed the + // same every time. Reset to CYCLES_PER_ACCELERATION_TICK/2 to follow the midpoint rule for + // an accurate approximation of the deceleration curve. + if (st.step_events_completed == current_block-> decelerate_after) { + st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; + } else { + // Iterate cycle counter and check if speeds need to be reduced. + if ( iterate_trapezoid_cycle_counter() ) { + // NOTE: We will only do a full speed reduction if the result is more than the minimum safe + // rate, initialized in trapezoid reset as 1.5 x rate_delta. Otherwise, reduce the speed by + // half increments until finished. The half increments are guaranteed not to exceed the + // CNC acceleration limits, because they will never be greater than rate_delta. This catches + // small errors that might leave steps hanging after the last trapezoid tick or a very slow + // step rate at the end of a full stop deceleration in certain situations. The half rate + // reductions should only be called once or twice per block and create a nice smooth + // end deceleration. + if (st.trapezoid_adjusted_rate > st.min_safe_rate) { + st.trapezoid_adjusted_rate -= current_block->rate_delta; + } else { + st.trapezoid_adjusted_rate >>= 1; // Bit shift divide by 2 + } + if (st.trapezoid_adjusted_rate < current_block->final_rate) { + // Reached final rate a little early. Cruise to end of block at final rate. + st.trapezoid_adjusted_rate = current_block->final_rate; + } + set_step_events_per_minute(st.trapezoid_adjusted_rate); + } + } + } else { + // No accelerations. Make sure we cruise exactly at the nominal rate. + if (st.trapezoid_adjusted_rate != current_block->nominal_rate) { + st.trapezoid_adjusted_rate = current_block->nominal_rate; + set_step_events_per_minute(st.trapezoid_adjusted_rate); + } } - } - + } } else { // If current block is finished, reset pointer current_block = NULL; plan_discard_current_block(); } - } - out_bits ^= settings.invert_mask; // Apply stepper invert mask - busy=false; + busy = false; } // 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); } +// Reset and clear stepper subsystem variables +void st_reset() +{ + memset(&st, 0, sizeof(st)); + set_step_events_per_minute(MINIMUM_STEPS_PER_MINUTE); + current_block = NULL; + busy = false; +} + // 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; - } else if (cycles <= 0x3fffffL) { - ceiling = cycles >> 6; + } else if (cycles <= 0x3fffffL) { + ceiling = cycles >> 6; prescaler = 2; // prescaler: 64 actual_cycles = ceiling * 64L; - } else if (cycles <= 0xffffffL) { - ceiling = (cycles >> 8); + } else if (cycles <= 0xffffffL) { + ceiling = (cycles >> 8); prescaler = 3; // prescaler: 256 actual_cycles = ceiling * 256L; - } else if (cycles <= 0x3ffffffL) { - ceiling = (cycles >> 10); + } 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; + } else { + // Okay, that was slower than we actually go. Just set the slowest speed + ceiling = 0xffff; prescaler = 4; actual_cycles = 0xffff * 1024; - } - // Set prescaler + } + // Set prescaler TCCR1B = (TCCR1B & ~(0x07<step_event_count - st.step_events_completed); + // Update initial rate and timers after feed hold. + st.trapezoid_adjusted_rate = 0; // Resumes from rest + set_step_events_per_minute(st.trapezoid_adjusted_rate); + st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule. + st.step_events_completed = 0; + st.feed_hold = false; // Release feed hold. Cycle is ready to re-start. +} diff --git a/stepper.h b/stepper.h index 73d3e7c..1bf8e6b 100644 --- a/stepper.h +++ b/stepper.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 @@ -29,16 +30,22 @@ #define DIRECTION_MASK ((1< Date: Fri, 6 Jan 2012 10:10:41 -0700 Subject: [PATCH 28/32] Position reporting, refactored system variables, serial print fixes, updated streaming scripts. - Added machine position reporting to status queries. This will be further developed with part positioning/offsets and maintaining location upon reset. - System variables refactored into a global struct for better readability. - Removed old obsolete Ruby streaming scripts. These were no longer compatible. Updated Python streaming scripts. - Fixed printFloat() and other printing functions. - Decreased planner buffer back to 18 blocks and increased TX serial buffer to 64 bytes. Need the memory space for future developments. - Begun adding run-time modes to grbl, where block delete toggle, mm/in reporting modes, jog modes, etc can be set during runtime. Will be fleshed out and placed into EEPROM when everything is added. --- config.h | 55 +++++---- gcode.c | 2 - main.c | 52 ++++----- motion_control.c | 24 ++-- nuts_bolts.h | 49 +++++--- planner.c | 4 +- planner.h | 2 +- print.c | 112 ++++++++++++------- print.h | 3 +- protocol.c | 87 ++++++++++---- script/{ => Obsolete}/console | 0 script/{ => Obsolete}/proxy | 0 script/{ => Obsolete}/stream | 0 script/{ => Obsolete}/stream.rb | 0 script/{ => Obsolete}/trapezoid_simulator.rb | 0 script/simple_stream.py | 9 ++ script/stream.py | 80 +++++++++++++ serial.c | 14 +-- settings.c | 16 ++- settings.h | 1 - stepper.c | 80 +++++++------ 21 files changed, 396 insertions(+), 194 deletions(-) rename script/{ => Obsolete}/console (100%) rename script/{ => Obsolete}/proxy (100%) rename script/{ => Obsolete}/stream (100%) rename script/{ => Obsolete}/stream.rb (100%) rename script/{ => Obsolete}/trapezoid_simulator.rb (100%) create mode 100755 script/stream.py diff --git a/config.h b/config.h index 56f4b9d..ac68acb 100644 --- a/config.h +++ b/config.h @@ -27,37 +27,38 @@ #define BAUD_RATE 9600 // Define pin-assignments -#define STEPPERS_DISABLE_DDR DDRB -#define STEPPERS_DISABLE_PORT PORTB -#define STEPPERS_DISABLE_BIT 0 - #define STEPPING_DDR DDRD #define STEPPING_PORT PORTD -#define X_STEP_BIT 2 -#define Y_STEP_BIT 3 -#define Z_STEP_BIT 4 -#define X_DIRECTION_BIT 5 -#define Y_DIRECTION_BIT 6 -#define Z_DIRECTION_BIT 7 +#define X_STEP_BIT 2 // Uno Digital Pin 2 +#define Y_STEP_BIT 3 // Uno Digital Pin 3 +#define Z_STEP_BIT 4 // Uno Digital Pin 4 +#define X_DIRECTION_BIT 5 // Uno Digital Pin 5 +#define Y_DIRECTION_BIT 6 // Uno Digital Pin 6 +#define Z_DIRECTION_BIT 7 // Uno Digital Pin 7 -#define LIMIT_DDR DDRB +#define STEPPERS_DISABLE_DDR DDRB +#define STEPPERS_DISABLE_PORT PORTB +#define STEPPERS_DISABLE_BIT 0 // Uno Digital Pin 8 + +#define LIMIT_DDR DDRB #define LIMIT_PIN PINB -#define X_LIMIT_BIT 1 -#define Y_LIMIT_BIT 2 -#define Z_LIMIT_BIT 3 +#define X_LIMIT_BIT 1 // Uno Digital Pin 9 +#define Y_LIMIT_BIT 2 // Uno Digital Pin 10 +#define Z_LIMIT_BIT 3 // Uno Digital Pin 11 #define SPINDLE_ENABLE_DDR DDRB #define SPINDLE_ENABLE_PORT PORTB -#define SPINDLE_ENABLE_BIT 4 +#define SPINDLE_ENABLE_BIT 4 // Uno Digital Pin 12 #define SPINDLE_DIRECTION_DDR DDRB #define SPINDLE_DIRECTION_PORT PORTB -#define SPINDLE_DIRECTION_BIT 5 +#define SPINDLE_DIRECTION_BIT 5 // Uno Digital Pin 13 // Define runtime command special characters. These characters are 'picked-off' directly from the // serial read data stream and are not passed to the grbl line execution parser. Select characters // that do not and must not exist in the streamed g-code program. ASCII control characters may be -// used, if they are available per user setup. +// used, if they are available per user setup. Also, extended ASCII codes (>127), which are never in +// g-code programs, maybe selected for interface programs. // TODO: Solidify these default characters. Temporary for now. #define CMD_STATUS_REPORT '?' #define CMD_FEED_HOLD '!' @@ -70,8 +71,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. -// NOTE: If defined 0, the delay will not be compiled. -#define STEPPER_IDLE_LOCK_TIME 25 // (milliseconds) - Integer >= 0 +// NOTE: If commented out, 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. @@ -109,4 +110,20 @@ // time step. Also, keep in mind that the Arduino delay timer is not very accurate for long delays. #define DWELL_TIME_STEP 50 // Integer (milliseconds) + +// ----------------------------------------------- + +// TODO: The following options are set as compile-time options for now, until the next EEPROM +// settings version has solidified. +#define CYCLE_AUTO_START 1 // Cycle auto-start boolean flag for the planner. +#define BLOCK_DELETE_ENABLE 0 // Block delete enable/disable flag during g-code parsing +#define REPORT_INCH_MODE 0 // Status reporting unit mode (1 = inch, 0 = mm) +#if REPORT_INCH_MODE + #define DECIMAL_PLACES 3 + #define DECIMAL_MULTIPLIER 1000 // 10^DECIMAL_PLACES +#else + #define DECIMAL_PLACES 2 // mm-mode + #define DECIMAL_MULTIPLIER 100 +#endif + #endif diff --git a/gcode.c b/gcode.c index 1936015..b9ca1f4 100644 --- a/gcode.c +++ b/gcode.c @@ -32,8 +32,6 @@ #include "errno.h" #include "protocol.h" -#define MM_PER_INCH (25.4) - #define NEXT_ACTION_DEFAULT 0 #define NEXT_ACTION_DWELL 1 #define NEXT_ACTION_GO_HOME 2 diff --git a/main.c b/main.c index a1b0b1d..17732ac 100644 --- a/main.c +++ b/main.c @@ -4,8 +4,7 @@ Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2011 Sungeun K. Jeon - Copyright (c) 2011 Jens Geisler - + Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or @@ -20,11 +19,8 @@ along with Grbl. If not, see . */ -#include -#include #include #include -#include #include "config.h" #include "planner.h" #include "nuts_bolts.h" @@ -34,15 +30,11 @@ #include "gcode.h" #include "protocol.h" #include "limits.h" - #include "settings.h" #include "serial.h" -#include "print.h" - -// Declare system global variables -uint8_t sys_abort; // Global system abort flag -volatile uint8_t sys_state; // Global system state variable +// Declare system global variable structure +system_t sys; int main(void) { @@ -50,17 +42,18 @@ int main(void) sei(); // Enable interrupts serial_init(BAUD_RATE); // Setup serial baud rate and interrupts st_init(); // Setup stepper pins and interrupt timers - sys_abort = true; // Set abort to complete initialization + + sys.abort = true; // Set abort to complete initialization while(1) { - // Upon a system abort, the main program will return to this loop. Once here, it is safe to - // re-initialize the system. Upon startup, the system will automatically reset to finish the - // initialization process. - if (sys_abort) { - // Execute system reset - sys_state = 0; // Reset system state - sys_abort = false; // Release system abort + // Execute system reset upon a system abort, where the main program will return to this loop. + // Once here, it is safe to re-initialize the system. At startup, the system will automatically + // reset to finish the initialization process. + if (sys.abort) { + + // Clear all system variables + memset(&sys, 0, sizeof(sys)); // Reset system. serial_reset_read_buffer(); // Clear serial read buffer @@ -70,19 +63,14 @@ int main(void) gc_init(); // Set g-code parser to default state spindle_init(); limits_init(); - - // TODO: For now, the stepper subsystem tracks the absolute stepper position from the point - // of power up or hard reset. This reset is a soft reset, where the information of the current - // position is not lost after a system abort. This is not guaranteed to be correct, since - // during an abort, the steppers can lose steps in the immediate stop. However, if a feed - // hold is performed before a system abort, this position should be correct. In the next few - // updates, this soft reset feature will be fleshed out along with the status reporting and - // jogging features. - st_reset(); // Clear stepper subsystem variables. Machine position variable is not reset. - - // Print grbl initialization message - printPgmString(PSTR("\r\nGrbl " GRBL_VERSION)); - printPgmString(PSTR("\r\n'$' to dump current settings\r\n")); + st_reset(); // Clear stepper subsystem variables. + + // Set system runtime defaults + // TODO: Eventual move to EEPROM from config.h when all of the new settings are worked out. + // Mainly to avoid having to maintain several different versions. + #ifdef CYCLE_AUTO_START + sys.auto_start = true; + #endif } protocol_execute_runtime(); diff --git a/motion_control.c b/motion_control.c index a5985c4..12c929e 100644 --- a/motion_control.c +++ b/motion_control.c @@ -33,8 +33,6 @@ #include "limits.h" #include "protocol.h" -#include "print.h" - // 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. @@ -56,17 +54,19 @@ void mc_line(double x, double y, double z, double feed_rate, uint8_t invert_feed // Remain in this loop until there is room in the buffer. do { protocol_execute_runtime(); // Check for any run-time commands - if (sys_abort) { return; } // Bail, if system abort. + if (sys.abort) { return; } // Bail, if system abort. } while ( plan_check_full_buffer() ); plan_buffer_line(x, y, z, feed_rate, invert_feed_rate); - // Auto-cycle start. - // TODO: Determine a more efficient and robust way of implementing the auto-starting the cycle. - // For example, only auto-starting when the buffer is full; if there was only one g-code command - // sent during manual operation; or if there is buffer starvation, making sure it minimizes any - // dwelling/motion hiccups. Additionally, these situations must not auto-start during a feed hold. - // Only the cycle start runtime command should be able to restart the cycle after a feed hold. - st_cycle_start(); + // Auto-cycle start immediately after planner finishes. Enabled/disabled by grbl settings. During + // a feed hold, auto-start is disabled momentarily until the cycle is resumed by the cycle-start + // runtime command. + // NOTE: This is allows the user to decide to exclusively use the cycle start runtime command to + // begin motion or let grbl auto-start it for them. This is useful when: manually cycle-starting + // when the buffer is completely full and primed; auto-starting, if there was only one g-code + // command sent during manual operation; or if a system is prone to buffer starvation, auto-start + // helps make sure it minimizes any dwelling/motion hiccups and keeps the cycle going. + if (sys.auto_start) { st_cycle_start(); } } @@ -167,7 +167,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui mc_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], feed_rate, invert_feed_rate); // Bail mid-circle on system abort. Runtime command check already performed by mc_line. - if (sys_abort) { return; } + if (sys.abort) { return; } } // Ensure last segment arrives at target location. mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); @@ -183,7 +183,7 @@ void mc_dwell(double seconds) while (i > 0) { // NOTE: Check and execute runtime commands during dwell every <= DWELL_TIME_STEP milliseconds. protocol_execute_runtime(); - if (sys_abort) { return; } + if (sys.abort) { return; } _delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment i--; } diff --git a/nuts_bolts.h b/nuts_bolts.h index 665de25..edbb96c 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -32,27 +32,50 @@ #define Y_AXIS 1 #define Z_AXIS 2 +#define MM_PER_INCH (25.4) + +// Useful macros #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)) -// Define system state bit map. Used internally by runtime protocol as runtime command flags. -// NOTE: The system state is an unsigned 8-bit volatile variable and has a 8 flag limit. The default -// flags are always false, so the runtime protocol only needs to check for a non-zero state value to +// Bit field and masking macros +#define bit(n) (1 << n) +#define bit_true(x,mask) (x |= mask) +#define bit_false(x,mask) (x &= ~mask) +#define bit_toggle(x,mask) (x ^= mask) +#define bit_istrue(x,mask) ((x & mask) != 0) +#define bit_isfalse(x,mask) ((x & mask) == 0) + +// Define system executor bit map. Used internally by runtime protocol as runtime command flags, +// which notifies the main program to execute the specified runtime command asynchronously. +// NOTE: The system executor uses an unsigned 8-bit volatile variable (8 flag limit.) The default +// flags are always false, so the runtime protocol only needs to check for a non-zero value to // know when there is a runtime command to execute. -#define BIT_STATUS_REPORT 1 // bit 00000001 -#define BIT_CYCLE_START 2 // bit 00000010 -#define BIT_FEED_HOLD 4 // bit 00000100 -#define BIT_RESET 8 // bit 00001000 -#define BIT_REPLAN_CYCLE 16 // bit 00010000 -// #define 32 // bit 00100000 -// #define 64 // bit 01000000 -// #define 128 // bit 10000000 +#define EXEC_STATUS_REPORT bit(0) // bitmask 00000001 +#define EXEC_CYCLE_START bit(1) // bitmask 00000010 +#define EXEC_CYCLE_STOP bit(2) // bitmask 00000100 +#define EXEC_FEED_HOLD bit(3) // bitmask 00001000 +#define EXEC_RESET bit(4) // bitmask 00010000 +// #define bit(5) // bitmask 00100000 +// #define bit(6) // bitmask 01000000 +// #define bit(7) // bitmask 10000000 // Define global system variables -extern uint8_t sys_abort; // Global system abort flag -extern volatile uint8_t sys_state; // Global system state variable +typedef struct { + uint8_t abort; // System abort flag. Forces exit back to main loop for reset. + uint8_t feed_hold; // Feed hold flag. Held true during feed hold. Released when ready to resume. + uint8_t auto_start; // Planner auto-start flag. Toggled off during feed hold. Defaulted by settings. + + int32_t position[3]; // Real-time machine position vector in steps. This may need to be a volatile + // variable, if problems arise. Subject to change. Need to add coordinate offset + // functionality to correctly track part zero and machine zero. + + volatile uint8_t cycle_start; // Cycle start flag. Set by stepper subsystem or main program. + volatile uint8_t execute; // Global system runtime executor bitflag variable. See EXEC bitmasks. +} system_t; +extern system_t sys; // 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 aeba0cd..772e168 100644 --- a/planner.c +++ b/planner.c @@ -34,7 +34,7 @@ #include "protocol.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 18 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 @@ -336,7 +336,7 @@ void plan_synchronize() { while(plan_get_current_block()) { protocol_execute_runtime(); // Check and execute run-time commands - if (sys_abort) { return; } // Check for system abort + if (sys.abort) { return; } // Check for system abort } } diff --git a/planner.h b/planner.h index fc692b5..451fdf9 100644 --- a/planner.h +++ b/planner.h @@ -35,7 +35,7 @@ typedef struct { // 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 + double entry_speed; // Entry speed at previous-current block 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 diff --git a/print.c b/print.c index 05524a3..f509e88 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 @@ -24,72 +25,107 @@ #include #include +#include "config.h" #include "serial.h" -#ifndef DECIMAL_PLACES -#define DECIMAL_PLACES 3 -#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) +// 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 print_uint8_base2(uint8_t n) { - unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. - unsigned long i = 0; + unsigned char buf[8]; + uint8_t i = 0; - if (n == 0) { - serial_write('0'); - return; - } - - while (n > 0) { - buf[i++] = n % base; - n /= base; + for (; i < 8; i++) { + buf[i] = n & 1; + n >>= 1; } for (; i > 0; i--) - serial_write(buf[i - 1] < 10 ? - '0' + buf[i - 1] : - 'A' + buf[i - 1] - 10); + serial_write('0' + buf[i - 1]); +} + +static void print_uint32_base10(unsigned long n) +{ + unsigned char buf[32]; + uint8_t i = 0; + + if (n == 0) { + serial_write('0'); + return; + } + + while (n > 0) { + buf[i++] = n % 10; + n /= 10; + } + + for (; i > 0; i--) + serial_write('0' + buf[i - 1]); } void printInteger(long n) { - if (n < 0) { - serial_write('-'); - n = -n; - } - - printIntegerInBase(n, 10); + if (n < 0) { + serial_write('-'); + n = -n; + } + print_uint32_base10(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; + print_uint32_base10(integer_part); + 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/print.h b/print.h index b2581ba..ff4ea99 100644 --- a/print.h +++ b/print.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 @@ -30,7 +31,7 @@ void printPgmString(const char *s); void printInteger(long n); -void printIntegerInBase(unsigned long n, unsigned long base); +void print_uint8_base2(uint8_t n); void printFloat(double n); diff --git a/protocol.c b/protocol.c index da31da8..7b812f3 100644 --- a/protocol.c +++ b/protocol.c @@ -69,15 +69,39 @@ void protocol_status_report() // may be distance to go on block, processed block id, and feed rate. A secondary, non-critical // status report may include g-code state, i.e. inch mode, plane mode, absolute mode, etc. // The report generated must be as short as possible, yet still provide the user easily readable - // information, i.e. 'x0.23 y120.4 z2.4'. This is necessary as it minimizes the computational + // information, i.e. 'x0.23,y120.4,z2.4'. This is necessary as it minimizes the computational // overhead and allows grbl to keep running smoothly, especially with g-code programs with fast, - // short line segments and interface setups that require real-time status reports (10-20Hz). - printString("Query Received.\r\n"); // Notify that it's working. + // short line segments and interface setups that require real-time status reports (5-20Hz). + // Additionally, during an abort, the steppers are immediately stopped regardless of what they + // are doing. If they are moving, the abort stop can cause grbl to lose steps. However, if a feed + // hold is performed before a system abort, the steppers will steadily decelerate at the max + // acceleration rate, hence the stopped machine position will be maintained and correct. + + + // Bare-bones status report. Provides real-time machine position relative to the initialization + // or system reset location (0,0,0), not a home position. This section is under construction and + // the following are needed: coordinate offsets/updating of machine position relative to home, work + // coordinate position?, user setting of output units (mm|inch), compressed (non-human readable) + // data for interfaces?, save last known position in EEPROM? + #if REPORT_INCH_MODE + printString("x"); printFloat(sys.position[X_AXIS]/(settings.steps_per_mm[X_AXIS]*MM_PER_INCH)); + printString(",y"); printFloat(sys.position[Y_AXIS]/(settings.steps_per_mm[Y_AXIS]*MM_PER_INCH)); + printString(",z"); printFloat(sys.position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS]*MM_PER_INCH)); + #else + printString("x"); printFloat(sys.position[X_AXIS]/(settings.steps_per_mm[X_AXIS])); + printString(",y"); printFloat(sys.position[Y_AXIS]/(settings.steps_per_mm[Y_AXIS])); + printString(",z"); printFloat(sys.position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS])); + #endif + printString("\r\n"); } void protocol_init() { + // Print grbl initialization message + printPgmString(PSTR("\r\nGrbl " GRBL_VERSION)); + printPgmString(PSTR("\r\n'$' to dump current settings\r\n")); + char_counter = 0; // Reset line input iscomment = false; } @@ -87,40 +111,46 @@ void protocol_init() // program, primarily where there may be a while loop waiting for a buffer to clear space or any // point where the execution time from the last check point may be more than a fraction of a second. // This is a way to execute runtime commands asynchronously (aka multitasking) with grbl's g-code -// parsing and planning functions. -// NOTE: The sys_state variable flags are set by the serial read subprogram, except where noted. +// parsing and planning functions. This function also serves as an interface for the interrupts to +// set the system runtime flags, where only the main program to handles them, removing the need to +// define more computationally-expensive volatile variables. +// NOTE: The sys.execute variable flags are set by the serial read subprogram, except where noted. void protocol_execute_runtime() { - if (sys_state) { // Enter only if any bit flag is enabled + if (sys.execute) { // Enter only if any bit flag is true + uint8_t rt_exec = sys.execute; // Avoid calling volatile multiple times // System abort. Steppers have already been force stopped. - if (sys_state & BIT_RESET) { - sys_abort = true; + if (rt_exec & EXEC_RESET) { + sys.abort = true; return; // Nothing else to do but exit. } // Execute and serial print status - if (sys_state & BIT_STATUS_REPORT) { + if (rt_exec & EXEC_STATUS_REPORT) { + bit_false(sys.execute,EXEC_STATUS_REPORT); protocol_status_report(); - sys_state ^= BIT_STATUS_REPORT; // Toggle off } // Initiate stepper feed hold - if (sys_state & BIT_FEED_HOLD) { - st_feed_hold(); - sys_state ^= BIT_FEED_HOLD; // Toggle off + if (rt_exec & EXEC_FEED_HOLD) { + st_feed_hold(); // Initiate feed hold. + bit_false(sys.execute,EXEC_FEED_HOLD); } - // Re-plans the buffer after a feed hold completes - // NOTE: BIT_REPLAN_CYCLE is set by the stepper subsystem when the feed hold is complete. - if (sys_state & BIT_REPLAN_CYCLE) { + // Reinitializes the stepper module running flags and re-plans the buffer after a feed hold. + // NOTE: EXEC_CYCLE_STOP is set by the stepper subsystem when a cycle or feed hold completes. + if (rt_exec & EXEC_CYCLE_STOP) { st_cycle_reinitialize(); - sys_state ^= BIT_REPLAN_CYCLE; // Toggle off + bit_false(sys.execute,EXEC_CYCLE_STOP); } - if (sys_state & BIT_CYCLE_START) { + if (rt_exec & EXEC_CYCLE_START) { st_cycle_start(); // Issue cycle start command to stepper subsystem - sys_state ^= BIT_CYCLE_START; // Toggle off + #ifdef CYCLE_AUTO_START + sys.auto_start = true; // Re-enable auto start after feed hold. + #endif + bit_false(sys.execute,EXEC_CYCLE_START); } } } @@ -130,6 +160,15 @@ void protocol_execute_runtime() uint8_t protocol_execute_line(char *line) { if(line[0] == '$') { + + // TODO: Re-write this '$' as a way to change runtime settings without having to reset, i.e. + // auto-starting, status query output formatting and type, jog mode (axes, direction, and + // nominal feedrate), toggle block delete, etc. This differs from the EEPROM settings, as they + // are considered defaults and loaded upon startup/reset. + // This use is envisioned where '$' itself dumps settings and help. Defined characters + // proceeding the '$' may be used to setup modes, such as jog mode with a '$J=X100' for X-axis + // motion with a nominal feedrate of 100mm/min. Writing EEPROM settings will likely stay the + // same or similar. Should be worked out in upcoming releases. return(settings_execute_line(line)); // Delegate lines starting with '$' to the settings module // } else if { @@ -165,7 +204,7 @@ void protocol_process() // NOTE: If there is no line, this function should quickly return to the main program when // the buffer empties of non-executable data. protocol_execute_runtime(); - if (sys_abort) { return; } // Bail to main program upon system abort + if (sys.abort) { return; } // Bail to main program upon system abort if (char_counter > 0) {// Line is complete. Then execute! line[char_counter] = 0; // Terminate string @@ -176,6 +215,7 @@ void protocol_process() } char_counter = 0; // Reset line buffer index iscomment = false; // Reset comment flag + } else { if (iscomment) { // Throw away all comment characters @@ -187,9 +227,10 @@ void protocol_process() 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; + // Disable block delete and throw away characters. Will ignore until EOL. + #if BLOCK_DELETE_ENABLE + iscomment = true; + #endif } else if (c == '(') { // Enable comments flag and ignore all characters until ')' or EOL. iscomment = true; diff --git a/script/console b/script/Obsolete/console similarity index 100% rename from script/console rename to script/Obsolete/console diff --git a/script/proxy b/script/Obsolete/proxy similarity index 100% rename from script/proxy rename to script/Obsolete/proxy diff --git a/script/stream b/script/Obsolete/stream similarity index 100% rename from script/stream rename to script/Obsolete/stream diff --git a/script/stream.rb b/script/Obsolete/stream.rb similarity index 100% rename from script/stream.rb rename to script/Obsolete/stream.rb diff --git a/script/trapezoid_simulator.rb b/script/Obsolete/trapezoid_simulator.rb similarity index 100% rename from script/trapezoid_simulator.rb rename to script/Obsolete/trapezoid_simulator.rb diff --git a/script/simple_stream.py b/script/simple_stream.py index 8bd4b3b..5119db4 100755 --- a/script/simple_stream.py +++ b/script/simple_stream.py @@ -1,6 +1,15 @@ #!/usr/bin/env python """\ Simple g-code streaming script for grbl + +Provided as an illustration of the basic communication interface +for grbl. When grbl has finished parsing the g-code block, it will +return an 'ok' or 'error' response. When the planner buffer is full, +grbl will not send a response until the planner buffer clears space. + +G02/03 arcs are special exceptions, where they inject short line +segments directly into the planner. So there may not be a response +from grbl for the duration of the arc. """ import serial diff --git a/script/stream.py b/script/stream.py new file mode 100755 index 0000000..32971ff --- /dev/null +++ b/script/stream.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python +"""\ +Stream g-code to grbl controller + +This script differs from the simple_stream.py script by +tracking the number of characters in grbl's serial read +buffer. This allows grbl to fetch the next line directly +from the serial buffer and does not have to wait for a +response from the computer. This effectively adds another +buffer layer to prevent buffer starvation. + +TODO: - Add runtime command capabilities + +Version: SKJ.20120104 +""" + +import serial +import re +import time +import sys +import argparse + +RX_BUFFER_SIZE = 128 + +# Define command line argument interface +parser = argparse.ArgumentParser(description='Stream g-code file to grbl. (pySerial library required)') +parser.add_argument('gcode', type=argparse.FileType('r'), + help='g-code filename to be streamed') +parser.add_argument('device', + help='serial device path') +parser.add_argument('-q','--quiet',action='store_true', default=False, + help='suppress output text') +args = parser.parse_args() + +# Initialize +s = serial.Serial(args.device_file,9600) +f = args.gcode_file +verbose = True +if args.quiet : verbose = False + +# Wake up grbl +print "Initializing grbl..." +s.write("\r\n\r\n") + +# Wait for grbl to initialize and flush startup text in serial input +time.sleep(2) +s.flushInput() + +# Stream g-code to grbl +print "Streaming ", args.gcode_file.name, " to ", args.device_file +l_count = 0 +g_count = 0 +c_line = [] +for line in f: + l_count += 1 # Iterate line counter +# l_block = re.sub('\s|\(.*?\)','',line).upper() # Strip comments/spaces/new line and capitalize + l_block = line.strip() + c_line.append(len(l_block)) # Track number of characters in grbl serial read buffer + grbl_out = '' + while sum(c_line) >= RX_BUFFER_SIZE-1 | s.inWaiting() : + out_temp = s.readline().strip() # Wait for grbl response + if out_temp not in ['ok','error'] : + print " Debug: ",out_temp # Debug response + else : + grbl_out += out_temp; + g_count += 1 # Iterate g-code counter + grbl_out += str(g_count); # Add line finished indicator + del c_line[0] + if verbose: print "SND: " + str(l_count) + " : " + l_block, + s.write(l_block + '\n') # Send block to grbl + if verbose : print "BUF:",str(sum(c_line)),"REC:",grbl_out + +# Wait for user input after streaming is completed +print "G-code streaming finished!\n" +print "WARNING: Wait until grbl completes buffered g-code blocks before exiting." +raw_input(" Press to exit and disable grbl.") + +# Close file and serial port +f.close() +s.close() \ No newline at end of file diff --git a/serial.c b/serial.c index 8aa9419..87aab50 100644 --- a/serial.c +++ b/serial.c @@ -31,7 +31,7 @@ #include "protocol.h" #define RX_BUFFER_SIZE 128 -#define TX_BUFFER_SIZE 32 +#define TX_BUFFER_SIZE 64 uint8_t rx_buffer[RX_BUFFER_SIZE]; uint8_t rx_buffer_head = 0; @@ -72,7 +72,7 @@ void serial_write(uint8_t data) { // Wait until there is space in the buffer while (next_head == tx_buffer_tail) { protocol_execute_runtime(); // Check for any run-time commands - if (sys_abort) { return; } // Bail, if system abort. + if (sys.abort) { return; } // Bail, if system abort. } // Store data and advance head @@ -124,15 +124,15 @@ ISR(USART_RX_vect) // Pick off runtime command characters directly from the serial stream. These characters are // not passed into the buffer, but these set system state flag bits for runtime execution. switch (data) { - case CMD_STATUS_REPORT: sys_state |= BIT_STATUS_REPORT; break; // Set as true - case CMD_CYCLE_START: sys_state |= BIT_CYCLE_START; break; // Set as true - case CMD_FEED_HOLD: sys_state |= BIT_FEED_HOLD; break; // Set as true + case CMD_STATUS_REPORT: sys.execute |= EXEC_STATUS_REPORT; break; // Set as true + case CMD_CYCLE_START: sys.execute |= EXEC_CYCLE_START; break; // Set as true + case CMD_FEED_HOLD: sys.execute |= EXEC_FEED_HOLD; break; // Set as true case CMD_RESET: // Immediately force stepper subsystem idle at an interrupt level. - if (!(sys_state & BIT_RESET)) { // Force stop only first time. + if (!(sys.execute & EXEC_RESET)) { // Force stop only first time. st_go_idle(); } - sys_state |= BIT_RESET; // Set as true + sys.execute |= EXEC_RESET; // Set as true break; default : // Write character to buffer rx_buffer[rx_buffer_head] = data; diff --git a/settings.c b/settings.c index 9d2c173..5cc3b16 100644 --- a/settings.c +++ b/settings.c @@ -54,6 +54,7 @@ typedef struct { #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 diff --git a/stepper.c b/stepper.c index f48cf77..3278c89 100644 --- a/stepper.c +++ b/stepper.c @@ -4,7 +4,6 @@ Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2011 Sungeun K. Jeon - Copyright (c) 2011 Jens Geisler Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -34,8 +33,6 @@ #include "planner.h" #include "limits.h" -#include "print.h" - // Some useful constants #define STEP_MASK ((1<initial_rate; set_step_events_per_minute(st.trapezoid_adjusted_rate); // Initialize cycles_per_step_event st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule. @@ -177,9 +172,9 @@ ISR(TIMER1_COMPA_vect,ISR_NOBLOCK) st.event_count = current_block->step_event_count; st.step_events_completed = 0; } else { - st.cycle_start = false; - st.feed_hold = false; st_go_idle(); + sys.cycle_start = false; + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end } } @@ -190,29 +185,29 @@ ISR(TIMER1_COMPA_vect,ISR_NOBLOCK) if (st.counter_x > 0) { out_bits |= (1<steps_y; if (st.counter_y > 0) { out_bits |= (1<steps_z; if (st.counter_z > 0) { out_bits |= (1<step_event_count) { - if (st.feed_hold) { + if (sys.feed_hold) { // Check for and execute feed hold by enforcing a steady deceleration from the moment of // execution. The rate of deceleration is limited by rate_delta and will never decelerate // faster or slower than in normal operation. If the distance required for the feed hold @@ -225,10 +220,11 @@ ISR(TIMER1_COMPA_vect,ISR_NOBLOCK) // If deceleration complete, set system flags and shutdown steppers. if (st.trapezoid_adjusted_rate <= current_block->rate_delta) { // Just go idle. Do not NULL current block. The bresenham algorithm variables must - // remain intact to ensure the stepper path is exactly the same. - st.cycle_start = false; + // remain intact to ensure the stepper path is exactly the same. Feed hold is still + // active and is released after the buffer has been reinitialized. st_go_idle(); - sys_state |= BIT_REPLAN_CYCLE; // Flag main program that feed hold is complete. + sys.cycle_start = false; + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program that feed hold is complete. } else { st.trapezoid_adjusted_rate -= current_block->rate_delta; set_step_events_per_minute(st.trapezoid_adjusted_rate); @@ -339,9 +335,6 @@ void st_init() TCCR2A = 0; // Normal operation TCCR2B = (1<step_event_count - st.step_events_completed); - // Update initial rate and timers after feed hold. - st.trapezoid_adjusted_rate = 0; // Resumes from rest - set_step_events_per_minute(st.trapezoid_adjusted_rate); - st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule. - st.step_events_completed = 0; - st.feed_hold = false; // Release feed hold. Cycle is ready to re-start. + if (current_block != NULL) { + // Replan buffer from the feed hold stop location. + plan_cycle_reinitialize(current_block->step_event_count - st.step_events_completed); + // Update initial rate and timers after feed hold. + st.trapezoid_adjusted_rate = 0; // Resumes from rest + set_step_events_per_minute(st.trapezoid_adjusted_rate); + st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule. + st.step_events_completed = 0; + } + sys.feed_hold = false; // Release feed hold. Cycle is ready to re-start. } + + From f40078110eda6b8939fdda8d38d2941deec7dd71 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Mon, 9 Jan 2012 18:51:53 -0700 Subject: [PATCH 29/32] Updated line in streaming script. --- script/stream.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/script/stream.py b/script/stream.py index 32971ff..b614549 100755 --- a/script/stream.py +++ b/script/stream.py @@ -59,7 +59,7 @@ for line in f: grbl_out = '' while sum(c_line) >= RX_BUFFER_SIZE-1 | s.inWaiting() : out_temp = s.readline().strip() # Wait for grbl response - if out_temp not in ['ok','error'] : + if out_temp.find('ok') < 0 and out_temp.find('error') < 0 : print " Debug: ",out_temp # Debug response else : grbl_out += out_temp; From 6f27e2cdb1625657ccb8a96c3fbc9bd637f29170 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Mon, 9 Jan 2012 21:41:02 -0700 Subject: [PATCH 30/32] Corrected a minor streaming script character counting bug. --- script/stream.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/script/stream.py b/script/stream.py index b614549..0c91a90 100755 --- a/script/stream.py +++ b/script/stream.py @@ -55,7 +55,7 @@ for line in f: l_count += 1 # Iterate line counter # l_block = re.sub('\s|\(.*?\)','',line).upper() # Strip comments/spaces/new line and capitalize l_block = line.strip() - c_line.append(len(l_block)) # Track number of characters in grbl serial read buffer + c_line.append(len(l_block)+1) # Track number of characters in grbl serial read buffer grbl_out = '' while sum(c_line) >= RX_BUFFER_SIZE-1 | s.inWaiting() : out_temp = s.readline().strip() # Wait for grbl response From 89a3b37e020aa4e657973baadb0ff0936aa25656 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Tue, 10 Jan 2012 08:34:48 -0700 Subject: [PATCH 31/32] Extended position reporting with both home and work coordinates. Home position now retained after reset. Other minor changes/fixes. - Grbl now tracks both home and work (G92) coordinate systems and does live updates when G92 is called. - Rudimentary home and work position status reporting. Works but still under major construction. - Updated the main streaming script. Has a disabled periodic timer for querying status reports, disabled only because the Python timer doesn't consistently restart after the script exits. Add here only for user testing. - Fixed a bug to prevent an endless serial_write loop during status reports. - Refactored the planner variables to make it more clear what they are and make it easier for clear them. --- limits.c | 3 +- main.c | 16 +++++++-- motion_control.c | 5 ++- nuts_bolts.h | 7 ++-- planner.c | 91 +++++++++++++++++++++++++----------------------- protocol.c | 40 ++++++++++++--------- script/stream.py | 10 +++++- serial.c | 3 +- stepper.c | 2 +- 9 files changed, 106 insertions(+), 71 deletions(-) diff --git a/limits.c b/limits.c index ec0b432..a5c4a3e 100644 --- a/limits.c +++ b/limits.c @@ -27,7 +27,8 @@ #include "motion_control.h" #include "planner.h" -// TODO: Deprecated. Need to update for new version. +// TODO: Deprecated. Need to update for new version. Sys.position now tracks position relative +// to the home position. Limits should update this vector directly. void limits_init() { LIMIT_DDR &= ~(LIMIT_MASK); diff --git a/main.c b/main.c index 17732ac..0ddd410 100644 --- a/main.c +++ b/main.c @@ -43,6 +43,7 @@ int main(void) serial_init(BAUD_RATE); // Setup serial baud rate and interrupts st_init(); // Setup stepper pins and interrupt timers + memset(&sys, 0, sizeof(sys)); // Clear all system variables sys.abort = true; // Set abort to complete initialization while(1) { @@ -51,10 +52,21 @@ int main(void) // Once here, it is safe to re-initialize the system. At startup, the system will automatically // reset to finish the initialization process. if (sys.abort) { - + + // Retain last known machine position. If the system abort occurred while in motion, machine + // position is not guaranteed, since a hard stop can cause the steppers to lose steps. Always + // perform a feedhold before an abort, if maintaining accurate machine position is required. + int32_t last_position[3]; + memcpy(last_position, sys.position, sizeof(sys.position)); // last_position[] = sys.position[] + // Clear all system variables memset(&sys, 0, sizeof(sys)); - + + // Update last known machine position. Set the post-abort work position as the origin [0,0,0], + // which corresponds to the g-code parser and planner positions after re-initialization. + memcpy(sys.position, last_position, sizeof(last_position)); // sys.position[] = last_position[] + memcpy(sys.coord_offset, last_position, sizeof(last_position)); // sys.coord_offset[] = last_position[] + // Reset system. serial_reset_read_buffer(); // Clear serial read buffer settings_init(); // Load grbl settings from EEPROM diff --git a/motion_control.c b/motion_control.c index 12c929e..176accc 100644 --- a/motion_control.c +++ b/motion_control.c @@ -48,7 +48,10 @@ void mc_line(double x, double y, double z, double feed_rate, uint8_t invert_feed { // TODO: Backlash compensation may be installed here. Only need direction info to track when // to insert a backlash line motion(s) before the intended line motion. Requires its own - // plan_check_full_buffer() and check for system abort loop. + // plan_check_full_buffer() and check for system abort loop. Also for position reporting + // backlash steps will need to be also tracked. Not sure what the best strategy is for this, + // i.e. keep the planner independent and do the computations in the status reporting, or let + // the planner handle the position corrections. The latter may get complicated. // If the buffer is full: good! That means we are well ahead of the robot. // Remain in this loop until there is room in the buffer. diff --git a/nuts_bolts.h b/nuts_bolts.h index edbb96c..f1b867f 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -68,9 +68,10 @@ typedef struct { uint8_t feed_hold; // Feed hold flag. Held true during feed hold. Released when ready to resume. uint8_t auto_start; // Planner auto-start flag. Toggled off during feed hold. Defaulted by settings. - int32_t position[3]; // Real-time machine position vector in steps. This may need to be a volatile - // variable, if problems arise. Subject to change. Need to add coordinate offset - // functionality to correctly track part zero and machine zero. + int32_t position[3]; // Real-time machine (aka home) position vector in steps. + // NOTE: This may need to be a volatile variable, if problems arise. + int32_t coord_offset[3]; // Retains the G92 coordinate offset (work coordinates) relative to + // machine zero in steps. volatile uint8_t cycle_start; // Cycle start flag. Set by stepper subsystem or main program. volatile uint8_t execute; // Global system runtime executor bitflag variable. See EXEC bitmasks. diff --git a/planner.c b/planner.c index 772e168..6e09945 100644 --- a/planner.c +++ b/planner.c @@ -41,11 +41,15 @@ static volatile uint8_t block_buffer_head; // Index of the next block to b static volatile uint8_t block_buffer_tail; // Index of the block to process now static uint8_t next_buffer_head; // Index of the next buffer head -static int32_t position[3]; // The planner position of the tool in absolute steps -// static int32_t coord_offset[3]; // Current coordinate offset from machine zero 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 - +// Define planner variables +typedef struct { + int32_t position[3]; // The planner position of the tool in absolute steps. Kept separate + // from g-code position for movements requiring multiple line motions, + // i.e. arcs, canned cycles, and backlash compensation. + double previous_unit_vec[3]; // Unit vector of previous path line segment + double previous_nominal_speed; // Nominal speed of previous path line segment +} planner_t; +static planner_t pl; // Returns the index of the next block in the ring buffer // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. @@ -305,10 +309,7 @@ void plan_reset_buffer() void plan_init() { plan_reset_buffer(); - clear_vector(position); -// clear_vector(coord_offset); - clear_vector_double(previous_unit_vec); - previous_nominal_speed = 0.0; + memset(&pl, 0, sizeof(pl)); // Clear planner struct } void plan_discard_current_block() @@ -357,14 +358,14 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in // 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<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]); - block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); + block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]); + block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]); + block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]); block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); // Bail if this is a zero-length block @@ -372,9 +373,9 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in // 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]; - delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; + delta_mm[X_AXIS] = (target[X_AXIS]-pl.position[X_AXIS])/settings.steps_per_mm[X_AXIS]; + delta_mm[Y_AXIS] = (target[Y_AXIS]-pl.position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; + delta_mm[Z_AXIS] = (target[Z_AXIS]-pl.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])); double inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides @@ -419,16 +420,16 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in 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)) { + if ((block_buffer_head != block_buffer_tail) && (pl.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] ; + double cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + - pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + - pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; // Skip and use default max junction speed for 0 degree acute junction. if (cos_theta < 0.95) { - vmax_junction = min(previous_nominal_speed,block->nominal_speed); + vmax_junction = min(pl.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 @@ -457,15 +458,15 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in 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; + memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] + pl.previous_nominal_speed = block->nominal_speed; // Update buffer head and next buffer head indices block_buffer_head = next_buffer_head; next_buffer_head = next_block_index(block_buffer_head); - // Update position - memcpy(position, target, sizeof(target)); // position[] = target[] + // Update planner position + memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[] planner_recalculate(); } @@ -473,22 +474,26 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in // Reset the planner position vector and planner speed void plan_set_current_position(double x, double y, double z) { - // Track the position offset from the initial position - // TODO: Need to make sure coord_offset is robust and/or needed. Can be used for a soft reset, - // where the machine position is retained after a system abort/reset. However, this is not - // correlated to the actual machine position after a soft reset and may not be needed. This could - // be left to a user interface to maintain. -// coord_offset[X_AXIS] += position[X_AXIS]; -// coord_offset[Y_AXIS] += position[Y_AXIS]; -// coord_offset[Z_AXIS] += position[Z_AXIS]; - 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]); -// coord_offset[X_AXIS] -= position[X_AXIS]; -// coord_offset[Y_AXIS] -= position[Y_AXIS]; -// coord_offset[Z_AXIS] -= position[Z_AXIS]; - previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. - clear_vector_double(previous_unit_vec); + // To correlate status reporting work position correctly, the planner must force the steppers to + // empty the block buffer and synchronize with the planner, as the real-time machine position and + // the planner position at the end of the buffer are different. This will only be called with a + // G92 is executed, which typically is used only at the beginning of a g-code program. + // TODO: Find a robust way to avoid a planner synchronize, but may require a bit of ingenuity. + plan_synchronize(); + + // Update the system coordinate offsets from machine zero + sys.coord_offset[X_AXIS] += pl.position[X_AXIS]; + sys.coord_offset[Y_AXIS] += pl.position[Y_AXIS]; + sys.coord_offset[Z_AXIS] += pl.position[Z_AXIS]; + + memset(&pl, 0, sizeof(pl)); // Clear planner variables. Assume start from rest. + + pl.position[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); // Update planner position + pl.position[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); + pl.position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); + sys.coord_offset[X_AXIS] -= pl.position[X_AXIS]; + sys.coord_offset[Y_AXIS] -= pl.position[Y_AXIS]; + sys.coord_offset[Z_AXIS] -= pl.position[Z_AXIS]; } // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. diff --git a/protocol.c b/protocol.c index 7b812f3..ca80439 100644 --- a/protocol.c +++ b/protocol.c @@ -72,25 +72,31 @@ void protocol_status_report() // information, i.e. 'x0.23,y120.4,z2.4'. This is necessary as it minimizes the computational // overhead and allows grbl to keep running smoothly, especially with g-code programs with fast, // short line segments and interface setups that require real-time status reports (5-20Hz). - // Additionally, during an abort, the steppers are immediately stopped regardless of what they - // are doing. If they are moving, the abort stop can cause grbl to lose steps. However, if a feed - // hold is performed before a system abort, the steppers will steadily decelerate at the max - // acceleration rate, hence the stopped machine position will be maintained and correct. - - // Bare-bones status report. Provides real-time machine position relative to the initialization - // or system reset location (0,0,0), not a home position. This section is under construction and - // the following are needed: coordinate offsets/updating of machine position relative to home, work - // coordinate position?, user setting of output units (mm|inch), compressed (non-human readable) - // data for interfaces?, save last known position in EEPROM? + // **Under construction** Bare-bones status report. Provides real-time machine position relative to + // the system power on location (0,0,0) and work coordinate position, updatable by the G92 command. + // The following are still needed: user setting of output units (mm|inch), compressed (non-human + // readable) data for interfaces?, save last known position in EEPROM?, code optimizations, solidify + // the reporting schemes, move to a separate .c file for easy user accessibility, and setting the + // home position by the user (likely through '$' setting interface). + // Successfully tested at a query rate of 10-20Hz while running a gauntlet of programs at various + // speeds. + int32_t print_position[3]; + memcpy(print_position,sys.position,sizeof(sys.position)); #if REPORT_INCH_MODE - printString("x"); printFloat(sys.position[X_AXIS]/(settings.steps_per_mm[X_AXIS]*MM_PER_INCH)); - printString(",y"); printFloat(sys.position[Y_AXIS]/(settings.steps_per_mm[Y_AXIS]*MM_PER_INCH)); - printString(",z"); printFloat(sys.position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS]*MM_PER_INCH)); + printString("MPos: x"); printFloat(print_position[X_AXIS]/(settings.steps_per_mm[X_AXIS]*MM_PER_INCH)); + printString(",y"); printFloat(print_position[Y_AXIS]/(settings.steps_per_mm[Y_AXIS]*MM_PER_INCH)); + printString(",z"); printFloat(print_position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS]*MM_PER_INCH)); + printString(" WPos: x"); printFloat((print_position[X_AXIS]-sys.coord_offset[X_AXIS])/(settings.steps_per_mm[X_AXIS]*MM_PER_INCH)); + printString(",y"); printFloat((print_position[Y_AXIS]-sys.coord_offset[Y_AXIS])/(settings.steps_per_mm[Y_AXIS]*MM_PER_INCH)); + printString(",z"); printFloat((print_position[Z_AXIS]-sys.coord_offset[Z_AXIS])/(settings.steps_per_mm[Z_AXIS]*MM_PER_INCH)); #else - printString("x"); printFloat(sys.position[X_AXIS]/(settings.steps_per_mm[X_AXIS])); - printString(",y"); printFloat(sys.position[Y_AXIS]/(settings.steps_per_mm[Y_AXIS])); - printString(",z"); printFloat(sys.position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS])); + printString("MPos: x"); printFloat(print_position[X_AXIS]/(settings.steps_per_mm[X_AXIS])); + printString(",y"); printFloat(print_position[Y_AXIS]/(settings.steps_per_mm[Y_AXIS])); + printString(",z"); printFloat(print_position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS])); + printString(" WPos: x"); printFloat((print_position[X_AXIS]-sys.coord_offset[X_AXIS])/(settings.steps_per_mm[X_AXIS])); + printString(",y"); printFloat((print_position[Y_AXIS]-sys.coord_offset[Y_AXIS])/(settings.steps_per_mm[Y_AXIS])); + printString(",z"); printFloat((print_position[Z_AXIS]-sys.coord_offset[Z_AXIS])/(settings.steps_per_mm[Z_AXIS])); #endif printString("\r\n"); } @@ -128,8 +134,8 @@ void protocol_execute_runtime() // Execute and serial print status if (rt_exec & EXEC_STATUS_REPORT) { - bit_false(sys.execute,EXEC_STATUS_REPORT); protocol_status_report(); + bit_false(sys.execute,EXEC_STATUS_REPORT); } // Initiate stepper feed hold diff --git a/script/stream.py b/script/stream.py index 0c91a90..10189fd 100755 --- a/script/stream.py +++ b/script/stream.py @@ -11,7 +11,7 @@ buffer layer to prevent buffer starvation. TODO: - Add runtime command capabilities -Version: SKJ.20120104 +Version: SKJ.20120110 """ import serial @@ -32,6 +32,13 @@ parser.add_argument('-q','--quiet',action='store_true', default=False, help='suppress output text') args = parser.parse_args() +# Periodic timer to query for status reports +# TODO: Need to track down why this doesn't restart consistently before a release. +# def periodic(): +# s.write('?') +# t = threading.Timer(0.1, periodic) # In seconds +# t.start() + # Initialize s = serial.Serial(args.device_file,9600) f = args.gcode_file @@ -51,6 +58,7 @@ print "Streaming ", args.gcode_file.name, " to ", args.device_file l_count = 0 g_count = 0 c_line = [] +# periodic() # Start status report periodic timer for line in f: l_count += 1 # Iterate line counter # l_block = re.sub('\s|\(.*?\)','',line).upper() # Strip comments/spaces/new line and capitalize diff --git a/serial.c b/serial.c index 87aab50..8a20223 100644 --- a/serial.c +++ b/serial.c @@ -71,8 +71,7 @@ void serial_write(uint8_t data) { // Wait until there is space in the buffer while (next_head == tx_buffer_tail) { - protocol_execute_runtime(); // Check for any run-time commands - if (sys.abort) { return; } // Bail, if system abort. + if (sys.execute & EXEC_RESET) { return; } // Only check for abort to avoid an endless loop. } // Store data and advance head diff --git a/stepper.c b/stepper.c index 3278c89..248c7e6 100644 --- a/stepper.c +++ b/stepper.c @@ -139,7 +139,7 @@ static uint8_t iterate_trapezoid_cycle_counter() // interrupt doing its thing, not that big of a deal, but the latter cause is unknown and worrisome. Need // to track down what is causing this problem. Functionally, this shouldn't cause any noticeable issues // as long as stepper drivers have a pulse minimum of 1usec or so (Pololu and any Allegro IC are ok). -// This seems to be an inherent issue that dates all the way back to Simen's v0.6b. +// ** This seems to be an inherent issue that dates all the way back to Simen's v0.6b or earlier. ** ISR(TIMER1_COMPA_vect,ISR_NOBLOCK) { From d27dd13a54badd0f8ecb21b3c06dbde7a2ea8cd3 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sun, 15 Jan 2012 18:25:12 -0700 Subject: [PATCH 32/32] Fix bug with premature step end. Refactored _delay_ms() and square() for better portability. - Fixed a premature step end bug dating back to Simen's 0.7b edge version is fixed, from which this code is forked from. Caused by Timer2 constantly overflowing calling the Step Reset Interrupt every 128usec. Now Timer2 is always disabled after a step end and should free up some cycles for the main program. Could be more than one way to fix this problem. I'm open to suggestions. - _delay_ms() refactored to accept only constants to comply with current compilers. square() removed since not available with some compilers. --- config.h | 2 +- main.c | 2 +- motion_control.c | 2 +- nuts_bolts.c | 8 ++++++++ nuts_bolts.h | 3 +++ planner.c | 11 ++++++----- script/stream.py | 1 + stepper.c | 38 ++++++++++++++++++-------------------- 8 files changed, 39 insertions(+), 28 deletions(-) diff --git a/config.h b/config.h index ac68acb..9f44602 100644 --- a/config.h +++ b/config.h @@ -108,7 +108,7 @@ // this delay will increase the maximum dwell time linearly, but also reduces the responsiveness of // run-time command executions, like status reports, since these are performed between each dwell // time step. Also, keep in mind that the Arduino delay timer is not very accurate for long delays. -#define DWELL_TIME_STEP 50 // Integer (milliseconds) +#define DWELL_TIME_STEP 50 // Integer (1-255) (milliseconds) // ----------------------------------------------- diff --git a/main.c b/main.c index 0ddd410..06b03b8 100644 --- a/main.c +++ b/main.c @@ -39,9 +39,9 @@ system_t sys; int main(void) { // Initialize system - sei(); // Enable interrupts serial_init(BAUD_RATE); // Setup serial baud rate and interrupts st_init(); // Setup stepper pins and interrupt timers + sei(); // Enable interrupts memset(&sys, 0, sizeof(sys)); // Clear all system variables sys.abort = true; // Set abort to complete initialization diff --git a/motion_control.c b/motion_control.c index 176accc..eba3346 100644 --- a/motion_control.c +++ b/motion_control.c @@ -182,7 +182,7 @@ void mc_dwell(double seconds) { uint16_t i = floor(1000/DWELL_TIME_STEP*seconds); plan_synchronize(); - _delay_ms(floor(1000*seconds-i*DWELL_TIME_STEP)); // Delay millisecond remainder + delay_ms(floor(1000*seconds-i*DWELL_TIME_STEP)); // Delay millisecond remainder while (i > 0) { // NOTE: Check and execute runtime commands during dwell every <= DWELL_TIME_STEP milliseconds. protocol_execute_runtime(); diff --git a/nuts_bolts.c b/nuts_bolts.c index 8a6960c..5e018f7 100644 --- a/nuts_bolts.c +++ b/nuts_bolts.c @@ -21,6 +21,7 @@ #include "nuts_bolts.h" #include #include +#include int read_double(char *line, uint8_t *char_counter, double *double_ptr) { @@ -36,3 +37,10 @@ int read_double(char *line, uint8_t *char_counter, double *double_ptr) return(true); } + +// Delays variable defined milliseconds. Compiler compatibility fix for _delay_ms(), +// which only accepts constants in future compiler releases. +void delay_ms(uint16_t ms) +{ + while ( ms-- ) { _delay_ms(1); } +} \ No newline at end of file diff --git a/nuts_bolts.h b/nuts_bolts.h index f1b867f..945129c 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -83,4 +83,7 @@ extern system_t sys; // a pointer to the result variable. Returns true when it succeeds int read_double(char *line, uint8_t *char_counter, double *double_ptr); +// Delays variable-defined milliseconds. Compiler compatibility fix for _delay_ms(). +void delay_ms(uint16_t ms); + #endif diff --git a/planner.c b/planner.c index 6e09945..579116f 100644 --- a/planner.c +++ b/planner.c @@ -376,8 +376,8 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in delta_mm[X_AXIS] = (target[X_AXIS]-pl.position[X_AXIS])/settings.steps_per_mm[X_AXIS]; delta_mm[Y_AXIS] = (target[Y_AXIS]-pl.position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; delta_mm[Z_AXIS] = (target[Z_AXIS]-pl.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])); + block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + + delta_mm[Z_AXIS]*delta_mm[Z_AXIS]); 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. @@ -476,9 +476,10 @@ void plan_set_current_position(double x, double y, double z) { // To correlate status reporting work position correctly, the planner must force the steppers to // empty the block buffer and synchronize with the planner, as the real-time machine position and - // the planner position at the end of the buffer are different. This will only be called with a - // G92 is executed, which typically is used only at the beginning of a g-code program. - // TODO: Find a robust way to avoid a planner synchronize, but may require a bit of ingenuity. + // the planner position at the end of the buffer can be and are usually different. This function is + // only called with a G92, which typically is used only at the beginning of a g-code program or + // between different operations. + // TODO: Find a robust way to avoid a planner synchronize, but this may require a bit of ingenuity. plan_synchronize(); // Update the system coordinate offsets from machine zero diff --git a/script/stream.py b/script/stream.py index 10189fd..17cded3 100755 --- a/script/stream.py +++ b/script/stream.py @@ -19,6 +19,7 @@ import re import time import sys import argparse +# import threading RX_BUFFER_SIZE = 128 diff --git a/stepper.c b/stepper.c index 248c7e6..4ebda7b 100644 --- a/stepper.c +++ b/stepper.c @@ -130,30 +130,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. - -// TODO: ISR_NOBLOCK is the same as the old SIGNAL with sei() method, but is optimizable by the compiler. On -// an oscilloscope there is a weird hitch in the step pulse during high load operation. Very infrequent, but -// when this does happen most of the time the pulse falling edge is randomly delayed by 20%-50% of the total -// intended pulse time, but sometimes it pulses less than 3usec. The former likely caused by the serial -// interrupt doing its thing, not that big of a deal, but the latter cause is unknown and worrisome. Need -// to track down what is causing this problem. Functionally, this shouldn't cause any noticeable issues -// as long as stepper drivers have a pulse minimum of 1usec or so (Pololu and any Allegro IC are ok). -// ** This seems to be an inherent issue that dates all the way back to Simen's v0.6b or earlier. ** - -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 = step_pulse_time; - + // Enable step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after + // exactly settings.pulse_microseconds microseconds, independent of the main Timer1 prescaler. + TCNT2 = step_pulse_time; // Reload timer counter + TCCR2B = (1<