From 1be772c1ba27433c42b5c492e1eff8b0a214b8e3 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Mon, 28 Jun 2021 10:48:32 -1000 Subject: [PATCH] Handle macros in protocol real time handler --- Grbl_Esp32/data/config.yaml | 3 +++ Grbl_Esp32/src/ControlPin.cpp | 3 --- Grbl_Esp32/src/Grbl.h | 2 -- Grbl_Esp32/src/Machine/Macros.h | 33 ++++++++++++++++++++++++++++-- Grbl_Esp32/src/ProcessSettings.cpp | 10 ++++++--- Grbl_Esp32/src/Protocol.cpp | 27 ++++++++++++++++++++++++ Grbl_Esp32/src/System.cpp | 23 --------------------- 7 files changed, 68 insertions(+), 33 deletions(-) diff --git a/Grbl_Esp32/data/config.yaml b/Grbl_Esp32/data/config.yaml index c499822b..9780d61a 100644 --- a/Grbl_Esp32/data/config.yaml +++ b/Grbl_Esp32/data/config.yaml @@ -87,3 +87,6 @@ PWM: spindown_ms: 1000 speeds: 0=0% 1000=100% tool: 0 + +macros: + n0: G0 X0 diff --git a/Grbl_Esp32/src/ControlPin.cpp b/Grbl_Esp32/src/ControlPin.cpp index 55e94c62..671c43f3 100644 --- a/Grbl_Esp32/src/ControlPin.cpp +++ b/Grbl_Esp32/src/ControlPin.cpp @@ -3,9 +3,6 @@ #include "Report.h" // addPinReport #include // IRAM_ATTR -// XXX we need to dispatch the user defined macros somehow -// user_defined_macro(N) - void IRAM_ATTR ControlPin::handleISR() { bool pinState = _pin.read(); _value = pinState; diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 0a33e4c9..b0673df7 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -48,8 +48,6 @@ bool limitsCheckTravel(float* target); // weak in Limits.cpp; true if out of ra void inverse_kinematics(float* position); // used to return a converted value void forward_kinematics(float* position); -void user_defined_macro(uint8_t index); - void user_m30(); void user_tool_change(uint8_t new_tool); diff --git a/Grbl_Esp32/src/Machine/Macros.h b/Grbl_Esp32/src/Machine/Macros.h index 5db60aae..503a498a 100644 --- a/Grbl_Esp32/src/Machine/Macros.h +++ b/Grbl_Esp32/src/Machine/Macros.h @@ -19,6 +19,7 @@ */ #include "../Configuration/Configurable.h" +#include "../WebUI/InputBuffer.h" // WebUI::inputBuffer namespace Machine { class Macros : public Configuration::Configurable { @@ -33,8 +34,36 @@ namespace Machine { public: Macros() = default; - String macro(size_t index) { return index < n_macros ? _macro[index] : ""; } - String startup_line(size_t index) { return index < n_startup_lines ? _startup_line[index] : ""; } + void run_macro(size_t index) { + if (index >= n_macros) { + return; + } + String macro = _macro[index]; + if (macro == "") { + return; + } + + // & is a proxy for newlines in macros, because you cannot + // enter a newline directly in a config file string value. + macro.replace('&', '\n'); + macro += "\n"; + + WebUI::inputBuffer.push(macro.c_str()); + } + + String startup_line(size_t index) { + if (index >= n_startup_lines) { + return ""; + } + String s = _startup_line[index]; + if (s == "") { + return s; + } + // & is a proxy for newlines in startup lines, because you cannot + // enter a newline directly in a config file string value. + s.replace('&', '\n'); + return s + "\n"; + } // Configuration helpers: diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index 99a47011..5a82bb7e 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -684,10 +684,14 @@ Error system_execute_line(char* line, uint8_t client, WebUI::AuthenticationLevel void system_execute_startup() { Error status_code; - char gcline[256]; for (int i = 0; i < config->_macros->n_startup_lines; i++) { - strncpy(gcline, config->_macros->startup_line(i).c_str(), 255); - if (*gcline) { + String str = config->_macros->startup_line(i); + const char* s = str.c_str(); + if (s && strlen(s)) { + // We have to copy this to a mutable array because + // gc_execute_line modifies the line while parsing. + char gcline[256]; + strncpy(gcline, s, 255); status_code = gc_execute_line(gcline, CLIENT_SERIAL); report_execute_startup_message(gcline, status_code, CLIENT_SERIAL); } diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index c978685f..3ae349b1 100644 --- a/Grbl_Esp32/src/Protocol.cpp +++ b/Grbl_Esp32/src/Protocol.cpp @@ -646,6 +646,16 @@ static void protocol_execute_overrides() { } } +void protocol_do_macro(int macro_num) { + // must be in Idle + if (sys.state != State::Idle) { + info_serial("Macro button only permitted in idle"); + return; + } + + config->_macros->run_macro(macro_num); +} + void protocol_exec_rt_system() { protocol_do_alarm(); // If there is a hard or soft limit, this will block until rtReset is set @@ -687,6 +697,23 @@ void protocol_exec_rt_system() { protocol_do_cycle_stop(); } + if (rtButtonMacro0) { + protocol_do_macro(0); + rtButtonMacro0 = false; + } + if (rtButtonMacro1) { + protocol_do_macro(1); + rtButtonMacro1 = false; + } + if (rtButtonMacro2) { + protocol_do_macro(2); + rtButtonMacro0 = false; + } + if (rtButtonMacro3) { + protocol_do_macro(3); + rtButtonMacro3 = false; + } + protocol_execute_overrides(); #ifdef DEBUG_PROTOCOL diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index ca28c5be..e3c0ad7b 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -26,7 +26,6 @@ #include "UserOutput.h" #include "SettingsDefinitions.h" #include "Machine/MachineConfig.h" -#include "WebUI/InputBuffer.h" // WebUI::inputBuffer #include #include // memset @@ -181,28 +180,6 @@ uint8_t sys_calc_pwm_precision(uint32_t freq) { return precision - 1; } -void __attribute__((weak)) user_defined_macro(uint8_t index) { - // must be in Idle - if (sys.state != State::Idle) { - info_serial("Macro button only permitted in idle"); - return; - } - - String user_macro = config->_macros->macro(index); - - if (user_macro == "") { - info_serial("User/Macro%d empty", index); - return; - } - - char line[255]; - - user_macro.replace('&', '\n'); - user_macro.toCharArray(line, 255, 0); - strcat(line, "\r"); - WebUI::inputBuffer.push(line); -} - std::map StateName = { { State::Idle, "Idle" }, { State::Alarm, "Alarm" },