From 0a46dfe0b91d58c8b40cf12252ac67516458bf00 Mon Sep 17 00:00:00 2001
From: Robert Grzesek <robert@robgrz.com>
Date: Tue, 25 Feb 2014 12:19:52 -0800
Subject: [PATCH] Minimal probing cycle working.  Supports both G38.2 for error
 and G38.3 when no errors are desired.

---
 cpu_map.h        |  3 +-
 gcode.c          | 26 +++++++++++++
 gcode.h          |  6 ++-
 motion_control.c | 97 ++++++++++++++++++++++++++++++++++++++++++++++++
 motion_control.h |  5 +++
 report.c         | 51 +++++++++++++++++++++++++
 report.h         |  6 +++
 stepper.c        |  6 +++
 system.c         |  5 +++
 system.h         |  9 +++++
 10 files changed, 211 insertions(+), 3 deletions(-)

diff --git a/cpu_map.h b/cpu_map.h
index 9adee32..0538bbc 100644
--- a/cpu_map.h
+++ b/cpu_map.h
@@ -107,10 +107,11 @@
   #define PIN_RESET        0  // Uno Analog Pin 0
   #define PIN_FEED_HOLD    1  // Uno Analog Pin 1
   #define PIN_CYCLE_START  2  // Uno Analog Pin 2
+  #define PIN_PROBE        5  // Uno Analog Pin 5
   #define PINOUT_INT       PCIE1  // Pin change interrupt enable pin
   #define PINOUT_INT_vect  PCINT1_vect
   #define PINOUT_PCMSK     PCMSK1 // Pin change interrupt register
-  #define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START))
+  #define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START)|(1<<PIN_PROBE))
   
   #ifdef VARIABLE_SPINDLE
     // Advanced Configuration Below You should not need to touch these variables
diff --git a/gcode.c b/gcode.c
index fc6ad6e..e0c31d9 100644
--- a/gcode.c
+++ b/gcode.c
@@ -147,6 +147,14 @@ uint8_t gc_execute_line(char *line)
               default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
             }
             break;
+          case 38: 
+            int_value = trunc(10*value); // Multiply by 10 to pick up Gxx.1
+            switch(int_value) {
+              case 382: non_modal_action = NON_MODAL_PROBE_WITH_ERROR; break;
+              case 383: non_modal_action = NON_MODAL_PROBE_NO_ERROR; break;
+              default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
+            }
+            break;  
           case 53: absolute_override = true; break;
           case 54: case 55: case 56: case 57: case 58: case 59:
             gc.coord_select = int_value-54;
@@ -344,6 +352,24 @@ uint8_t gc_execute_line(char *line)
       memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[];
       axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
       break;
+    case NON_MODAL_PROBE_WITH_ERROR:
+        if (!axis_words) { // No axis words
+          FAIL(STATUS_INVALID_STATEMENT);
+          break;
+        }
+        if(mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number)){
+          FAIL(STATUS_PROBE_ERROR);
+        }
+        axis_words = 0;
+      break;
+    case NON_MODAL_PROBE_NO_ERROR:
+        if (!axis_words) { // No axis words
+          FAIL(STATUS_INVALID_STATEMENT);
+          break;
+        }
+        mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number);
+        axis_words = 0;
+      break;   
     case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1:
       if (non_modal_action == NON_MODAL_SET_HOME_0) {
         settings_write_coord_data(SETTING_INDEX_G28,gc.position);
diff --git a/gcode.h b/gcode.h
index 60684ea..f4fd7bd 100644
--- a/gcode.h
+++ b/gcode.h
@@ -60,8 +60,10 @@
 #define NON_MODAL_SET_HOME_0 4 // G28.1
 #define NON_MODAL_GO_HOME_1 5 // G30
 #define NON_MODAL_SET_HOME_1 6 // G30.1
-#define NON_MODAL_SET_COORDINATE_OFFSET 7 // G92
-#define NON_MODAL_RESET_COORDINATE_OFFSET 8 //G92.1
+#define NON_MODAL_PROBE_WITH_ERROR 7 //G38.2
+#define NON_MODAL_PROBE_NO_ERROR 8 //G38.3 
+#define NON_MODAL_SET_COORDINATE_OFFSET 9 // G92
+#define NON_MODAL_RESET_COORDINATE_OFFSET 10 //G92.1
 
 typedef struct {
   uint8_t status_code;             // Parser status for current block
diff --git a/motion_control.c b/motion_control.c
index d11b0b4..1298291 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -30,6 +30,7 @@
 #include "spindle_control.h"
 #include "coolant_control.h"
 #include "limits.h"
+#include "report.h"
 
 
 // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
@@ -246,6 +247,102 @@ void mc_homing_cycle()
   limits_init();
 }
 
+uint8_t mc_probe_cycle(float *t, float feed_rate, uint8_t invert_feed_rate, int32_t line_number)
+{
+  protocol_buffer_synchronize(); //finish all queued commands
+
+  if (sys.abort) { return STATUS_OK; } // Return if system reset has been issued.
+    
+  uint8_t i;
+  float target[N_AXIS];
+
+  //copy target position since we'll be modifying it with the probe position on a successful move
+  //The gc_sync_position() at the end may elimiante the need for this.  Not sure though.
+  for(i=0; i<N_AXIS; ++i){
+    target[i] = t[i];
+  }
+
+  plan_reset(); // Reset planner buffer to zero planner current position and to clear previous motions.
+  
+  // Perform probing cycle. Planner buffer should be empty at this point.
+  // An empty buffer is needed because we need to enable the probe pin along the same move that we're about to execute.
+  
+  sys.state = STATE_CYCLE;
+  plan_buffer_line(target, feed_rate, invert_feed_rate, line_number); // Bypass mc_line(). Directly plan homing motion.
+  st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
+  st_wake_up(); // Initiate motion
+  
+  sys.probe_state = PROBE_ACTIVE;
+  //TODO - make sure the probe isn't already closed
+  do {
+      
+    if( sys.probe_state == PROBE_OFF ){
+      sys.execute |= EXEC_FEED_HOLD;
+      protocol_execute_runtime();
+      break;
+    }
+    protocol_execute_runtime();
+    st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us.
+    
+    if (sys.execute & EXEC_RESET) { 
+      sys.probe_state = PROBE_OFF;
+      protocol_execute_runtime(); 
+      return STATUS_OK; 
+    }
+
+    //Check for motion ended because switch never triggered
+    if(sys.state != STATE_CYCLE && sys.state != STATE_HOLD){
+      sys.probe_state = PROBE_OFF;
+      report_realtime_status_probe();
+      return STATUS_PROBE_ERROR;
+    }
+
+  } while (1);
+
+  //report_realtime_status(); //debug
+
+  while((sys.execute & EXEC_CYCLE_STOP) == 0 && (sys.state == STATE_CYCLE || sys.state == STATE_HOLD)){
+    protocol_execute_runtime();
+    if (sys.abort) { return STATUS_OK; } // Check for system abort
+  }
+
+  //Prep the new target based on the position that the probe triggered
+  for(i=0; i<N_AXIS; ++i){
+    target[i] = (float)sys.probe_position[i]/settings.steps_per_mm[i];
+  }
+
+  protocol_execute_runtime();
+
+  st_reset(); // Immediately force kill steppers and reset step segment buffer.
+  plan_reset(); // Reset planner buffer. Zero planner positions. Ensure homing motion is cleared.
+  plan_sync_position(); // Sync planner position to current machine position for pull-off move.
+
+  //report_realtime_status(); //debug
+
+  plan_buffer_line(target, feed_rate, invert_feed_rate, line_number); // Bypass mc_line(). Directly plan motion.
+  st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
+  st_wake_up(); // Initiate motion
+
+  protocol_execute_runtime();
+  sys.execute |= EXEC_CYCLE_START;
+  protocol_buffer_synchronize(); // Complete pull-off motion.
+
+  //report_realtime_status(); //debug
+    
+  protocol_execute_runtime(); // Check for reset and set system abort.
+  if (sys.abort) { return STATUS_OK; } // Did not complete. Alarm state set by mc_alarm.
+
+  // Gcode parser position was circumvented by the this routine, so sync position now.
+  gc_sync_position();
+  
+  // Set idle state after probing completes and before returning to main program.  
+  sys.state = STATE_IDLE;
+  st_go_idle(); 
+
+  //TODO - ouput a mandatory status update with the probe position.  What if another was recently sent?
+  report_realtime_status_probe();
+  return STATUS_OK;
+}
 
 // Method to ready the system to reset by setting the runtime reset command and killing any
 // active processes in the system. This also checks if a system reset is issued while Grbl
diff --git a/motion_control.h b/motion_control.h
index 703cede..0e063fe 100644
--- a/motion_control.h
+++ b/motion_control.h
@@ -43,6 +43,11 @@ void mc_dwell(float seconds);
 // Perform homing cycle to locate machine zero. Requires limit switches.
 void mc_homing_cycle();
 
+// Perform tool length probe cycle. Requires probe switch.
+// Returns STATUS_OK in all cases except when the motion is completed without the probe being triggered.
+// In that case, it returns a STATUS_PROBE_ERROR
+uint8_t mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number);
+
 // Performs system reset. If in motion state, kills all motion and sets system alarm.
 void mc_reset();
 
diff --git a/report.c b/report.c
index 13c27a9..bcecfc8 100644
--- a/report.c
+++ b/report.c
@@ -79,6 +79,8 @@ void report_status_message(uint8_t status_code)
       printPgmString(PSTR("Homing not enabled")); break;
       case STATUS_OVERFLOW:
       printPgmString(PSTR("Line overflow")); break; 
+      case STATUS_PROBE_ERROR:
+      printPgmString(PSTR("Probe error")); break; 
     }
     printPgmString(PSTR("\r\n"));
   }
@@ -364,3 +366,52 @@ void report_realtime_status()
     
   printPgmString(PSTR(">\r\n"));
 }
+
+// Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram 
+ // and the actual location of the CNC machine. Users may change the following function to their
+ // specific needs.  It is kept separate from the "normal" report_realtime_status() to allow customization. 
+void report_realtime_status_probe()
+{
+  // **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 (G54 and G92 applied). 
+  uint8_t i;
+  int32_t current_position[N_AXIS]; // Copy current state of the system position variable
+  memcpy(current_position,sys.position,sizeof(sys.position));
+  float print_position[N_AXIS];
+ 
+  printPgmString(PSTR("<Probe")); 
+
+  // Report machine position
+  printPgmString(PSTR(",MPos:")); 
+  for (i=0; i< N_AXIS; i++) {
+    print_position[i] = current_position[i]/settings.steps_per_mm[i];
+    if (bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)) { print_position[i] *= INCH_PER_MM; }
+    printFloat(print_position[i]);
+    printPgmString(PSTR(","));
+  }
+  
+  // Report work position
+  printPgmString(PSTR("WPos:")); 
+  for (i=0; i< N_AXIS; i++) {
+    if (bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)) {
+      print_position[i] -= (gc.coord_system[i]+gc.coord_offset[i])*INCH_PER_MM;
+    } else {
+      print_position[i] -= gc.coord_system[i]+gc.coord_offset[i];
+    }
+    printFloat(print_position[i]);
+    if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); }
+  }
+    
+#ifdef USE_LINE_NUMBERS
+  // Report current line number
+  printPgmString(PSTR(",Ln:")); 
+  int32_t ln=0;
+  plan_block_t * pb = plan_get_current_block();
+  if(pb != NULL) {
+    ln = pb->line_number;
+  } 
+  printInteger(ln);
+#endif
+    
+  printPgmString(PSTR(">\r\n"));
+}
diff --git a/report.h b/report.h
index 92984df..f05bc42 100644
--- a/report.h
+++ b/report.h
@@ -36,6 +36,7 @@
 #define STATUS_ALARM_LOCK 12
 #define STATUS_SOFT_LIMIT_ERROR 13
 #define STATUS_OVERFLOW 14
+#define STATUS_PROBE_ERROR 15
 
 // Define Grbl alarm codes. Less than zero to distinguish alarm error from status error.
 #define ALARM_LIMIT_ERROR -1
@@ -69,6 +70,11 @@ void report_grbl_settings();
 // Prints realtime status report
 void report_realtime_status();
 
+// Prints realtime position status report at the end of a probe cycle
+// This is in leiu of saving the probe position to internal variables like an 
+// EMC machine
+void report_realtime_status_probe();
+
 // Prints Grbl persistent coordinate parameters
 void report_gcode_parameters();
 
diff --git a/stepper.c b/stepper.c
index bfe1f74..370d798 100644
--- a/stepper.c
+++ b/stepper.c
@@ -282,6 +282,12 @@ ISR(TIMER1_COMPA_vect)
 {        
 // SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
   if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
+
+  //Check if we need to copy the current position to the probe_position
+  if(sys.probe_state == PROBE_COPY_POSITION){
+    sys.probe_state = PROBE_OFF;
+    memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS);
+  }
   
   // Set the direction pins a couple of nanoseconds before we step the steppers
   DIRECTION_PORT = (DIRECTION_PORT & ~DIRECTION_MASK) | (st.dir_outbits & DIRECTION_MASK);
diff --git a/system.c b/system.c
index 110fa82..fe29523 100644
--- a/system.c
+++ b/system.c
@@ -49,6 +49,11 @@ ISR(PINOUT_INT_vect)
       sys.execute |= EXEC_FEED_HOLD; 
     } else if (bit_isfalse(PINOUT_PIN,bit(PIN_CYCLE_START))) {
       sys.execute |= EXEC_CYCLE_START;
+    } else if (bit_isfalse(PINOUT_PIN,bit(PIN_PROBE))) {
+      if(sys.probe_state == PROBE_ACTIVE){
+        sys.probe_state = PROBE_COPY_POSITION;
+        //sys.execute |= EXEC_FEED_HOLD; //Probably OK to call a feedhold here.  I'd prefer to do it in the main probe loop for now
+      }
     }
   }
 }
diff --git a/system.h b/system.h
index 12d8965..0b84eab 100644
--- a/system.h
+++ b/system.h
@@ -67,6 +67,13 @@
 #define STATE_HOLD       bit(5) // Executing feed hold
 // #define STATE_JOG     bit(6) // Jogging mode is unique like homing.
 
+// Values that define the probing state machine.  
+#define PROBE_OFF     0 //No probing
+#define PROBE_ACTIVE  1 //Actively watching the input pin.  If it is triggered, the stante is changed to PROBE_COPY_POSITION
+#define PROBE_COPY_POSITION 2 //In this state, the current position will be copied to probe_position in the stepper ISR.  State is then changed to PROBE_OFF.
+                              //Copying to a separate set of variables ensures that no race condition can occur if the ISR updates the main position variables
+                              //while the probing routine is copying them.
+
 // Define global system variables
 typedef struct {
   uint8_t abort;                 // System abort flag. Forces exit back to main loop for reset.
@@ -76,6 +83,8 @@ typedef struct {
   int32_t position[N_AXIS];      // Real-time machine (aka home) position vector in steps. 
                                  // NOTE: This may need to be a volatile variable, if problems arise.                             
   uint8_t auto_start;            // Planner auto-start flag. Toggled off during feed hold. Defaulted by settings.
+  uint8_t probe_state;            // Probing state value.  Used in the mc_probe_cycle(), the PINOUT_PIN IRT, and the stepper ISR to coordinate the probing cycle.
+  int32_t probe_position[N_AXIS];      // Copy of the position when the probe is triggered that can be read/copied without worring about changes in the middle of a read.   
 } system_t;
 extern system_t sys;