From 76ab1b6a4282ec09df7b2c1dd534b83e61b1251c Mon Sep 17 00:00:00 2001
From: Sonny Jeon <chamnit@gmail.com>
Date: Fri, 28 Feb 2014 22:03:26 -0700
Subject: [PATCH] G38.2 probe feature rough draft installed. Working but needs
 testing.
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- G38.2 straight probe now supported. Rough draft. May be tweaked more
as testing ramps up.

- G38.2 requires at least one axis word. Multiple axis words work too.
When commanded, the probe cycle will move at the last ‘F’ feed rate
specified in a straight line.

- During a probe cycle: If the probe pin goes low (normal high), Grbl
will record that immediate position and engage a feed hold. Meaning
that the CNC machine will move a little past the probe switch point, so
keep federates low to stop sooner. Once stopped, Grbl will issue a move
to go back to the recorded probe trigger point.

- During a probe cycle: If the probe switch does not engage by the time
the machine has traveled to its target coordinates, Grbl will issue an
ALARM and the user will be forced to reset Grbl. (Currently G38.3 probe
without error isn’t supported, but would be easy to implement later.)

- After a successful probe, Grbl will send a feedback message
containing the recorded probe coordinates in the machine coordinate
system. This is as the g-code standard on probe parameters specifies.

- The recorded probe parameters are retained in Grbl memory and can be
viewed with the ‘$#’ print parameters command. Upon a power-cycle, not
a soft-reset, Grbl will re-zero these values.

- Moved ‘$#’ command to require IDLE or ALARM mode, because it accesses
EEPROM to fetch the coordinate system offsets.

- Updated the Grbl version to v0.9d.

- The probe cycle is subject to change upon testing or user-feedback.
---
 Makefile         |  2 +-
 cpu_map.h        | 11 +++++-
 gcode.c          | 44 ++++++++--------------
 gcode.h          | 11 +++---
 limits.c         |  6 +--
 main.c           |  2 +
 motion_control.c | 98 ++++++++++++------------------------------------
 motion_control.h |  6 +--
 probe.c          | 45 ++++++++++++++++++++++
 probe.h          | 36 ++++++++++++++++++
 protocol.c       |  6 ++-
 report.c         | 84 ++++++++++++++---------------------------
 report.h         | 12 +++---
 settings.h       |  4 +-
 stepper.c        | 11 +++---
 system.c         | 17 +++------
 system.h         | 10 +----
 17 files changed, 195 insertions(+), 210 deletions(-)
 create mode 100644 probe.c
 create mode 100644 probe.h

diff --git a/Makefile b/Makefile
index ea1ea61..ab8c9ec 100644
--- a/Makefile
+++ b/Makefile
@@ -33,7 +33,7 @@ CLOCK      = 16000000
 PROGRAMMER ?= -c avrisp2 -P usb
 OBJECTS    = main.o motion_control.o gcode.o spindle_control.o coolant_control.o serial.o \
              protocol.o stepper.o eeprom.o settings.o planner.o nuts_bolts.o limits.o \
-             print.o report.o system.o
+             print.o probe.o report.o system.o
 # FUSES      = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
 FUSES      = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m
 # update that line with this when programmer is back up:
diff --git a/cpu_map.h b/cpu_map.h
index 0538bbc..060135a 100644
--- a/cpu_map.h
+++ b/cpu_map.h
@@ -107,11 +107,18 @@
   #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)|(1<<PIN_PROBE))
+  #define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START))
+  
+  // Define probe switch input pin.
+  #define PROBE_DDR       DDRC
+  #define PROBE_PIN       PINC
+  #define PROBE_PORT      PORTC
+  #define PROBE_BIT       5  // Uno Analog Pin 5
+  #define PROBE_MASK      (1<<PROBE_BIT)
+
   
   #ifdef VARIABLE_SPINDLE
     // Advanced Configuration Below You should not need to touch these variables
diff --git a/gcode.c b/gcode.c
index 7b31552..93c79ea 100644
--- a/gcode.c
+++ b/gcode.c
@@ -119,7 +119,7 @@ uint8_t gc_execute_line(char *line)
         // Set modal group values
         switch(int_value) {
           case 4: case 10: case 28: case 30: case 53: case 92: group_number = MODAL_GROUP_0; break;
-          case 0: case 1: case 2: case 3: case 80: group_number = MODAL_GROUP_1; break;
+          case 0: case 1: case 2: case 3: case 38: case 80: group_number = MODAL_GROUP_1; break;
           case 17: case 18: case 19: group_number = MODAL_GROUP_2; break;
           case 90: case 91: group_number = MODAL_GROUP_3; break;
           case 93: case 94: group_number = MODAL_GROUP_5; break;
@@ -152,8 +152,10 @@ uint8_t gc_execute_line(char *line)
           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;
+              case 382: gc.motion_mode = MOTION_MODE_PROBE; break;
+              // case 383: gc.motion_mode = MOTION_MODE_PROBE_NO_ERROR; break; // Not supported.
+              // case 384: // Not supported.
+              // case 385: // Not supported.                            
               default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
             }
             break;  
@@ -366,32 +368,6 @@ 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;
-        }
-        #ifdef USE_LINE_NUMBERS
-        if(mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number)){
-        #else
-        if(mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode)){
-        #endif
-          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;
-        }
-        #ifdef USE_LINE_NUMBERS
-        mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number);
-        #else
-        mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
-        #endif
-        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);
@@ -512,6 +488,16 @@ uint8_t gc_execute_line(char *line)
           #endif
         }            
         break;
+      case MOTION_MODE_PROBE:
+        if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT); }
+        else {
+          #ifdef USE_LINE_NUMBERS
+          mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number);
+          #else
+          mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
+          #endif
+        }
+        break;
     }
     
     // Report any errors.
diff --git a/gcode.h b/gcode.h
index f4fd7bd..acd7046 100644
--- a/gcode.h
+++ b/gcode.h
@@ -31,7 +31,7 @@
 // and are similar/identical to other g-code interpreters by manufacturers (Haas,Fanuc,Mazak,etc).
 #define MODAL_GROUP_NONE 0
 #define MODAL_GROUP_0 1 // [G4,G10,G28,G30,G53,G92,G92.1] Non-modal
-#define MODAL_GROUP_1 2 // [G0,G1,G2,G3,G80] Motion
+#define MODAL_GROUP_1 2 // [G0,G1,G2,G3,G38.2,G80] Motion
 #define MODAL_GROUP_2 3 // [G17,G18,G19] Plane selection
 #define MODAL_GROUP_3 4 // [G90,G91] Distance mode
 #define MODAL_GROUP_4 5 // [M0,M1,M2,M30] Stopping
@@ -47,7 +47,8 @@
 #define MOTION_MODE_LINEAR 1 // G1
 #define MOTION_MODE_CW_ARC 2  // G2
 #define MOTION_MODE_CCW_ARC 3  // G3
-#define MOTION_MODE_CANCEL 4 // G80
+#define MOTION_MODE_PROBE 4 // G38.x
+#define MOTION_MODE_CANCEL 5 // G80
 
 #define PROGRAM_FLOW_RUNNING 0
 #define PROGRAM_FLOW_PAUSED 1 // M0, M1
@@ -60,10 +61,8 @@
 #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_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
+#define NON_MODAL_SET_COORDINATE_OFFSET 7 // G92
+#define NON_MODAL_RESET_COORDINATE_OFFSET 8 //G92.1
 
 typedef struct {
   uint8_t status_code;             // Parser status for current block
diff --git a/limits.c b/limits.c
index 636701d..c31988a 100644
--- a/limits.c
+++ b/limits.c
@@ -86,7 +86,7 @@ ISR(LIMIT_INT_vect) // DEFAULT: Limit pin change interrupt process.
   if (sys.state != STATE_ALARM) { 
     if (bit_isfalse(sys.execute,EXEC_ALARM)) {
       mc_reset(); // Initiate system kill.
-      sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event
+      sys.execute |= (EXEC_ALARM | EXEC_CRIT_EVENT); // Indicate hard limit critical event
     }
   }
 }  
@@ -103,7 +103,7 @@ ISR(WDT_vect) // Watchdog timer ISR
       if (bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { bits ^= LIMIT_MASK; }
       if (bits & LIMIT_MASK) {
         mc_reset(); // Initiate system kill.
-        sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event
+        sys.execute |= (EXEC_ALARM | EXEC_CRIT_EVENT); // Indicate hard limit critical event
       }
     }  
   }
@@ -267,7 +267,7 @@ void limits_soft_check(float *target)
       }
       
       mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
-      sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event
+      sys.execute |= (EXEC_ALARM | EXEC_CRIT_EVENT); // Indicate soft limit critical event
       protocol_execute_runtime(); // Execute to enter critical event loop and system abort
       return;
     
diff --git a/main.c b/main.c
index 117261e..c981cb1 100644
--- a/main.c
+++ b/main.c
@@ -30,6 +30,7 @@
 #include "coolant_control.h"
 #include "motion_control.h"
 #include "limits.h"
+#include "probe.h"
 #include "report.h"
 
 
@@ -73,6 +74,7 @@ int main(void)
     spindle_init();
     coolant_init();
     limits_init(); 
+    probe_init();
     plan_reset(); // Clear block buffer and planner variables
     st_reset(); // Clear stepper subsystem variables.
 
diff --git a/motion_control.c b/motion_control.c
index 00d2423..3b2080c 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 "probe.h"
 #include "report.h"
 
 
@@ -270,75 +271,39 @@ void mc_homing_cycle()
 }
 
 
+// Perform tool length probe cycle. Requires probe switch.
+// NOTE: Upon probe failure, the program will be stopped and placed into ALARM state.
 #ifdef USE_LINE_NUMBERS
-uint8_t mc_probe_cycle(float *t, float feed_rate, uint8_t invert_feed_rate, int32_t line_number)
+void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number)
 #else
-uint8_t mc_probe_cycle(float *t, float feed_rate, uint8_t invert_feed_rate)
+void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate)
 #endif
 {
-  protocol_buffer_synchronize(); //finish all queued commands
+  protocol_buffer_synchronize(); // Finish all queued commands
+  if (sys.abort) { return; } // Return if system reset has been issued.
 
-  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;
-
   #ifdef USE_LINE_NUMBERS
-  plan_buffer_line(target, feed_rate, invert_feed_rate, line_number); // Bypass mc_line(). Directly plan homing motion.
+  mc_line(target, feed_rate, invert_feed_rate, line_number);
   #else
-  plan_buffer_line(target, feed_rate, invert_feed_rate); // Bypass mc_line(). Directly plan homing motion.
+  mc_line(target, feed_rate, invert_feed_rate);
   #endif
-  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
+  sys.probe_state = PROBE_ACTIVE;
+
+  sys.execute |= EXEC_CYCLE_START;
   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; 
-    }
+    protocol_execute_runtime(); 
+    if (sys.abort) { return; } // Check for system abort
+  } while ((sys.state != STATE_IDLE) && (sys.state != STATE_QUEUED));
 
-    //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
-  }
+  if (sys.probe_state == PROBE_ACTIVE) { sys.execute |= EXEC_CRIT_EVENT; }
+  protocol_execute_runtime();   // Check and execute run-time commands
+  if (sys.abort) { return; } // Check for system abort
 
   //Prep the new target based on the position that the probe triggered
+  uint8_t i;
   for(i=0; i<N_AXIS; ++i){
     target[i] = (float)sys.probe_position[i]/settings.steps_per_mm[i];
   }
@@ -349,37 +314,24 @@ uint8_t mc_probe_cycle(float *t, float feed_rate, uint8_t invert_feed_rate)
   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
-
   #ifdef USE_LINE_NUMBERS
-  plan_buffer_line(target, feed_rate, invert_feed_rate, line_number); // Bypass mc_line(). Directly plan homing motion.
+  mc_line(target, feed_rate, invert_feed_rate, line_number); // Bypass mc_line(). Directly plan homing motion.
   #else
-  plan_buffer_line(target, feed_rate, invert_feed_rate); // Bypass mc_line(). Directly plan homing motion.
+  mc_line(target, feed_rate, invert_feed_rate); // Bypass mc_line(). Directly plan homing motion.
   #endif
-  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.
+  if (sys.abort) { return; } // 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;
+  report_probe_parameters();
 }
 
+
 // 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
 // is in a motion state. If so, kills the steppers and sets the system alarm to flag position
diff --git a/motion_control.h b/motion_control.h
index 4956487..b8a116b 100644
--- a/motion_control.h
+++ b/motion_control.h
@@ -52,12 +52,10 @@ void mc_dwell(float seconds);
 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
 #ifdef USE_LINE_NUMBERS
-uint8_t mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number);
+void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number);
 #else
-uint8_t mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate);
+void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate);
 #endif
 
 // Performs system reset. If in motion state, kills all motion and sets system alarm.
diff --git a/probe.c b/probe.c
new file mode 100644
index 0000000..20b43a8
--- /dev/null
+++ b/probe.c
@@ -0,0 +1,45 @@
+/*
+  probe.c - code pertaining to probing methods
+  Part of Grbl
+
+  Copyright (c) 2014 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 <http://www.gnu.org/licenses/>.
+*/
+  
+#include "system.h"
+#include "probe.h"
+
+
+// Probe pin initialization routine.
+void probe_init() 
+{
+  PROBE_DDR &= ~(PROBE_MASK); // Configure as input pins
+  PROBE_PORT |= PROBE_MASK;   // Enable internal pull-up resistors. Normal high operation.
+}
+
+
+// Monitors probe pin state and records the system position when detected. Called by the
+// stepper ISR per ISR tick.
+// NOTE: This function must be extremely efficient as to not bog down the stepper ISR.
+void probe_state_monitor()
+{
+  if (sys.probe_state == PROBE_ACTIVE) { 
+    if (!(PROBE_PIN & PROBE_MASK)) {
+      sys.probe_state = PROBE_OFF;
+      memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS);
+      sys.execute |= EXEC_FEED_HOLD;
+    }
+  }
+}
diff --git a/probe.h b/probe.h
new file mode 100644
index 0000000..e5aef86
--- /dev/null
+++ b/probe.h
@@ -0,0 +1,36 @@
+/*
+  probe.h - code pertaining to probing methods
+  Part of Grbl
+
+  Copyright (c) 2014 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 <http://www.gnu.org/licenses/>.
+*/
+  
+#ifndef probe_h
+#define probe_h 
+
+// Values that define the probing state machine.  
+#define PROBE_OFF     0 // No probing. (Must be zero.)
+#define PROBE_ACTIVE  1 // Actively watching the input pin.
+
+
+// Probe pin initialization routine.
+void probe_init();
+
+// Monitors probe pin state and records the system position when detected. Called by the
+// stepper ISR per ISR tick.
+void probe_state_monitor();
+
+#endif
diff --git a/protocol.c b/protocol.c
index c8406d0..2bf7bcd 100644
--- a/protocol.c
+++ b/protocol.c
@@ -173,9 +173,11 @@ void protocol_execute_runtime()
     if (rt_exec & (EXEC_ALARM | EXEC_CRIT_EVENT)) {      
       sys.state = STATE_ALARM; // Set system alarm state
 
-      // Critical event. Only hard/soft limit errors currently qualify.
+      // Critical events. Hard/soft limit events identified by both critical event and alarm exec
+      // flags. Probe fail is identified by the critical event exec flag only.
       if (rt_exec & EXEC_CRIT_EVENT) {
-        report_alarm_message(ALARM_LIMIT_ERROR); 
+        if (rt_exec & EXEC_ALARM) { report_alarm_message(ALARM_LIMIT_ERROR); }
+        else { report_alarm_message(ALARM_PROBE_FAIL); }
         report_feedback_message(MESSAGE_CRITICAL_EVENT);
         bit_false(sys.execute,EXEC_RESET); // Disable any existing reset
         do { 
diff --git a/report.c b/report.c
index bcecfc8..e766ee5 100644
--- a/report.c
+++ b/report.c
@@ -79,8 +79,6 @@ 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"));
   }
@@ -95,8 +93,10 @@ void report_alarm_message(int8_t alarm_code)
     printPgmString(PSTR("Hard/soft limit")); break;
     case ALARM_ABORT_CYCLE: 
     printPgmString(PSTR("Abort during cycle")); break;
+    case ALARM_PROBE_FAIL:
+    printPgmString(PSTR("Probe fail")); break;
   }
-  printPgmString(PSTR(". MPos?\r\n"));
+  printPgmString(PSTR("\r\n"));
   delay_ms(500); // Force delay to ensure message clears serial write buffer.
 }
 
@@ -190,8 +190,28 @@ void report_grbl_settings() {
 }
 
 
-// Prints gcode coordinate offset parameters
-void report_gcode_parameters()
+// Prints current probe parameters. Upon a probe command, these parameters are updated upon a
+// successful probe or upon a failed probe with the G38.3 without errors command (if supported). 
+// These values are retained until Grbl is power-cycled, whereby they will be re-zeroed.
+void report_probe_parameters()
+{
+  uint8_t i;
+  float print_position[N_AXIS];
+ 
+  // Report in terms of machine position.
+  printPgmString(PSTR("[Probe:")); 
+  for (i=0; i< N_AXIS; i++) {
+    print_position[i] = sys.probe_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]);
+    if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); }
+  }  
+  printPgmString(PSTR("]\r\n"));
+}
+
+
+// Prints Grbl NGC parameters (coordinate offsets, probing)
+void report_ngc_parameters()
 {
   float coord_data[N_AXIS];
   uint8_t coord_select, i;
@@ -226,6 +246,7 @@ void report_gcode_parameters()
     if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); }
     else { printPgmString(PSTR("]\r\n")); }
   } 
+  report_probe_parameters(); // Print probe parameters. Not persistent in memory.
 }
 
 
@@ -353,7 +374,7 @@ void report_realtime_status()
     if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); }
   }
     
-#ifdef USE_LINE_NUMBERS
+  #ifdef USE_LINE_NUMBERS
   // Report current line number
   printPgmString(PSTR(",Ln:")); 
   int32_t ln=0;
@@ -362,56 +383,7 @@ void report_realtime_status()
     ln = pb->line_number;
   } 
   printInteger(ln);
-#endif
-    
-  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
+  #endif
     
   printPgmString(PSTR(">\r\n"));
 }
diff --git a/report.h b/report.h
index f05bc42..91d7797 100644
--- a/report.h
+++ b/report.h
@@ -36,11 +36,11 @@
 #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
 #define ALARM_ABORT_CYCLE -2
+#define ALARM_PROBE_FAIL -3
 
 // Define Grbl feedback message codes.
 #define MESSAGE_CRITICAL_EVENT 1
@@ -70,13 +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 recorded probe position
+void report_probe_parameters();
 
-// Prints Grbl persistent coordinate parameters
-void report_gcode_parameters();
+// Prints Grbl NGC parameters (coordinate offsets, probe)
+void report_ngc_parameters();
 
 // Prints current g-code parser mode state
 void report_gcode_modes();
diff --git a/settings.h b/settings.h
index 3551ec2..62dfcfc 100644
--- a/settings.h
+++ b/settings.h
@@ -25,8 +25,8 @@
 #include "system.h"
 
 
-#define GRBL_VERSION "0.9c"
-#define GRBL_VERSION_BUILD "20140215"
+#define GRBL_VERSION "0.9d"
+#define GRBL_VERSION_BUILD "20140228"
 
 // 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/stepper.c b/stepper.c
index 370d798..58ec9c1 100644
--- a/stepper.c
+++ b/stepper.c
@@ -24,6 +24,7 @@
 #include "stepper.h"
 #include "settings.h"
 #include "planner.h"
+#include "probe.h"
 
 
 // Some useful constants.
@@ -282,12 +283,6 @@ 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);
@@ -351,6 +346,10 @@ ISR(TIMER1_COMPA_vect)
       return; // Nothing to do but exit.
     }  
   }
+  
+  
+  // Check probing state.
+  probe_state_monitor();
    
   // Reset step out bits.
   st.step_outbits = 0; 
diff --git a/system.c b/system.c
index fe29523..25db0b6 100644
--- a/system.c
+++ b/system.c
@@ -49,12 +49,7 @@ 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
-      }
-    }
+    } 
   }
 }
 
@@ -91,14 +86,10 @@ uint8_t system_execute_line(char *line)
   float parameter, value;
   switch( line[char_counter] ) {
     case 0 : report_grbl_help(); break;
-    case '#' : // Print gcode parameters
-      if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); }
-      else { report_gcode_parameters(); }
-      break;
     case 'G' : // Prints gcode parser state
       if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); }
       else { report_gcode_modes(); }
-      break;
+      break;   
     case 'C' : // Set check g-code mode [IDLE/CHECK]
       if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); }
       // Perform reset when toggling off. Check g-code mode should only work if Grbl
@@ -141,6 +132,10 @@ uint8_t system_execute_line(char *line)
           if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); }
           else { report_grbl_settings(); }
           break;
+        case '#' : // Print Grbl NGC parameters
+          if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); }
+          else { report_ngc_parameters(); }
+          break;          
         case 'H' : // Perform homing cycle [IDLE/ALARM]
           if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { 
             // Only perform homing if Grbl is idle or lost.
diff --git a/system.h b/system.h
index 0b84eab..2b8caac 100644
--- a/system.h
+++ b/system.h
@@ -67,12 +67,6 @@
 #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 {
@@ -83,8 +77,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.   
+  volatile uint8_t probe_state;   // Probing state value.  Used to coordinate the probing cycle with stepper ISR.
+  int32_t probe_position[N_AXIS]; // Last probe position in machine coordinates and steps.
 } system_t;
 extern system_t sys;