diff --git a/gcode.c b/gcode.c index 27ed4e4..9d0f81d 100644 --- a/gcode.c +++ b/gcode.c @@ -124,7 +124,7 @@ uint8_t gc_execute_line(char *line) float value; uint8_t int_value = 0; uint8_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,22 +210,21 @@ 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: - sys.probe_away = false; + case 20: // G38.2 gc_block.modal.motion = MOTION_MODE_PROBE; - break; // G38.2 - case 30: - sys.probe_away = false; - gc_block.modal.motion = MOTION_MODE_PROBE_NO_ERROR; - break; // G38.3 - case 40: - sys.probe_away = true; + break; + case 30: // G38.3 gc_block.modal.motion = MOTION_MODE_PROBE; - break; // G38.4 - case 50: - sys.probe_away = true; - gc_block.modal.motion = MOTION_MODE_PROBE_NO_ERROR; - break; // G38.5 + 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] } @@ -982,13 +981,12 @@ uint8_t gc_execute_line(char *line) #endif break; case MOTION_MODE_PROBE: - case MOTION_MODE_PROBE_NO_ERROR: // 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 fc28d32..98efe23 100644 --- a/gcode.h +++ b/gcode.h @@ -71,9 +71,8 @@ #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, G38.4 -#define MOTION_MODE_PROBE_NO_ERROR 5 // G38.3, G38.5 -#define MOTION_MODE_NONE 6 // G80 +#define MOTION_MODE_PROBE 4 // G38.2, G38.3, G38.4, G38.5 +#define MOTION_MODE_NONE 5 // G80 // Modal Group G2: Plane select #define PLANE_SELECT_XY 0 // G17 (Default: Must be zero) diff --git a/motion_control.c b/motion_control.c index 01fa0f2..b069267 100644 --- a/motion_control.c +++ b/motion_control.c @@ -276,9 +276,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. @@ -289,7 +289,7 @@ void mc_homing_cycle() uint8_t auto_start_state = sys.auto_start; // Store run state // After syncing, check if probe is already triggered. If so, halt and issue alarm. - if ((sys.probe_away << PROBE_BIT) ^ probe_get_state()) { + if (probe_get_state(mode)) { bit_true_atomic(sys.execute, EXEC_CRIT_EVENT); protocol_execute_runtime(); } @@ -303,7 +303,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); @@ -314,6 +314,7 @@ void mc_homing_cycle() // 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) { bit_true_atomic(sys.execute, EXEC_CRIT_EVENT); } protocol_execute_runtime(); // Check and execute run-time commands if (sys.abort) { return; } // Check for system abort diff --git a/motion_control.h b/motion_control.h index 5a1d8c9..54aa1e5 100644 --- a/motion_control.h +++ b/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. diff --git a/probe.c b/probe.c index 581f9f1..5a65939 100644 --- a/probe.c +++ b/probe.c @@ -37,22 +37,22 @@ void probe_init() PROBE_PORT |= PROBE_MASK; // Enable internal pull-up resistors. Normal high operation. probe_invert_mask = PROBE_MASK; } - - sys.probe_away = false; } // 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); +} // 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 ((sys.probe_away << PROBE_BIT) ^ probe_get_state()) { + if (sys.probe_state != PROBE_OFF) { + if (probe_get_state(sys.probe_state)) { sys.probe_state = PROBE_OFF; memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS); bit_true(sys.execute, EXEC_FEED_HOLD); diff --git a/probe.h b/probe.h index 81fb55b..5986d86 100644 --- a/probe.h +++ b/probe.h @@ -25,12 +25,18 @@ #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); // Monitors probe pin state and records the system position when detected. Called by the // stepper ISR per ISR tick. diff --git a/system.h b/system.h index ddd7625..93ab2b9 100644 --- a/system.h +++ b/system.h @@ -79,7 +79,6 @@ 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_away; // probe away from work by reversing the switch direction (G38.4, G38.5) } system_t; extern system_t sys;