From 885f57e9701fabca0f001e1583cf9e4d729d2ba5 Mon Sep 17 00:00:00 2001 From: bdring Date: Sun, 4 Oct 2020 17:22:30 -0500 Subject: [PATCH] Probe cleanup (#625) * Cleanup probing code * Update Grbl.h * Update after review --- Grbl_Esp32/src/Defaults.h | 4 +++ Grbl_Esp32/src/GCode.cpp | 9 ++++--- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/MotionControl.cpp | 12 ++++----- Grbl_Esp32/src/Probe.cpp | 45 ++++++++++++++------------------ Grbl_Esp32/src/Probe.h | 8 +++--- Grbl_Esp32/src/Settings.cpp | 4 +++ 7 files changed, 41 insertions(+), 43 deletions(-) diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 1fd2241b..f75812a0 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -599,6 +599,10 @@ # define USER_ANALOG_PIN_3 UNDEFINED_PIN #endif +#ifndef PROBE_PIN +# define PROBE_PIN UNDEFINED_PIN +#endif + #ifndef USER_ANALOG_PIN_0_FREQ # define USER_ANALOG_PIN_0_FREQ 5000 #endif diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 8031c95e..0761eafd 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -271,10 +271,11 @@ Error gc_execute_line(char* line, uint8_t client) { mg_word_bit = ModalGroup::MG1; break; case 38: // G38 - probe -#ifndef PROBE_PIN //only allow G38 "Probe" commands if a probe pin is defined. - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "No probe pin defined"); - FAIL(Error::GcodeUnsupportedCommand); // [Unsupported G command] -#endif + //only allow G38 "Probe" commands if a probe pin is defined. + if (PROBE_PIN == UNDEFINED_PIN) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "No probe pin defined"); + FAIL(Error::GcodeUnsupportedCommand); // [Unsupported G command] + } // Check for G0/1/2/3/38 being called with G10/28/30/92 on same block. // * G43.1 is also an axis command but is not explicitly defined this way. if (axis_command != AxisCommand::None) { diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 860c2021..2f3cbf92 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -23,7 +23,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20201001"; +const char* const GRBL_VERSION_BUILD = "20201004"; //#include #include diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index d122529f..33238736 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -375,14 +375,13 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par uint8_t is_probe_away = bit_istrue(parser_flags, GCParserProbeIsAway); uint8_t is_no_error = bit_istrue(parser_flags, GCParserProbeIsNoError); sys.probe_succeeded = false; // Re-initialize probe history before beginning cycle. - probe_configure_invert_mask(is_probe_away); + set_probe_direction(is_probe_away); // After syncing, check if probe is already triggered. If so, halt and issue alarm. // NOTE: This probe initialization error applies to all probing cycles. - if (probe_get_state()) { // Check probe pin state. + if (probe_get_state() ^ is_probe_away) { // Check probe pin state. system_set_exec_alarm(ExecAlarm::ProbeFailInitial); protocol_execute_realtime(); - probe_configure_invert_mask(false); // Re-initialize invert mask before returning. - return GCUpdatePos::None; // Nothing else to do but bail. + return GCUpdatePos::None; // Nothing else to do but bail. } // Setup and queue probing motion. Auto cycle-start should not start the cycle. mc_line(target, pl_data); @@ -407,9 +406,8 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par } else { sys.probe_succeeded = true; // Indicate to system the probing cycle completed successfully. } - sys_probe_state = PROBE_OFF; // Ensure probe state monitor is disabled. - probe_configure_invert_mask(false); // Re-initialize invert mask. - protocol_execute_realtime(); // Check and execute run-time commands + sys_probe_state = PROBE_OFF; // Ensure probe state monitor is disabled. + protocol_execute_realtime(); // Check and execute run-time commands // Reset the stepper and planner buffers to remove the remainder of the probe motion. st_reset(); // Reset step segment buffer. plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared. diff --git a/Grbl_Esp32/src/Probe.cpp b/Grbl_Esp32/src/Probe.cpp index 681b8821..058b936e 100644 --- a/Grbl_Esp32/src/Probe.cpp +++ b/Grbl_Esp32/src/Probe.cpp @@ -24,47 +24,40 @@ #include "Grbl.h" // Inverts the probe pin state depending on user settings and probing cycle mode. -uint8_t probe_invert_mask; +static bool is_probe_away; // Probe pin initialization routine. void probe_init() { -#ifdef PROBE_PIN -# ifdef DISABLE_PROBE_PIN_PULL_UP - pinMode(PROBE_PIN, INPUT); -# else - pinMode(PROBE_PIN, INPUT_PULLUP); // Enable internal pull-up resistors. Normal high operation. -# endif - probe_configure_invert_mask(false); // Initialize invert mask. + static bool show_init_msg = true; // used to show message only once. + + if (PROBE_PIN != UNDEFINED_PIN) { +#ifdef DISABLE_PROBE_PIN_PULL_UP + pinMode(PROBE_PIN, INPUT); +#else + pinMode(PROBE_PIN, INPUT_PULLUP); // Enable internal pull-up resistors. Normal high operation. #endif + + if (show_init_msg) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Probe on pin %s", pinName(PROBE_PIN).c_str()); + show_init_msg = false; + } + } } -// Called by probe_init() and the mc_probe() routines. Sets up the probe pin invert mask to -// appropriately set the pin logic according to setting for normal-high/normal-low operation -// and the probing cycle modes for toward-workpiece/away-from-workpiece. -void probe_configure_invert_mask(uint8_t is_probe_away) { - probe_invert_mask = 0; // Initialize as zero. - if (probe_invert->get()) { - probe_invert_mask ^= PROBE_MASK; - } - if (is_probe_away) { - probe_invert_mask ^= PROBE_MASK; - } +void set_probe_direction(bool is_away) { + is_probe_away = is_away; } // Returns the probe pin state. Triggered = true. Called by gcode parser and probe state monitor. -uint8_t probe_get_state() { -#ifdef PROBE_PIN - return digitalRead(PROBE_PIN) ^ probe_invert_mask; -#else - return false; -#endif +bool probe_get_state() { + return digitalRead(PROBE_PIN) ^ probe_invert->get(); } // 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 (probe_get_state()) { + if (probe_get_state() ^ is_probe_away) { sys_probe_state = PROBE_OFF; memcpy(sys_probe_position, sys_position, sizeof(sys_position)); bit_true(sys_rt_exec_state, EXEC_MOTION_CANCEL); diff --git a/Grbl_Esp32/src/Probe.h b/Grbl_Esp32/src/Probe.h index 2cc5bdf2..f1612c93 100644 --- a/Grbl_Esp32/src/Probe.h +++ b/Grbl_Esp32/src/Probe.h @@ -30,13 +30,11 @@ const int PROBE_ACTIVE = 1; // Actively watching the input pin. // Probe pin initialization routine. void probe_init(); -// Called by probe_init() and the mc_probe() routines. Sets up the probe pin invert mask to -// appropriately set the pin logic according to setting for normal-high/normal-low operation -// and the probing cycle modes for toward-workpiece/away-from-workpiece. -void probe_configure_invert_mask(uint8_t is_probe_away); +// setup probing direction G38.2 vs. G38.4 +void set_probe_direction(bool is_away); // Returns probe pin state. Triggered = true. Called by gcode parser and probe state monitor. -uint8_t probe_get_state(); +bool probe_get_state(); // Monitors probe pin state and records the system position when detected. Called by the // stepper ISR per ISR tick. diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index f1be2536..e53c313c 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -537,6 +537,10 @@ void FlagSetting::setDefault() { Error FlagSetting::setStringValue(char* s) { s = trim(s); + Error err = check(s); + if (err != Error::Ok) { + return err; + } _currentValue = (strcasecmp(s, "on") == 0) || (strcasecmp(s, "true") == 0) || (strcasecmp(s, "enabled") == 0) || (strcasecmp(s, "yes") == 0) || (strcasecmp(s, "1") == 0); // _storedValue is -1, 0, or 1