diff --git a/gcode.c b/gcode.c index 1977d83..4afa027 100644 --- a/gcode.c +++ b/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 } diff --git a/gcode.h b/gcode.h index 81ae4d9..0e196a9 100644 --- a/gcode.h +++ b/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 diff --git a/motion_control.c b/motion_control.c index 66e9701..ed62d33 100644 --- a/motion_control.c +++ b/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 @@ -332,24 +341,26 @@ void mc_homing_cycle() st_reset(); // Reest step segment buffer. plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared. plan_sync_position(); // Sync planner position to current machine position. - - // 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; - for(idx=0; idx> 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); } } } diff --git a/probe.h b/probe.h index 81fb55b..17028eb 100644 --- a/probe.h +++ b/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. diff --git a/report.c b/report.c index 92033b3..9235f37 100644 --- a/report.c +++ b/report.c @@ -235,7 +235,9 @@ void report_probe_parameters() print_position[i] = sys.probe_position[i]/settings.steps_per_mm[i]; printFloat_CoordValue(print_position[i]); if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); } - } + } + printPgmString(PSTR(":")); + print_uint8_base10(sys.probe_succeeded); printPgmString(PSTR("]\r\n")); } diff --git a/system.h b/system.h index 93ab2b9..3263af8 100644 --- a/system.h +++ b/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;