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;