From 89a3b37e020aa4e657973baadb0ff0936aa25656 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Tue, 10 Jan 2012 08:34:48 -0700 Subject: [PATCH] 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) {