Merge pull request #491 from tmpvar/G38.2+

G38.2+

gcode.c

@@ -123,8 +123,8 @@ uint8_t gc_execute_line(char *line)
   char letter;
   float value;
   uint8_t int_value = 0;
-  uint8_t mantissa = 0; // NOTE: For mantissa values > 255, variable type must be changed to uint16_t.
-
+  uint16_t mantissa = 0; // NOTE: For mantissa values > 255, variable type must be changed to uint16_t.
+  uint8_t probe_mode = 0;
 
   while (line[char_counter] != 0) { // Loop until no more g-code words in line.
     
@@ -210,11 +210,22 @@ uint8_t gc_execute_line(char *line)
               case 3: gc_block.modal.motion = MOTION_MODE_CCW_ARC; break; // G3
               case 38: 
                 switch(mantissa) {
-                  case 20: gc_block.modal.motion = MOTION_MODE_PROBE; break;  // G38.2
-                  // NOTE: If G38.3+ are enabled, change mantissa variable type to uint16_t.
-                  // case 30: gc_block.modal.motion = MOTION_MODE_PROBE_NO_ERROR; break; // G38.3 Not supported.
-                  // case 40: // Not supported.
-                  // case 50: // Not supported.
+                  case 20:  // G38.2
+                    gc_block.modal.motion = MOTION_MODE_PROBE;
+                    break;
+                  case 30:  // G38.3
+                    gc_block.modal.motion = MOTION_MODE_PROBE;
+                    probe_mode = PROBE_NO_ERROR;
+                    break;
+                  case 40:  // G38.4
+                    gc_block.modal.motion = MOTION_MODE_PROBE;
+                    probe_mode = PROBE_AWAY;
+                    break;
+                  case 50:  // G38.5
+                    gc_block.modal.motion = MOTION_MODE_PROBE;
+                    probe_mode = PROBE_AWAY | PROBE_NO_ERROR;
+                    break;
+
                   default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G38.x command]
                 }
                 mantissa = 0; // Set to zero to indicate valid non-integer G command.
@@ -973,9 +984,9 @@ uint8_t gc_execute_line(char *line)
           // NOTE: gc_block.values.xyz is returned from mc_probe_cycle with the updated position value. So
           // upon a successful probing cycle, the machine position and the returned value should be the same.
           #ifdef USE_LINE_NUMBERS
-            mc_probe_cycle(gc_block.values.xyz, gc_state.feed_rate, gc_state.modal.feed_rate, gc_block.values.n);
+            mc_probe_cycle(gc_block.values.xyz, gc_state.feed_rate, gc_state.modal.feed_rate, probe_mode, gc_block.values.n);
           #else
-            mc_probe_cycle(gc_block.values.xyz, gc_state.feed_rate, gc_state.modal.feed_rate);
+            mc_probe_cycle(gc_block.values.xyz, gc_state.feed_rate, gc_state.modal.feed_rate, probe_mode);
           #endif
       }
     

gcode.h

@@ -71,7 +71,7 @@
 #define MOTION_MODE_LINEAR 1 // G1
 #define MOTION_MODE_CW_ARC 2  // G2
 #define MOTION_MODE_CCW_ARC 3  // G3
-#define MOTION_MODE_PROBE 4 // G38.2
+#define MOTION_MODE_PROBE 4 // G38.2, G38.3, G38.4, G38.5
 #define MOTION_MODE_NONE 5 // G80
 
 // Modal Group G2: Plane select

motion_control.c

@@ -287,9 +287,9 @@ 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
-  void 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, uint8_t mode, int32_t line_number)
 #else
-  void 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, uint8_t mode)
 #endif
 { 
   // TODO: Need to update this cycle so it obeys a non-auto cycle start.
@@ -298,9 +298,10 @@ void mc_homing_cycle()
   // Finish all queued commands and empty planner buffer before starting probe cycle.
   protocol_buffer_synchronize();
   uint8_t auto_start_state = sys.auto_start; // Store run state
+  uint8_t perform_pull_off = 1;
 
   // After syncing, check if probe is already triggered. If so, halt and issue alarm.
-  if (probe_get_state()) { 
+  if (probe_get_state(mode) && probe_errors_enabled(mode)) {
     bit_true_atomic(sys.execute, EXEC_CRIT_EVENT);
     protocol_execute_runtime();
   }
@@ -314,7 +315,7 @@ void mc_homing_cycle()
   #endif
   
   // Activate the probing monitor in the stepper module.
-  sys.probe_state = PROBE_ACTIVE;
+  sys.probe_state = PROBE_ACTIVE | mode;
 
   // Perform probing cycle. Wait here until probe is triggered or motion completes.
   bit_true_atomic(sys.execute, EXEC_CYCLE_START);
@@ -324,7 +325,15 @@ void mc_homing_cycle()
   } while ((sys.state != STATE_IDLE) && (sys.state != STATE_QUEUED));
 
   // Probing motion complete. If the probe has not been triggered, error out.
-  if (sys.probe_state == PROBE_ACTIVE) { bit_true_atomic(sys.execute, EXEC_CRIT_EVENT); }
+  if (sys.probe_state & PROBE_ACTIVE) {
+
+    if (probe_errors_enabled(mode)) {
+      bit_true_atomic(sys.execute, EXEC_CRIT_EVENT);
+    } else {
+      perform_pull_off = 0;
+      probe_finalize(0);
+    }
+  }
   protocol_execute_runtime();   // Check and execute run-time commands
   if (sys.abort) { return; } // Check for system abort
 
@@ -333,6 +342,7 @@ void mc_homing_cycle()
   plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared.
   plan_sync_position(); // Sync planner position to current machine position.
 
+  if (perform_pull_off) {
     // Pull-off triggered probe to the trigger location since we had to decelerate a little beyond
     // it to stop the machine in a controlled manner.
     uint8_t idx;
@@ -350,6 +360,7 @@ void mc_homing_cycle()
     bit_true_atomic(sys.execute, EXEC_CYCLE_START);
     protocol_buffer_synchronize();
     if (sys.abort) { return; } // Return if system reset has been issued.
+  }
 
   sys.auto_start = auto_start_state; // Restore run state before returning
 

motion_control.h

@@ -58,9 +58,9 @@ void mc_homing_cycle();
 
 // Perform tool length probe cycle. Requires probe switch.
 #ifdef USE_LINE_NUMBERS
-void 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, uint8_t motion, int32_t line_number);
 #else
-void 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, uint8_t motion);
 #endif
 
 // Performs system reset. If in motion state, kills all motion and sets system alarm.

probe.c

@@ -41,19 +41,30 @@ void probe_init()
 
 
 // Returns the probe pin state. Triggered = true. Called by gcode parser and probe state monitor.
-uint8_t probe_get_state() { return((PROBE_PIN & PROBE_MASK) ^ probe_invert_mask); }
+uint8_t probe_get_state(uint8_t mode) {
+  mode = ((mode >> PROBE_AWAY_BIT) & 1) << PROBE_BIT;
+  return mode ^ ((PROBE_PIN & PROBE_MASK) ^ probe_invert_mask);
+}
 
+uint8_t probe_errors_enabled(uint8_t mode) {
+  return !(mode & PROBE_NO_ERROR);
+}
+
+void probe_finalize(uint8_t probe_succeeded) {
+  sys.probe_state = PROBE_OFF;
+  sys.probe_succeeded = probe_succeeded;
+  memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS);
+  bit_true(sys.execute, EXEC_FEED_HOLD);
+}
 
 // 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_get_state()) {
-      sys.probe_state = PROBE_OFF;
-      memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS);
-      bit_true(sys.execute, EXEC_FEED_HOLD);
+  if (sys.probe_state != PROBE_OFF) {
+    if (probe_get_state(sys.probe_state)) {
+      probe_finalize(1);
     }
   }
 }

probe.h

@@ -25,12 +25,22 @@
 #define PROBE_OFF     0 // No probing. (Must be zero.)
 #define PROBE_ACTIVE  1 // Actively watching the input pin.
 
+// Probe direction and error modes
+#define PROBE_AWAY 2 // G38.4, G38.5
+#define PROBE_NO_ERROR 4 // G38.3, G38.5
+
+#define PROBE_AWAY_BIT 1
+#define PROBE_NO_ERROR_BIT 2
 
 // Probe pin initialization routine.
 void probe_init();
 
 // Returns probe pin state.
-uint8_t probe_get_state();
+uint8_t probe_get_state(uint8_t mode);
+
+uint8_t probe_errors_enabled(uint8_t mode);
+
+void probe_finalize(uint8_t probe_succeeded);
 
 // Monitors probe pin state and records the system position when detected. Called by the
 // stepper ISR per ISR tick.

report.c

@@ -236,6 +236,8 @@ void report_probe_parameters()
     printFloat_CoordValue(print_position[i]);
     if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); }
   }
+  printPgmString(PSTR(":"));
+  print_uint8_base10(sys.probe_succeeded);
   printPgmString(PSTR("]\r\n"));
 }
 

system.h

@@ -79,6 +79,7 @@ typedef struct {
   uint8_t auto_start;            // Planner auto-start flag. Toggled off during feed hold. Defaulted by settings.
   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.
+  uint8_t probe_succeeded;
 } system_t;
 extern system_t sys;