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.
2012-01-10 08:34:48 -07:00
parent 6f27e2cdb1
commit 89a3b37e02
9 changed files with 106 additions and 71 deletions
--- 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);
--- 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
--- 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.
--- 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.
--- 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<<X_DIRECTION_BIT); }
-  if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
-  if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
+  if (target[X_AXIS] < pl.position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
+  if (target[Y_AXIS] < pl.position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
+  if (target[Z_AXIS] < pl.position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
  
  // Number of steps for each axis
-  block->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.
--- 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
--- 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
--- 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
--- 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)
 {