From 0c9c237e5c8dc89c62cff3f2d52ced7eea2e6bad Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Mon, 24 Jul 2023 10:11:55 -1000 Subject: [PATCH 01/11] First cut --- FluidNC/src/Control.cpp | 2 +- FluidNC/src/CoolantControl.cpp | 2 +- FluidNC/src/Limits.cpp | 36 +++-------------- FluidNC/src/Machine/Homing.cpp | 9 ++--- FluidNC/src/Machine/LimitPin.cpp | 1 - FluidNC/src/MotionControl.cpp | 32 ++------------- FluidNC/src/MotionControl.h | 2 +- FluidNC/src/ProcessSettings.cpp | 3 +- FluidNC/src/Protocol.cpp | 62 ++++++++++++----------------- FluidNC/src/Protocol.h | 3 +- FluidNC/src/Report.cpp | 1 + FluidNC/src/Report.h | 2 +- FluidNC/src/Serial.cpp | 2 +- FluidNC/src/Spindles/VFDSpindle.cpp | 8 ++-- FluidNC/src/Types.h | 1 + 15 files changed, 52 insertions(+), 114 deletions(-) diff --git a/FluidNC/src/Control.cpp b/FluidNC/src/Control.cpp index 96fcd879b..2e8848405 100644 --- a/FluidNC/src/Control.cpp +++ b/FluidNC/src/Control.cpp @@ -9,7 +9,7 @@ Control::Control() { // The SafetyDoor pin must be defined first because it is checked explicity in safety_door_ajar() _pins.push_back(new ControlPin(&safetyDoorEvent, "safety_door_pin", 'D')); - _pins.push_back(new ControlPin(&resetEvent, "reset_pin", 'R')); + _pins.push_back(new ControlPin(&rtResetEvent, "reset_pin", 'R')); _pins.push_back(new ControlPin(&feedHoldEvent, "feed_hold_pin", 'H')); _pins.push_back(new ControlPin(&cycleStartEvent, "cycle_start_pin", 'S')); _pins.push_back(new ControlPin(¯o0Event, "macro0_pin", '0')); diff --git a/FluidNC/src/CoolantControl.cpp b/FluidNC/src/CoolantControl.cpp index eb96ec1d3..7daa6d1a1 100644 --- a/FluidNC/src/CoolantControl.cpp +++ b/FluidNC/src/CoolantControl.cpp @@ -55,7 +55,7 @@ void CoolantControl::write(CoolantState state) { } } -// Directly called by coolant_init(), coolant_set_state(), and mc_reset(), which can be at +// Directly called by coolant_init(), coolant_set_state(), which can be at // an interrupt-level. No report flag set, but only called by routines that don't need it. void CoolantControl::stop() { CoolantState disable = {}; diff --git a/FluidNC/src/Limits.cpp b/FluidNC/src/Limits.cpp index 526429bc4..1750e8a4d 100644 --- a/FluidNC/src/Limits.cpp +++ b/FluidNC/src/Limits.cpp @@ -5,14 +5,14 @@ #include "Limits.h" #include "Machine/MachineConfig.h" -#include "MotionControl.h" // mc_reset +#include "MotionControl.h" // mc_critical #include "System.h" // sys.* #include "Protocol.h" // protocol_execute_realtime #include "Platform.h" // WEAK_LINK #include #include -#include // fence +#include // fence xQueueHandle limit_sw_queue; // used by limit switch debouncing @@ -43,10 +43,10 @@ MotorMask limits_get_state() { return Machine::Axes::posLimitMask | Machine::Axes::negLimitMask; } +// Called only from Kinematics canHome() methods, hence from states allowing homing bool ambiguousLimit() { if (Machine::Axes::posLimitMask & Machine::Axes::negLimitMask) { - mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. - rtAlarm = ExecAlarm::HardLimit; + mc_critical(ExecAlarm::HardLimit); return true; } return false; @@ -85,7 +85,7 @@ void constrainToSoftLimits(float* cartesian) { // if jog is positive and only the positive switch is active, then kill the move // if jog is negative and only the negative switch is active, then kill the move - if (posLimited != negLimited) { // XOR, because ambiguous (both) is OK + if (posLimited != negLimited) { // XOR, because ambiguous (both) is OK if ((negLimited && (jog_dist < 0)) || (posLimited && (jog_dist > 0))) { cartesian[axis] = current_position[axis]; // cancel the move on this axis log_debug("Jog into active switch blocked on " << Machine::Axes::_names[axis]); @@ -144,34 +144,10 @@ void limits_soft_check(float* cartesian) { } while (sys.state != State::Idle); } log_debug("Soft limits"); - mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. - rtAlarm = ExecAlarm::SoftLimit; // Indicate soft limit critical event - protocol_execute_realtime(); // Execute to enter critical event loop and system abort + mc_critical(ExecAlarm::SoftLimit); } } -#ifdef LATER // We need to rethink debouncing -void limitCheckTask(void* pvParameters) { - while (true) { - std::atomic_thread_fence(std::memory_order::memory_order_seq_cst); // read fence for settings - - int evt; - xQueueReceive(limit_sw_queue, &evt, portMAX_DELAY); // block until receive queue - vTaskDelay(config->_softwareDebounceMs / portTICK_PERIOD_MS); // delay a while - auto switch_state = limits_get_state(); - if (switch_state) { - log_debug("Limit Switch State " << to_hex(switch_state)); - mc_reset(); // Initiate system kill. - rtAlarm = ExecAlarm::HardLimit; // Indicate hard limit critical event - } - static UBaseType_t uxHighWaterMark = 0; -# ifdef DEBUG_TASK_STACK - reportTaskStackSize(uxHighWaterMark); -# endif - } -} -#endif - float limitsMaxPosition(size_t axis) { auto axisConfig = config->_axes->_axis[axis]; auto homing = axisConfig->_homing; diff --git a/FluidNC/src/Machine/Homing.cpp b/FluidNC/src/Machine/Homing.cpp index d4e00ec4e..bfbad79de 100644 --- a/FluidNC/src/Machine/Homing.cpp +++ b/FluidNC/src/Machine/Homing.cpp @@ -1,10 +1,9 @@ #include "Homing.h" -#include "../MotionControl.h" // mc_reset -#include "../System.h" // sys.* -#include "../Stepper.h" // st_wake -#include "../Protocol.h" // protocol_handle_events -#include "../Limits.h" // ambiguousLimit +#include "../System.h" // sys.* +#include "../Stepper.h" // st_wake +#include "../Protocol.h" // protocol_handle_events +#include "../Limits.h" // ambiguousLimit #include "../Machine/Axes.h" #include "../Machine/MachineConfig.h" // config diff --git a/FluidNC/src/Machine/LimitPin.cpp b/FluidNC/src/Machine/LimitPin.cpp index c4a10008b..35c6dfecd 100644 --- a/FluidNC/src/Machine/LimitPin.cpp +++ b/FluidNC/src/Machine/LimitPin.cpp @@ -2,7 +2,6 @@ #include "src/Machine/Axes.h" #include "src/Machine/MachineConfig.h" // config -#include "src/MotionControl.h" // mc_reset #include "src/Limits.h" #include "src/Protocol.h" // protocol_send_event_from_ISR() diff --git a/FluidNC/src/MotionControl.cpp b/FluidNC/src/MotionControl.cpp index 9c986f8dd..95aede1a1 100644 --- a/FluidNC/src/MotionControl.cpp +++ b/FluidNC/src/MotionControl.cpp @@ -102,9 +102,9 @@ void mc_cancel_jog() { // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. bool mc_linear(float* target, plan_line_data_t* pl_data, float* position) { - if (!pl_data->is_jog) { // soft limits for jogs have already been dealt with + if (!pl_data->is_jog) { // soft limits for jogs have already been dealt with limits_soft_check(target); - } + } return config->_kinematics->cartesian_to_motors(target, pl_data, position); } @@ -368,30 +368,6 @@ void mc_override_ctrl_update(Override override_state) { sys.override_ctrl = override_state; } -// Method to ready the system to reset by setting the realtime reset command and killing any -// active processes in the system. This also checks if a system reset is issued while in -// motion state. If so, kills the steppers and sets the system alarm to flag position -// lost, since there was an abrupt uncontrolled deceleration. Called at an interrupt level by -// realtime abort command and hard limits. So, keep to a minimum. Stuff that cannot be -// done quickly is handled later when Protocol.cpp responds to rtReset. -void mc_reset() { - // Only this function can set the system reset. Helps prevent multiple kill calls. - if (!rtReset) { - rtReset = true; - - // Kill steppers only if in any motion state, i.e. cycle, actively holding, or homing. - // NOTE: If steppers are kept enabled via the step idle delay setting, this also keeps - // the steppers enabled by avoiding the go_idle call altogether, unless the motion state is - // violated, by which, all bets are off. - if (inMotionState() || sys.step_control.executeHold || sys.step_control.executeSysMotion) { - if (sys.state == State::Homing) { - if (rtAlarm == ExecAlarm::None) { - rtAlarm = ExecAlarm::HomingFailReset; - } - } else { - rtAlarm = ExecAlarm::AbortCycle; - } - Stepper::stop_stepping(); // Stop stepping immediately, possibly losing position - } - } +void mc_critical(ExecAlarm alarm) { + rtAlarm = alarm; } diff --git a/FluidNC/src/MotionControl.h b/FluidNC/src/MotionControl.h index df51a154c..21a2539c9 100644 --- a/FluidNC/src/MotionControl.h +++ b/FluidNC/src/MotionControl.h @@ -56,7 +56,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, bool away, void mc_override_ctrl_update(Override override_state); // Performs system reset. If in motion state, kills all motion and sets system alarm. -void mc_reset(); +void mc_critical(ExecAlarm alarm); void mc_cancel_jog(); diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index 349ad9290..34de06103 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -253,9 +253,8 @@ static Error toggle_check_mode(const char* value, WebUI::AuthenticationLevel aut // idle and ready, regardless of alarm locks. This is mainly to keep things // simple and consistent. if (sys.state == State::CheckMode) { - log_debug("Check mode"); - mc_reset(); report_feedback_message(Message::Disabled); + sys.abort = true; } else { if (sys.state != State::Idle) { return Error::IdleError; // Requires no alarm mode. diff --git a/FluidNC/src/Protocol.cpp b/FluidNC/src/Protocol.cpp index ab7ce0f68..fabe57ed0 100644 --- a/FluidNC/src/Protocol.cpp +++ b/FluidNC/src/Protocol.cpp @@ -43,8 +43,6 @@ const char* alarmString(ExecAlarm alarmNumber) { return it == AlarmNames.end() ? NULL : it->second; } -volatile bool rtReset; - static volatile bool rtSafetyDoor; volatile bool runLimitLoop; // Interface to show_limits() @@ -74,12 +72,8 @@ static SpindleStop spindle_stop_ovr; void protocol_reset() { probeState = ProbeState::Off; soft_limit = false; - rtReset = false; rtSafetyDoor = false; spindle_stop_ovr.value = 0; - - // Do not clear rtAlarm because it might have been set during configuration - // rtAlarm = ExecAlarm::None; } static int32_t idleEndTime = 0; @@ -380,25 +374,22 @@ static void protocol_do_alarm() { if (rtAlarm == ExecAlarm::None) { return; } + if (inMotionState() || sys.step_control.executeHold || sys.step_control.executeSysMotion) { + Stepper::stop_stepping(); // Stop stepping immediately, possibly losing position + } if (spindle->_off_on_alarm) { spindle->stop(); } - sys.state = State::Alarm; // Set system alarm state alarm_msg(rtAlarm); if (rtAlarm == ExecAlarm::HardLimit || rtAlarm == ExecAlarm::SoftLimit || rtAlarm == ExecAlarm::HardStop) { + rtAlarm = ExecAlarm::None; + sys.state = State::Critical; // Set system alarm state report_error_message(Message::CriticalEvent); protocol_disable_steppers(); - rtReset = false; // Disable any existing reset - do { - protocol_handle_events(); - // Block everything except reset and status reports until user issues reset or power - // cycles. Hard limits typically occur while unattended or not paying attention. Gives - // the user and a GUI time to do what is needed before resetting, like killing the - // incoming stream. The same could be said about soft limits. While the position is not - // lost, continued streaming could cause a serious crash if by chance it gets executed. - } while (!rtReset); + return; } - rtAlarm = ExecAlarm::None; + rtAlarm = ExecAlarm::None; + sys.state = State::Alarm; } static void protocol_start_holding() { @@ -747,7 +738,7 @@ static void update_velocities() { plan_cycle_reinitialize(); } -// This is the final phase of the shutdown activity that is initiated by mc_reset(). +// This is the final phase of the shutdown activity that is initiated by mc_critical(). // The stuff herein is not necessarily safe to do in an ISR. static void protocol_do_late_reset() { // Kill spindle and coolant. @@ -766,18 +757,7 @@ static void protocol_do_late_reset() { } void protocol_exec_rt_system() { - protocol_do_alarm(); // If there is a hard or soft limit, this will block until rtReset is set - - if (rtReset) { - rtReset = false; - if (sys.state == State::Homing) { - Machine::Homing::fail(ExecAlarm::HomingFailReset); - } - protocol_do_late_reset(); - // Trigger system abort. - sys.abort = true; // Only place this is set true. - return; // Nothing else to do but exit. - } + protocol_do_alarm(); if (rtSafetyDoor) { protocol_do_safety_door(); @@ -1017,21 +997,32 @@ static void protocol_do_limit(void* arg) { if (sys.state == State::Cycle || sys.state == State::Jog) { if (limit->isHard() && rtAlarm == ExecAlarm::None) { log_debug("Hard limits"); - mc_reset(); // Initiate system kill. - rtAlarm = ExecAlarm::HardLimit; + mc_critical(ExecAlarm::HardLimit); } } } static void protocol_do_fault_pin(void* arg) { if (rtAlarm == ExecAlarm::None) { if (sys.state == State::Cycle || sys.state == State::Jog) { - mc_reset(); // Initiate system kill. + mc_critical(ExecAlarm::HardStop); // Initiate system kill. } ControlPin* pin = (ControlPin*)arg; log_info("Stopped by " << pin->_legend); - rtAlarm = ExecAlarm::HardStop; } } +void protocol_do_rt_reset() { + if (sys.state == State::Homing) { + Machine::Homing::fail(ExecAlarm::HomingFailReset); + } else if (sys.state == State::Cycle || sys.state == State::Jog || sys.step_control.executeHold || sys.step_control.executeSysMotion) { + Stepper::stop_stepping(); // Stop stepping immediately, possibly losing position + rtAlarm = ExecAlarm::AbortCycle; + } else if (sys.state == State::Critical) { + sys.state == State::Alarm; + } + protocol_do_late_reset(); + sys.abort = true; // Trigger system abort. +} + ArgEvent feedOverrideEvent { protocol_do_feed_override }; ArgEvent rapidOverrideEvent { protocol_do_rapid_override }; ArgEvent spindleOverrideEvent { protocol_do_spindle_override }; @@ -1049,8 +1040,7 @@ NoArgEvent motionCancelEvent { protocol_do_motion_cancel }; NoArgEvent sleepEvent { protocol_do_sleep }; NoArgEvent debugEvent { report_realtime_debug }; -// Only mc_reset() is permitted to set rtReset. -NoArgEvent resetEvent { mc_reset }; +NoArgEvent rtResetEvent { protocol_do_rt_reset }; // The problem is that report_realtime_status needs a channel argument // Event statusReportEvent { protocol_do_status_report(XXX) }; diff --git a/FluidNC/src/Protocol.h b/FluidNC/src/Protocol.h index 0076e2c46..acb37b506 100644 --- a/FluidNC/src/Protocol.h +++ b/FluidNC/src/Protocol.h @@ -44,7 +44,6 @@ void protocol_buffer_synchronize(); void protocol_disable_steppers(); void protocol_cancel_disable_steppers(); -extern volatile bool rtReset; extern volatile bool rtCycleStop; extern volatile bool runLimitLoop; @@ -96,7 +95,7 @@ extern NoArgEvent cycleStartEvent; extern NoArgEvent cycleStopEvent; extern NoArgEvent motionCancelEvent; extern NoArgEvent sleepEvent; -extern NoArgEvent resetEvent; +extern NoArgEvent rtResetEvent; extern NoArgEvent debugEvent; // extern NoArgEvent statusReportEvent; diff --git a/FluidNC/src/Report.cpp b/FluidNC/src/Report.cpp index 023c0e3d5..9ade3d5ed 100644 --- a/FluidNC/src/Report.cpp +++ b/FluidNC/src/Report.cpp @@ -470,6 +470,7 @@ const char* state_name() { return "Jog"; case State::Homing: return "Home"; + case State::Critical: case State::ConfigAlarm: case State::Alarm: return "Alarm"; diff --git a/FluidNC/src/Report.h b/FluidNC/src/Report.h index ba4df81f3..48d03da6e 100644 --- a/FluidNC/src/Report.h +++ b/FluidNC/src/Report.h @@ -38,7 +38,7 @@ enum class Message : uint8_t { SleepMode = 11, ConfigAlarmLock = 12, HardStop = 13, - FileQuit = 60, // mc_reset was called during a file job + FileQuit = 60, // mc_critical was called during a file job }; typedef uint8_t Counter; // Report interval diff --git a/FluidNC/src/Serial.cpp b/FluidNC/src/Serial.cpp index 58c1eb353..9b9348e14 100644 --- a/FluidNC/src/Serial.cpp +++ b/FluidNC/src/Serial.cpp @@ -87,7 +87,7 @@ void execute_realtime_command(Cmd command, Channel& channel) { switch (command) { case Cmd::Reset: log_debug("Cmd::Reset"); - protocol_send_event(&resetEvent); + protocol_send_event(&rtResetEvent); break; case Cmd::StatusReport: report_realtime_status(channel); // direct call instead of setting flag diff --git a/FluidNC/src/Spindles/VFDSpindle.cpp b/FluidNC/src/Spindles/VFDSpindle.cpp index e198d3488..a52032f93 100644 --- a/FluidNC/src/Spindles/VFDSpindle.cpp +++ b/FluidNC/src/Spindles/VFDSpindle.cpp @@ -21,7 +21,7 @@ #include "VFDSpindle.h" #include "../Machine/MachineConfig.h" -#include "../MotionControl.h" // mc_reset +#include "../MotionControl.h" // mc_critical #include "../Protocol.h" // rtAlarm #include "../Report.h" // hex message @@ -245,8 +245,7 @@ namespace Spindles { } if (next_cmd.critical) { log_error("Critical VFD RS485 Unresponsive"); - mc_reset(); - rtAlarm = ExecAlarm::SpindleControl; + mc_critical(ExecAlarm::SpindleControl); } } } @@ -366,8 +365,7 @@ namespace Spindles { if (unchanged == limit) { log_error(name() << " spindle did not reach device units " << dev_speed << ". Reported value is " << _sync_dev_speed); - mc_reset(); - rtAlarm = ExecAlarm::SpindleControl; + mc_critical(ExecAlarm::SpindleControl); } _syncing = false; diff --git a/FluidNC/src/Types.h b/FluidNC/src/Types.h index 54cb3907b..b0b9ef9b1 100644 --- a/FluidNC/src/Types.h +++ b/FluidNC/src/Types.h @@ -20,4 +20,5 @@ enum class State : uint8_t { SafetyDoor, // Safety door is ajar. Feed holds and de-energizes system. Sleep, // Sleep state. ConfigAlarm, // You can't do anything but fix your config file. + Critical, // You can't do anything but reset with CTRL-x or the reset button }; From e9800bda730f1abac27c9b2687915b0d2928c9fa Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Tue, 25 Jul 2023 19:29:20 -1000 Subject: [PATCH 02/11] Queue alarms instead of using a global variable --- FluidNC/src/Machine/Homing.cpp | 2 +- FluidNC/src/MotionControl.cpp | 15 ++++++-- FluidNC/src/ProcessSettings.cpp | 8 ++--- FluidNC/src/Protocol.cpp | 54 +++++++++++++---------------- FluidNC/src/Protocol.h | 5 ++- FluidNC/src/Spindles/VFDSpindle.cpp | 10 +++--- 6 files changed, 50 insertions(+), 44 deletions(-) diff --git a/FluidNC/src/Machine/Homing.cpp b/FluidNC/src/Machine/Homing.cpp index bfbad79de..ef780b54b 100644 --- a/FluidNC/src/Machine/Homing.cpp +++ b/FluidNC/src/Machine/Homing.cpp @@ -351,7 +351,7 @@ namespace Machine { void Homing::fail(ExecAlarm alarm) { Stepper::reset(); // Stop moving - rtAlarm = alarm; + send_alarm(alarm); config->_axes->set_homing_mode(_cycleAxes, false); // tell motors homing is done...failed config->_axes->set_disable(config->_stepping->_idleMsecs != 255); } diff --git a/FluidNC/src/MotionControl.cpp b/FluidNC/src/MotionControl.cpp index 95aede1a1..3dbad10b2 100644 --- a/FluidNC/src/MotionControl.cpp +++ b/FluidNC/src/MotionControl.cpp @@ -291,7 +291,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, bool 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 (config->_probe->tripped()) { - rtAlarm = ExecAlarm::ProbeFailInitial; + send_alarm(ExecAlarm::ProbeFailInitial); protocol_execute_realtime(); config->_stepping->endLowLatency(); return GCUpdatePos::None; // Nothing else to do but bail. @@ -318,7 +318,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, bool away, if (no_error) { copyAxes(probe_steps, get_motor_steps()); } else { - rtAlarm = ExecAlarm::ProbeFailContact; + send_alarm(ExecAlarm::ProbeFailContact); } } else { probe_succeeded = true; // Indicate to system the probing cycle completed successfully. @@ -368,6 +368,15 @@ void mc_override_ctrl_update(Override override_state) { sys.override_ctrl = override_state; } +// Method to ready the system to reset by setting the realtime reset command and killing any +// active processes in the system. This also checks if a system reset is issued while in +// motion state. If so, kills the steppers and sets the system alarm to flag position +// lost, since there was an abrupt uncontrolled deceleration. Called at an interrupt level by +// realtime abort command and hard limits. So, keep to a minimum. void mc_critical(ExecAlarm alarm) { - rtAlarm = alarm; + if (inMotionState() || sys.step_control.executeHold || sys.step_control.executeSysMotion) { + Stepper::reset(); // Stop stepping immediately, possibly losing position + // Stepper::stop_stepping(); // Stop stepping immediately, possibly losing position + } + send_alarm(alarm); } diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index 34de06103..d73155f64 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -267,12 +267,12 @@ static Error toggle_check_mode(const char* value, WebUI::AuthenticationLevel aut static Error isStuck() { // Block if a control pin is stuck on if (config->_control->safety_door_ajar()) { - rtAlarm = ExecAlarm::ControlPin; + send_alarm(ExecAlarm::ControlPin); return Error::CheckDoor; } if (config->_control->stuck()) { log_info("Control pins:" << config->_control->report_status()); - rtAlarm = ExecAlarm::ControlPin; + send_alarm(ExecAlarm::ControlPin); return Error::CheckControlPins; } return Error::Ok; @@ -495,8 +495,8 @@ static Error doJog(const char* value, WebUI::AuthenticationLevel auth_level, Cha static Error listAlarms(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { if (sys.state == State::ConfigAlarm) { log_to(out, "Configuration alarm is active. Check the boot messages for 'ERR'."); - } else if (rtAlarm != ExecAlarm::None) { - log_to(out, "Active alarm: ", int(rtAlarm) << " (" << alarmString(rtAlarm)); + } else if (sys.state == State::Alarm) { + log_to(out, "Active alarm: ", int(lastAlarm) << " (" << alarmString(lastAlarm)); } if (value) { char* endptr = NULL; diff --git a/FluidNC/src/Protocol.cpp b/FluidNC/src/Protocol.cpp index fabe57ed0..587c818a5 100644 --- a/FluidNC/src/Protocol.cpp +++ b/FluidNC/src/Protocol.cpp @@ -19,7 +19,7 @@ #include "Settings.h" // settings_execute_startup #include "Machine/LimitPin.h" -volatile ExecAlarm rtAlarm; // Global realtime executor bitflag variable for setting various alarms. +volatile ExecAlarm lastAlarm; // The most recent alarm code std::map AlarmNames = { { ExecAlarm::None, "None" }, @@ -249,7 +249,7 @@ static void check_startup_state() { } } if (config->_control->startup_check()) { - rtAlarm = ExecAlarm::ControlPin; + send_alarm(ExecAlarm::ControlPin); } else if (sys.state == State::Alarm || sys.state == State::Sleep) { report_feedback_message(Message::AlarmLock); sys.state = State::Alarm; // Ensure alarm state is set. @@ -370,25 +370,18 @@ void protocol_execute_realtime() { // Executes run-time commands, when required. This function is the primary state // machine that controls the various real-time features. // NOTE: Do not alter this unless you know exactly what you are doing! -static void protocol_do_alarm() { - if (rtAlarm == ExecAlarm::None) { - return; - } - if (inMotionState() || sys.step_control.executeHold || sys.step_control.executeSysMotion) { - Stepper::stop_stepping(); // Stop stepping immediately, possibly losing position - } +static void protocol_do_alarm(void* alarmVoid) { + lastAlarm = (ExecAlarm)((int)alarmVoid); if (spindle->_off_on_alarm) { spindle->stop(); } - alarm_msg(rtAlarm); - if (rtAlarm == ExecAlarm::HardLimit || rtAlarm == ExecAlarm::SoftLimit || rtAlarm == ExecAlarm::HardStop) { - rtAlarm = ExecAlarm::None; + alarm_msg(lastAlarm); + if (lastAlarm == ExecAlarm::HardLimit || lastAlarm == ExecAlarm::SoftLimit || lastAlarm == ExecAlarm::HardStop) { sys.state = State::Critical; // Set system alarm state report_error_message(Message::CriticalEvent); protocol_disable_steppers(); return; } - rtAlarm = ExecAlarm::None; sys.state = State::Alarm; } @@ -654,7 +647,7 @@ void protocol_disable_steppers() { config->_axes->set_disable(false); return; } - if (sys.state == State::Sleep || rtAlarm != ExecAlarm::None) { + if (sys.state == State::Sleep || sys.state == State::Alarm) { // Disable steppers immediately in sleep or alarm state config->_axes->set_disable(true); return; @@ -738,7 +731,7 @@ static void update_velocities() { plan_cycle_reinitialize(); } -// This is the final phase of the shutdown activity that is initiated by mc_critical(). +// This is the final phase of the shutdown activity for a reset // The stuff herein is not necessarily safe to do in an ISR. static void protocol_do_late_reset() { // Kill spindle and coolant. @@ -757,8 +750,6 @@ static void protocol_do_late_reset() { } void protocol_exec_rt_system() { - protocol_do_alarm(); - if (rtSafetyDoor) { protocol_do_safety_door(); } @@ -993,29 +984,25 @@ static void protocol_do_limit(void* arg) { Machine::Homing::limitReached(); return; } - log_debug("Limit switch tripped for " << config->_axes->axisName(limit->_axis) << " motor " << limit->_motorNum); - if (sys.state == State::Cycle || sys.state == State::Jog) { - if (limit->isHard() && rtAlarm == ExecAlarm::None) { - log_debug("Hard limits"); - mc_critical(ExecAlarm::HardLimit); - } + if ((sys.state == State::Cycle || sys.state == State::Jog) && limit->isHard()) { + mc_critical(ExecAlarm::HardLimit); } + log_debug("Limit switch tripped for " << config->_axes->axisName(limit->_axis) << " motor " << limit->_motorNum); } static void protocol_do_fault_pin(void* arg) { - if (rtAlarm == ExecAlarm::None) { - if (sys.state == State::Cycle || sys.state == State::Jog) { - mc_critical(ExecAlarm::HardStop); // Initiate system kill. - } - ControlPin* pin = (ControlPin*)arg; - log_info("Stopped by " << pin->_legend); + if (sys.state == State::Cycle || sys.state == State::Jog) { + mc_critical(ExecAlarm::HardStop); // Initiate system kill. } + ControlPin* pin = (ControlPin*)arg; + log_info("Stopped by " << pin->_legend); } void protocol_do_rt_reset() { if (sys.state == State::Homing) { Machine::Homing::fail(ExecAlarm::HomingFailReset); } else if (sys.state == State::Cycle || sys.state == State::Jog || sys.step_control.executeHold || sys.step_control.executeSysMotion) { Stepper::stop_stepping(); // Stop stepping immediately, possibly losing position - rtAlarm = ExecAlarm::AbortCycle; + // Probably unnecessary because system will restart soon + // send_alarm(ExecAlarm::AbortCycle); } else if (sys.state == State::Critical) { sys.state == State::Alarm; } @@ -1044,6 +1031,7 @@ NoArgEvent rtResetEvent { protocol_do_rt_reset }; // The problem is that report_realtime_status needs a channel argument // Event statusReportEvent { protocol_do_status_report(XXX) }; +ArgEvent alarmEvent { (void (*)(void*))protocol_do_alarm }; xQueueHandle event_queue; @@ -1067,3 +1055,9 @@ void protocol_handle_events() { item.event->run(item.arg); } } +void send_alarm(ExecAlarm alarm) { + protocol_send_event(&alarmEvent, (void*)alarm); +} +void IRAM_ATTR send_alarm_from_ISR(ExecAlarm alarm) { + protocol_send_event_from_ISR(&alarmEvent, (void*)alarm); +} diff --git a/FluidNC/src/Protocol.h b/FluidNC/src/Protocol.h index acb37b506..2b44361dd 100644 --- a/FluidNC/src/Protocol.h +++ b/FluidNC/src/Protocol.h @@ -66,7 +66,7 @@ enum class ExecAlarm : uint8_t { HardStop = 13, }; -extern volatile ExecAlarm rtAlarm; // Global realtime executor variable for setting various alarms. +extern volatile ExecAlarm lastAlarm; #include extern std::map AlarmNames; @@ -112,6 +112,9 @@ struct EventItem { void protocol_send_event(Event*, void* arg = 0); void protocol_handle_events(); +void send_alarm(ExecAlarm alarm); +void send_alarm_from_ISR(ExecAlarm alarm); + inline void protocol_send_event(Event* evt, int arg) { protocol_send_event(evt, (void*)arg); } diff --git a/FluidNC/src/Spindles/VFDSpindle.cpp b/FluidNC/src/Spindles/VFDSpindle.cpp index a52032f93..b6055ecb6 100644 --- a/FluidNC/src/Spindles/VFDSpindle.cpp +++ b/FluidNC/src/Spindles/VFDSpindle.cpp @@ -244,8 +244,8 @@ namespace Spindles { pollidx = -1; } if (next_cmd.critical) { - log_error("Critical VFD RS485 Unresponsive"); mc_critical(ExecAlarm::SpindleControl); + log_error("Critical VFD RS485 Unresponsive"); } } } @@ -364,8 +364,8 @@ namespace Spindles { #endif if (unchanged == limit) { - log_error(name() << " spindle did not reach device units " << dev_speed << ". Reported value is " << _sync_dev_speed); mc_critical(ExecAlarm::SpindleControl); + log_error(name() << " spindle did not reach device units " << dev_speed << ". Reported value is " << _sync_dev_speed); } _syncing = false; @@ -415,9 +415,9 @@ namespace Spindles { action.action = actionSetSpeed; action.arg = dev_speed; action.critical = (dev_speed == 0); - if (xQueueSendFromISR(vfd_cmd_queue, &action, 0) != pdTRUE) { - log_info("VFD Queue Full"); - } + // Ignore errors because reporting is not safe from an ISR. + // Perhaps set a flag instead? + xQueueSendFromISR(vfd_cmd_queue, &action, 0); } } From 0636c0a1c44cbcc874ad3b6d52efd2b707487f0a Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Tue, 25 Jul 2023 19:29:56 -1000 Subject: [PATCH 03/11] Check soft limits on arcs directly, instead of on their segments --- FluidNC/src/Limits.cpp | 48 ++++---- FluidNC/src/Limits.h | 2 + FluidNC/src/MotionControl.cpp | 206 ++++++++++++++++++++++++++++++---- 3 files changed, 212 insertions(+), 44 deletions(-) diff --git a/FluidNC/src/Limits.cpp b/FluidNC/src/Limits.cpp index 1750e8a4d..9d3200411 100644 --- a/FluidNC/src/Limits.cpp +++ b/FluidNC/src/Limits.cpp @@ -116,35 +116,37 @@ void constrainToSoftLimits(float* cartesian) { // Performs a soft limit check. Called from mcline() only. Assumes the machine has been homed, // the workspace volume is in all negative space, and the system is in normal operation. // NOTE: Used by jogging to limit travel within soft-limit volume. -void limits_soft_check(float* cartesian) { - bool limit_error = false; +void limit_error(size_t axis, float coordinate) { + log_info("Soft limit on " << Machine::Axes::_names[axis] << " target:" << coordinate); + + soft_limit = true; + // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within + // workspace volume so just come to a controlled stop so position is not lost. When complete + // enter alarm mode. + protocol_buffer_synchronize(); + if (sys.state == State::Cycle) { + protocol_send_event(&feedHoldEvent); + do { + protocol_execute_realtime(); + if (sys.abort) { + return; + } + } while (sys.state != State::Idle); + } + mc_critical(ExecAlarm::SoftLimit); + log_debug("Soft limits"); +} +void limits_soft_check(float* cartesian) { auto axes = config->_axes; auto n_axis = config->_axes->_numberAxis; for (int axis = 0; axis < n_axis; axis++) { - if (axes->_axis[axis]->_softLimits && (cartesian[axis] < limitsMinPosition(axis) || cartesian[axis] > limitsMaxPosition(axis))) { - log_info("Soft limit on " << Machine::Axes::_names[axis] << " target:" << cartesian[axis]); - limit_error = true; - } - } - - if (limit_error) { - soft_limit = true; - // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within - // workspace volume so just come to a controlled stop so position is not lost. When complete - // enter alarm mode. - if (sys.state == State::Cycle) { - protocol_send_event(&feedHoldEvent); - do { - protocol_execute_realtime(); - if (sys.abort) { - return; - } - } while (sys.state != State::Idle); + float coordinate = cartesian[axis]; + if (axes->_axis[axis]->_softLimits && (coordinate < limitsMinPosition(axis) || coordinate > limitsMaxPosition(axis))) { + limit_error(axis, coordinate); + return; } - log_debug("Soft limits"); - mc_critical(ExecAlarm::SoftLimit); } } diff --git a/FluidNC/src/Limits.h b/FluidNC/src/Limits.h index e24e85bf2..107a875c3 100644 --- a/FluidNC/src/Limits.h +++ b/FluidNC/src/Limits.h @@ -16,6 +16,8 @@ void limits_init(); // Returns limit state MotorMask limits_get_state(); +void limit_error(size_t axis, float cordinate); + // Check for soft limit violations void limits_soft_check(float* cartesian); diff --git a/FluidNC/src/MotionControl.cpp b/FluidNC/src/MotionControl.cpp index 3dbad10b2..fcb054391 100644 --- a/FluidNC/src/MotionControl.cpp +++ b/FluidNC/src/MotionControl.cpp @@ -101,11 +101,173 @@ void mc_cancel_jog() { // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. + +// mc_linear_no_check() is used by mc_arc() which pre-checks the arc limits using +// a fast algorithm, so checking each segment is unnecessary. +static bool mc_linear_no_check(float* target, plan_line_data_t* pl_data, float* position) { + return config->_kinematics->cartesian_to_motors(target, pl_data, position); +} bool mc_linear(float* target, plan_line_data_t* pl_data, float* position) { if (!pl_data->is_jog) { // soft limits for jogs have already been dealt with limits_soft_check(target); } - return config->_kinematics->cartesian_to_motors(target, pl_data, position); + return mc_linear_no_check(target, pl_data, position); +} + +// Check that the arc does not exceed the soft limits using a fast +// algorithm that requires no transcendental functions. +// caxes[] depends on the plane selection via G17, G18, and G19. caxes[0] is the first +// circle plane axis, caxes[1] is the second circle plane axis, and caxes[2] is the +// orthogonal plane. So for G17 mode, caxes[] is { 0, 1, 2} for { X, Y, Z}. G18 is {2, 0, 1} i.e. {Z, X, Y}, and G19 is {1, 2, 0} i.e. {Y, Z, X} +void check_arc_limits(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) { + auto axes = config->_axes; + + // Handle the orthognal axis first to get it out of the way. + size_t the_axis = caxes[2]; + if (axes->_axis[the_axis]->_softLimits) { + float amin = std::min(position[the_axis], target[the_axis]); + if (amin < limitsMinPosition(the_axis)) { + limit_error(the_axis, amin); + return; + } + float amax = std::max(position[the_axis], target[the_axis]); + if (amax > limitsMaxPosition(the_axis)) { + limit_error(the_axis, amax); + return; + } + } + + bool limited[2] = { axes->_axis[caxes[0]]->_softLimits, axes->_axis[caxes[1]]->_softLimits }; + + // If neither axis of the circular plane has limits enabled, skip the computation + if (!(limited[0] || limited[1])) { + return; + } + + // The origin for this calculation's coordinate system is at the center of the arc. + // The 0 and 1 entries are for the circle plane + // and the 2 entry is the orthogonal (linear) direction + + float s[2], e[2]; // Start and end of arc in the circle plane, relative to center + + // Depending on the arc direction, set the arc start and end points relative + // to the arc center. Afterwards, end is always counterclockwise relative to + // start, thus simplifying the following decision tree. + if (is_clockwise_arc) { + s[0] = target[caxes[0]] - center[0]; + s[1] = target[caxes[1]] - center[1]; + e[0] = position[caxes[0]] - center[0]; + e[1] = position[caxes[1]] - center[1]; + } else { + s[0] = position[caxes[0]] - center[0]; + s[1] = position[caxes[1]] - center[1]; + e[0] = target[caxes[0]] - center[0]; + e[1] = target[caxes[1]] - center[1]; + } + + // Axis crossings - plus and minus caxes[0] and caxes[1] + bool p[2] = { false, false }; + bool m[2] = { false, false }; + + // The following decision tree determines whether the arc crosses + // the horizontal and vertical axes of the circular plane in the + // positive and negative half planes. There are ways to express + // it in fewer lines of code by converting to alternate + // representations like angles, but this way is computationally + // efficient since it avoids any use of transcendental functions. + // Every path through this decision tree is either 4 or 5 simple + // comparisons. + if (e[1] >= 0) { // End in upper half plane + if (e[0] > 0) { // End in quadrant 0 - X+ Y+ + if (s[1] >= 0) { // Start in upper half plane + if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ + if (s[0] <= e[0]) { // wraparound + p[0] = p[1] = m[0] = m[1] = true; + } + } else { // Start in quadrant 1 - X- Y+ + m[0] = m[1] = p[0] = true; + } + } else { // Start in lower half plane + if (s[0] > 0) { // Start in quadrant 3 - X+ Y- + p[0] = true; + } else { // Start in quadrant 2 - X- Y- + m[1] = p[0] = true; + } + } + } else { // End in quadrant 1 - X- Y+ + if (s[1] >= 0) { // Start in upper half plane + if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ + p[1] = true; + } else { // Start in quadrant 1 - X- Y+ + if (s[0] <= e[0]) { // wraparound + p[0] = p[1] = m[0] = m[1] = true; + } + } + } else { // Start in lower half plane + if (s[0] > 0) { // Start in quadrant 3 - X+ Y- + p[0] = p[1] = true; + } else { // Start in quadrant 2 - X- Y- + m[1] = p[0] = p[1] = true; + } + } + } + } else { // e[1] < 0 - end in lower half plane + if (e[0] > 0) { // End in quadrant 3 - X+ Y+ + if (s[1] >= 0) { // Start in upper half plane + if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ + p[1] = m[0] = m[1] = true; + } else { // Start in quadrant 1 - X- Y+ + m[0] = m[1] = true; + } + } else { // Start in lower half plane + if (s[0] > 0) { // Start in quadrant 3 - X+ Y- + if (s[0] >= e[0]) { // wraparound + p[0] = p[1] = m[0] = m[1] = true; + } + } else { // Start in quadrant 2 - X- Y- + m[1] = true; + } + } + } else { // End in quadrant 2 - X- Y+ + if (s[1] >= 0) { // Start in upper half plane + if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ + p[1] = m[0] = true; + } else { // Start in quadrant 1 - X- Y+ + m[0] = true; + } + } else { // Start in lower half plane + if (s[0] > 0) { // Start in quadrant 3 - X+ Y- + p[0] = p[1] = m[0] = true; + } else { // Start in quadrant 2 - X- Y- + if (s[0] >= e[0]) { // wraparound + p[0] = p[1] = m[0] = m[1] = true; + } + } + } + } + } + + // Now check limits based on arc endpoints and axis crossings + for (size_t a = 0; a < 2; ++a) { + the_axis = caxes[a]; + if (limited[a]) { + // If we crossed the axis in the positive half plane, the + // maximum extent along that axis is at center + radius. + // Otherwise it is the maximum coordinate of the start and + // end positions. Similarly for the negative half plane + // and the minimum extent. + float amin = m[a] ? center[a] - radius : std::min(target[the_axis], position[the_axis]); + if (amin < limitsMinPosition(the_axis)) { + limit_error(the_axis, amin); + return; + } + float amax = p[a] ? center[a] + radius : std::max(target[the_axis], position[the_axis]); + if (amax > limitsMaxPosition(the_axis)) { + limit_error(the_axis, amax); + return; + } + } + } } // Execute an arc in offset mode format. position == current xyz, target == target xyz, @@ -125,12 +287,15 @@ void mc_arc(float* target, size_t axis_linear, bool is_clockwise_arc, int pword_rotations) { - float center_axis0 = position[axis_0] + offset[axis_0]; - float center_axis1 = position[axis_1] + offset[axis_1]; - float r_axis0 = -offset[axis_0]; // Radius vector from center to current location - float r_axis1 = -offset[axis_1]; - float rt_axis0 = target[axis_0] - center_axis0; - float rt_axis1 = target[axis_1] - center_axis1; + float center[3] = { position[axis_0] + offset[axis_0], position[axis_1] + offset[axis_1], 0 }; + + // The first two axes are the circle plane and the third is the orthogonal plane + size_t caxes[3] = { axis_0, axis_1, axis_linear }; + check_arc_limits(target, position, center, radius, caxes, is_clockwise_arc); + + // Radius vector from center to current location + float radii[2] = { -offset[axis_0], -offset[axis_1] }; + float rt[2] = { target[axis_0] - center[0], target[axis_1] - center[1] }; auto n_axis = config->_axes->_numberAxis; @@ -140,7 +305,7 @@ void mc_arc(float* target, } // CCW angle between position and target from circle center. Only one atan2() trig computation required. - float angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); + float angular_travel = atan2f(radii[0] * rt[1] - radii[1] * rt[0], radii[0] * rt[0] + radii[1] * rt[1]); if (is_clockwise_arc) { // Correct atan2 output per direction if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= 2 * float(M_PI); @@ -211,35 +376,34 @@ void mc_arc(float* target, cos_T *= 0.5; float sin_Ti; float cos_Ti; - float r_axisi; uint16_t i; size_t count = 0; float original_feedrate = pl_data->feed_rate; // Kinematics may alter the feedrate, so save an original copy for (i = 1; i < segments; i++) { // Increment (segments-1). if (count < N_ARC_CORRECTION) { // Apply vector rotation matrix. ~40 usec - r_axisi = r_axis0 * sin_T + r_axis1 * cos_T; - r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T; - r_axis1 = r_axisi; + float ri = radii[0] * sin_T + radii[1] * cos_T; + radii[0] = radii[0] * cos_T - radii[1] * sin_T; + radii[1] = ri; count++; } else { // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. ~375 usec // Compute exact location by applying transformation matrix from initial radius vector(=-offset). - cos_Ti = cosf(i * theta_per_segment); - sin_Ti = sinf(i * theta_per_segment); - r_axis0 = -offset[axis_0] * cos_Ti + offset[axis_1] * sin_Ti; - r_axis1 = -offset[axis_0] * sin_Ti - offset[axis_1] * cos_Ti; - count = 0; + cos_Ti = cosf(i * theta_per_segment); + sin_Ti = sinf(i * theta_per_segment); + radii[0] = -offset[axis_0] * cos_Ti + offset[axis_1] * sin_Ti; + radii[1] = -offset[axis_0] * sin_Ti - offset[axis_1] * cos_Ti; + count = 0; } // Update arc_target location - position[axis_0] = center_axis0 + r_axis0; - position[axis_1] = center_axis1 + r_axis1; + position[axis_0] = center[0] + radii[0]; + position[axis_1] = center[1] + radii[1]; position[axis_linear] += linear_per_segment[axis_linear]; for (size_t i = A_AXIS; i < n_axis; i++) { position[i] += linear_per_segment[i]; } pl_data->feed_rate = original_feedrate; // This restores the feedrate kinematics may have altered - mc_linear(position, pl_data, previous_position); + mc_linear_no_check(position, pl_data, previous_position); previous_position[axis_0] = position[axis_0]; previous_position[axis_1] = position[axis_1]; previous_position[axis_linear] = position[axis_linear]; @@ -250,7 +414,7 @@ void mc_arc(float* target, } } // Ensure last segment arrives at target location. - mc_linear(target, pl_data, previous_position); + mc_linear_no_check(target, pl_data, previous_position); } // Execute dwell in seconds. From 2e24fb32b2cd523ef42c89bce9b8da2d440e6fb8 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Fri, 28 Jul 2023 12:33:11 -1000 Subject: [PATCH 04/11] Lots of fixes --- FluidNC/src/Jog.cpp | 6 +- FluidNC/src/Kinematics/Cartesian.cpp | 228 +++++++++++++++++++++++++ FluidNC/src/Kinematics/Cartesian.h | 10 +- FluidNC/src/Kinematics/Kinematics.cpp | 15 ++ FluidNC/src/Kinematics/Kinematics.h | 19 ++- FluidNC/src/Limits.cpp | 73 -------- FluidNC/src/Limits.h | 8 +- FluidNC/src/Machine/Axes.cpp | 3 +- FluidNC/src/Machine/Axes.h | 6 +- FluidNC/src/Machine/Axis.cpp | 2 +- FluidNC/src/Machine/Homing.cpp | 17 +- FluidNC/src/Machine/Homing.h | 12 +- FluidNC/src/Main.cpp | 4 +- FluidNC/src/MotionControl.cpp | 164 +----------------- FluidNC/src/Motors/StandardStepper.cpp | 4 +- FluidNC/src/Planner.cpp | 12 +- FluidNC/src/Planner.h | 1 + FluidNC/src/ProcessSettings.cpp | 1 + FluidNC/src/Protocol.cpp | 1 + FluidNC/src/Protocol.h | 2 + 20 files changed, 329 insertions(+), 259 deletions(-) diff --git a/FluidNC/src/Jog.cpp b/FluidNC/src/Jog.cpp index f50844897..7dc23136c 100644 --- a/FluidNC/src/Jog.cpp +++ b/FluidNC/src/Jog.cpp @@ -12,6 +12,10 @@ // Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog. // cancelledInflight will be set to true if was not added to parser due to a cancelJog. Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block, bool* cancelledInflight) { + if (config->_kinematics->invalid_line(gc_block->values.xyz)) { + config->_kinematics->constrain_line(gc_block->values.xyz, gc_state.position); + } + // Initialize planner data struct for jogging motions. // NOTE: Spindle and coolant are allowed to fully function with overrides during a jog. pl_data->feed_rate = gc_block->values.f; @@ -19,8 +23,6 @@ Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block, bool* can pl_data->is_jog = true; pl_data->line_number = gc_block->values.n; - constrainToSoftLimits(gc_block->values.xyz); - if (!mc_linear(gc_block->values.xyz, pl_data, gc_state.position)) { return Error::JogCancelled; } diff --git a/FluidNC/src/Kinematics/Cartesian.cpp b/FluidNC/src/Kinematics/Cartesian.cpp index 359c1baf6..8735988e9 100644 --- a/FluidNC/src/Kinematics/Cartesian.cpp +++ b/FluidNC/src/Kinematics/Cartesian.cpp @@ -18,6 +18,234 @@ namespace Kinematics { } } + // Check that the arc does not exceed the soft limits using a fast + // algorithm that requires no transcendental functions. + // caxes[] depends on the plane selection via G17, G18, and G19. caxes[0] is the first + // circle plane axis, caxes[1] is the second circle plane axis, and caxes[2] is the + // orthogonal plane. So for G17 mode, caxes[] is { 0, 1, 2} for { X, Y, Z}. G18 is {2, 0, 1} i.e. {Z, X, Y}, and G19 is {1, 2, 0} i.e. {Y, Z, X} + bool Cartesian::invalid_arc(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) { + auto axes = config->_axes; + + // Handle the orthognal axis first to get it out of the way. + size_t the_axis = caxes[2]; + if (axes->_axis[the_axis]->_softLimits) { + float amin = std::min(position[the_axis], target[the_axis]); + if (amin < limitsMinPosition(the_axis)) { + limit_error(the_axis, amin); + return true; + } + float amax = std::max(position[the_axis], target[the_axis]); + if (amax > limitsMaxPosition(the_axis)) { + limit_error(the_axis, amax); + return true; + } + } + + bool limited[2] = { axes->_axis[caxes[0]]->_softLimits, axes->_axis[caxes[1]]->_softLimits }; + + // If neither axis of the circular plane has limits enabled, skip the computation + if (!(limited[0] || limited[1])) { + return false; + } + + // The origin for this calculation's coordinate system is at the center of the arc. + // The 0 and 1 entries are for the circle plane + // and the 2 entry is the orthogonal (linear) direction + + float s[2], e[2]; // Start and end of arc in the circle plane, relative to center + + // Depending on the arc direction, set the arc start and end points relative + // to the arc center. Afterwards, end is always counterclockwise relative to + // start, thus simplifying the following decision tree. + if (is_clockwise_arc) { + s[0] = target[caxes[0]] - center[0]; + s[1] = target[caxes[1]] - center[1]; + e[0] = position[caxes[0]] - center[0]; + e[1] = position[caxes[1]] - center[1]; + } else { + s[0] = position[caxes[0]] - center[0]; + s[1] = position[caxes[1]] - center[1]; + e[0] = target[caxes[0]] - center[0]; + e[1] = target[caxes[1]] - center[1]; + } + + // Axis crossings - plus and minus caxes[0] and caxes[1] + bool p[2] = { false, false }; + bool m[2] = { false, false }; + + // The following decision tree determines whether the arc crosses + // the horizontal and vertical axes of the circular plane in the + // positive and negative half planes. There are ways to express + // it in fewer lines of code by converting to alternate + // representations like angles, but this way is computationally + // efficient since it avoids any use of transcendental functions. + // Every path through this decision tree is either 4 or 5 simple + // comparisons. + if (e[1] >= 0) { // End in upper half plane + if (e[0] > 0) { // End in quadrant 0 - X+ Y+ + if (s[1] >= 0) { // Start in upper half plane + if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ + if (s[0] <= e[0]) { // wraparound + p[0] = p[1] = m[0] = m[1] = true; + } + } else { // Start in quadrant 1 - X- Y+ + m[0] = m[1] = p[0] = true; + } + } else { // Start in lower half plane + if (s[0] > 0) { // Start in quadrant 3 - X+ Y- + p[0] = true; + } else { // Start in quadrant 2 - X- Y- + m[1] = p[0] = true; + } + } + } else { // End in quadrant 1 - X- Y+ + if (s[1] >= 0) { // Start in upper half plane + if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ + p[1] = true; + } else { // Start in quadrant 1 - X- Y+ + if (s[0] <= e[0]) { // wraparound + p[0] = p[1] = m[0] = m[1] = true; + } + } + } else { // Start in lower half plane + if (s[0] > 0) { // Start in quadrant 3 - X+ Y- + p[0] = p[1] = true; + } else { // Start in quadrant 2 - X- Y- + m[1] = p[0] = p[1] = true; + } + } + } + } else { // e[1] < 0 - end in lower half plane + if (e[0] > 0) { // End in quadrant 3 - X+ Y+ + if (s[1] >= 0) { // Start in upper half plane + if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ + p[1] = m[0] = m[1] = true; + } else { // Start in quadrant 1 - X- Y+ + m[0] = m[1] = true; + } + } else { // Start in lower half plane + if (s[0] > 0) { // Start in quadrant 3 - X+ Y- + if (s[0] >= e[0]) { // wraparound + p[0] = p[1] = m[0] = m[1] = true; + } + } else { // Start in quadrant 2 - X- Y- + m[1] = true; + } + } + } else { // End in quadrant 2 - X- Y+ + if (s[1] >= 0) { // Start in upper half plane + if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ + p[1] = m[0] = true; + } else { // Start in quadrant 1 - X- Y+ + m[0] = true; + } + } else { // Start in lower half plane + if (s[0] > 0) { // Start in quadrant 3 - X+ Y- + p[0] = p[1] = m[0] = true; + } else { // Start in quadrant 2 - X- Y- + if (s[0] >= e[0]) { // wraparound + p[0] = p[1] = m[0] = m[1] = true; + } + } + } + } + } + // Now check limits based on arc endpoints and axis crossings + for (size_t a = 0; a < 2; ++a) { + the_axis = caxes[a]; + if (limited[a]) { + // If we crossed the axis in the positive half plane, the + // maximum extent along that axis is at center + radius. + // Otherwise it is the maximum coordinate of the start and + // end positions. Similarly for the negative half plane + // and the minimum extent. + float amin = m[a] ? center[a] - radius : std::min(target[the_axis], position[the_axis]); + if (amin < limitsMinPosition(the_axis)) { + limit_error(the_axis, amin); + return true; + } + float amax = p[a] ? center[a] + radius : std::max(target[the_axis], position[the_axis]); + if (amax > limitsMaxPosition(the_axis)) { + limit_error(the_axis, amax); + return true; + } + } + } + return false; + } + + void Cartesian::constrain_line(float* target, float* position) { + auto axes = config->_axes; + auto n_axis = config->_axes->_numberAxis; + + float* current_position = get_mpos(); + MotorMask lim_pin_state = limits_get_state(); + + for (int axis = 0; axis < n_axis; axis++) { + auto axisSetting = axes->_axis[axis]; + // If the axis is moving from the current location and soft limits are on. + if (axisSetting->_softLimits && target[axis] != current_position[axis]) { + // When outside the axis range, only small nudges to clear switches are allowed + bool move_positive = target[axis] > current_position[axis]; + if ((!move_positive && (current_position[axis] < limitsMinPosition(axis))) || + (move_positive && (current_position[axis] > limitsMaxPosition(axis)))) { + // only allow a nudge if a switch is active + if (bitnum_is_false(lim_pin_state, Machine::Axes::motor_bit(axis, 0)) && + bitnum_is_false(lim_pin_state, Machine::Axes::motor_bit(axis, 1))) { + target[axis] = current_position[axis]; // cancel the move on this axis + log_debug("Soft limit violation on " << Machine::Axes::_names[axis]); + continue; + } + float jog_dist = target[axis] - current_position[axis]; + + MotorMask axisMotors = Machine::Axes::axes_to_motors(1 << axis); + bool posLimited = bits_are_true(Machine::Axes::posLimitMask, axisMotors); + bool negLimited = bits_are_true(Machine::Axes::negLimitMask, axisMotors); + + // if jog is positive and only the positive switch is active, then kill the move + // if jog is negative and only the negative switch is active, then kill the move + if (posLimited != negLimited) { // XOR, because ambiguous (both) is OK + if ((negLimited && (jog_dist < 0)) || (posLimited && (jog_dist > 0))) { + target[axis] = current_position[axis]; // cancel the move on this axis + log_debug("Jog into active switch blocked on " << Machine::Axes::_names[axis]); + continue; + } + } + + auto nudge_max = axisSetting->_motors[0]->_pulloff; + if (abs(jog_dist) > nudge_max) { + target[axis] = (jog_dist >= 0) ? current_position[axis] + nudge_max : current_position[axis] + nudge_max; + log_debug("Jog amount limited when outside soft limits") + } + continue; + } + + if (target[axis] < limitsMinPosition(axis)) { + target[axis] = limitsMinPosition(axis); + } else if (target[axis] > limitsMaxPosition(axis)) { + target[axis] = limitsMaxPosition(axis); + } else { + continue; + } + log_debug("Jog constrained to axis range"); + } + } + } + + bool Cartesian::invalid_line(float* cartesian) { + auto axes = config->_axes; + auto n_axis = config->_axes->_numberAxis; + + for (int axis = 0; axis < n_axis; axis++) { + float coordinate = cartesian[axis]; + if (axes->_axis[axis]->_softLimits && (coordinate < limitsMinPosition(axis) || coordinate > limitsMaxPosition(axis))) { + limit_error(axis, coordinate); + return true; + } + } + return false; + } + bool Cartesian::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { // Motor space is cartesian space, so we do no transform. return mc_move_motors(target, pl_data); diff --git a/FluidNC/src/Kinematics/Cartesian.h b/FluidNC/src/Kinematics/Cartesian.h index 50476241a..2b026cd54 100644 --- a/FluidNC/src/Kinematics/Cartesian.h +++ b/FluidNC/src/Kinematics/Cartesian.h @@ -16,13 +16,17 @@ namespace Kinematics { public: Cartesian() = default; - Cartesian(const Cartesian&) = delete; - Cartesian(Cartesian&&) = delete; + Cartesian(const Cartesian&) = delete; + Cartesian(Cartesian&&) = delete; Cartesian& operator=(const Cartesian&) = delete; - Cartesian& operator=(Cartesian&&) = delete; + Cartesian& operator=(Cartesian&&) = delete; // Kinematic Interface + virtual void constrain_line(float* cartesian, float* position) override; + virtual bool invalid_line(float* cartesian) override; + virtual bool invalid_arc(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) override; + virtual bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) override; virtual void init() override; virtual void init_position() override; diff --git a/FluidNC/src/Kinematics/Kinematics.cpp b/FluidNC/src/Kinematics/Kinematics.cpp index 1a186f7d0..cb30a5ae9 100644 --- a/FluidNC/src/Kinematics/Kinematics.cpp +++ b/FluidNC/src/Kinematics/Kinematics.cpp @@ -8,6 +8,21 @@ #include "Cartesian.h" namespace Kinematics { + void Kinematics::constrain_line(float* target, float* position) { + Assert(_system != nullptr, "No kinematic system"); + return _system->constrain_line(target, position); + } + + bool Kinematics::invalid_line(float* target) { + Assert(_system != nullptr, "No kinematic system"); + return _system->invalid_line(target); + } + + bool Kinematics::invalid_arc(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) { + Assert(_system != nullptr, "No kinematic system"); + return _system->invalid_arc(target, position, center, radius, caxes, is_clockwise_arc); + } + bool Kinematics::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { Assert(_system != nullptr, "No kinematic system"); return _system->cartesian_to_motors(target, pl_data, position); diff --git a/FluidNC/src/Kinematics/Kinematics.h b/FluidNC/src/Kinematics/Kinematics.h index e8ea30352..9add47c4c 100644 --- a/FluidNC/src/Kinematics/Kinematics.h +++ b/FluidNC/src/Kinematics/Kinematics.h @@ -48,6 +48,10 @@ namespace Kinematics { void motors_to_cartesian(float* cartesian, float* motors, int n_axis); void transform_cartesian_to_motors(float* motors, float* cartesian); + void constrain_line(float* target, float* position); + bool invalid_line(float* target); + bool invalid_arc(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc); + bool canHome(AxisMask axisMask); void releaseMotors(AxisMask axisMask, MotorMask motors); bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited); @@ -60,15 +64,22 @@ namespace Kinematics { public: KinematicSystem() = default; - KinematicSystem(const KinematicSystem&) = delete; - KinematicSystem(KinematicSystem&&) = delete; + KinematicSystem(const KinematicSystem&) = delete; + KinematicSystem(KinematicSystem&&) = delete; KinematicSystem& operator=(const KinematicSystem&) = delete; - KinematicSystem& operator=(KinematicSystem&&) = delete; + KinematicSystem& operator=(KinematicSystem&&) = delete; // Kinematic system interface. virtual bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) = 0; virtual void init() = 0; - virtual void init_position() = 0; // used to set the machine position at init + virtual void init_position() = 0; // used to set the machine position at init + + virtual void constrain_line(float* cartesian, float* position) {} + virtual bool invalid_line(float* cartesian) { return false; } + virtual bool invalid_arc(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) { + return false; + } + virtual void motors_to_cartesian(float* cartesian, float* motors, int n_axis) = 0; virtual void transform_cartesian_to_motors(float* motors, float* cartesian) = 0; diff --git a/FluidNC/src/Limits.cpp b/FluidNC/src/Limits.cpp index 9d3200411..c8fc0d74e 100644 --- a/FluidNC/src/Limits.cpp +++ b/FluidNC/src/Limits.cpp @@ -54,65 +54,6 @@ bool ambiguousLimit() { bool soft_limit = false; -// Constrain the coordinates to stay within the soft limit envelope -void constrainToSoftLimits(float* cartesian) { - auto axes = config->_axes; - auto n_axis = config->_axes->_numberAxis; - - float* current_position = get_mpos(); - MotorMask lim_pin_state = limits_get_state(); - - for (int axis = 0; axis < n_axis; axis++) { - auto axisSetting = axes->_axis[axis]; - // If the axis is moving from the current location and soft limits are on. - if (axisSetting->_softLimits && cartesian[axis] != current_position[axis]) { - // When outside the axis range, only small nudges to clear switches are allowed - bool move_positive = cartesian[axis] > current_position[axis]; - if ((!move_positive && (current_position[axis] < limitsMinPosition(axis))) || - (move_positive && (current_position[axis] > limitsMaxPosition(axis)))) { - // only allow a nudge if a switch is active - if (bitnum_is_false(lim_pin_state, Machine::Axes::motor_bit(axis, 0)) && - bitnum_is_false(lim_pin_state, Machine::Axes::motor_bit(axis, 1))) { - cartesian[axis] = current_position[axis]; // cancel the move on this axis - log_debug("Soft limit violation on " << Machine::Axes::_names[axis]); - continue; - } - float jog_dist = cartesian[axis] - current_position[axis]; - - MotorMask axisMotors = Machine::Axes::axes_to_motors(1 << axis); - bool posLimited = bits_are_true(Machine::Axes::posLimitMask, axisMotors); - bool negLimited = bits_are_true(Machine::Axes::negLimitMask, axisMotors); - - // if jog is positive and only the positive switch is active, then kill the move - // if jog is negative and only the negative switch is active, then kill the move - if (posLimited != negLimited) { // XOR, because ambiguous (both) is OK - if ((negLimited && (jog_dist < 0)) || (posLimited && (jog_dist > 0))) { - cartesian[axis] = current_position[axis]; // cancel the move on this axis - log_debug("Jog into active switch blocked on " << Machine::Axes::_names[axis]); - continue; - } - } - - auto nudge_max = axisSetting->_motors[0]->_pulloff; - if (abs(jog_dist) > nudge_max) { - cartesian[axis] = (jog_dist >= 0) ? current_position[axis] + nudge_max : current_position[axis] + nudge_max; - log_debug("Jog amount limited when outside soft limits") - } - continue; - } - - if (cartesian[axis] < limitsMinPosition(axis)) { - cartesian[axis] = limitsMinPosition(axis); - } else if (cartesian[axis] > limitsMaxPosition(axis)) { - cartesian[axis] = limitsMaxPosition(axis); - } else { - continue; - } - log_debug("Jog constrained to axis range"); - } - } -} - // Performs a soft limit check. Called from mcline() only. Assumes the machine has been homed, // the workspace volume is in all negative space, and the system is in normal operation. // NOTE: Used by jogging to limit travel within soft-limit volume. @@ -134,20 +75,6 @@ void limit_error(size_t axis, float coordinate) { } while (sys.state != State::Idle); } mc_critical(ExecAlarm::SoftLimit); - log_debug("Soft limits"); -} - -void limits_soft_check(float* cartesian) { - auto axes = config->_axes; - auto n_axis = config->_axes->_numberAxis; - - for (int axis = 0; axis < n_axis; axis++) { - float coordinate = cartesian[axis]; - if (axes->_axis[axis]->_softLimits && (coordinate < limitsMinPosition(axis) || coordinate > limitsMaxPosition(axis))) { - limit_error(axis, coordinate); - return; - } - } } float limitsMaxPosition(size_t axis) { diff --git a/FluidNC/src/Limits.h b/FluidNC/src/Limits.h index 107a875c3..0c2ef2d11 100644 --- a/FluidNC/src/Limits.h +++ b/FluidNC/src/Limits.h @@ -4,7 +4,7 @@ #pragma once -#include "System.h" // AxisMask +#include "System.h" #include @@ -18,12 +18,6 @@ MotorMask limits_get_state(); void limit_error(size_t axis, float cordinate); -// Check for soft limit violations -void limits_soft_check(float* cartesian); - -// Constrain the coordinates to stay within the soft limit envelope -void constrainToSoftLimits(float* cartesian); - float limitsMaxPosition(size_t axis); float limitsMinPosition(size_t axis); diff --git a/FluidNC/src/Machine/Axes.cpp b/FluidNC/src/Machine/Axes.cpp index 4e94efd7b..06f349f9a 100644 --- a/FluidNC/src/Machine/Axes.cpp +++ b/FluidNC/src/Machine/Axes.cpp @@ -13,10 +13,11 @@ EnumItem axisType[] = { { 0, "X" }, { 1, "Y" }, { 2, "Z" }, { 3, "A" }, { 4, "B" namespace Machine { MotorMask Axes::posLimitMask = 0; MotorMask Axes::negLimitMask = 0; - MotorMask Axes::homingMask = 0; MotorMask Axes::limitMask = 0; MotorMask Axes::motorMask = 0; + AxisMask Axes::homingMask = 0; + Axes::Axes() : _axis() { for (int i = 0; i < MAX_N_AXIS; ++i) { _axis[i] = nullptr; diff --git a/FluidNC/src/Machine/Axes.h b/FluidNC/src/Machine/Axes.h index bb2e65588..90bff23de 100644 --- a/FluidNC/src/Machine/Axes.h +++ b/FluidNC/src/Machine/Axes.h @@ -24,10 +24,11 @@ namespace Machine { // Bitmasks to collect information about axes that have limits and homing static MotorMask posLimitMask; static MotorMask negLimitMask; - static MotorMask homingMask; static MotorMask limitMask; static MotorMask motorMask; + static AxisMask homingMask; + Pin _sharedStepperDisable; Pin _sharedStepperReset; @@ -72,7 +73,8 @@ namespace Machine { void config_motors(); std::string maskToNames(AxisMask mask); - bool namesToMask(const char* names, AxisMask& mask); + + bool namesToMask(const char* names, AxisMask& mask); std::string motorMaskToNames(MotorMask mask); diff --git a/FluidNC/src/Machine/Axis.cpp b/FluidNC/src/Machine/Axis.cpp index 3d0be46ce..8c78cd6bb 100644 --- a/FluidNC/src/Machine/Axis.cpp +++ b/FluidNC/src/Machine/Axis.cpp @@ -45,7 +45,7 @@ namespace Machine { m->init(); } } - if (_homing) { + if (_homing->_cycle) { _homing->init(); set_bitnum(Axes::homingMask, _axis); } diff --git a/FluidNC/src/Machine/Homing.cpp b/FluidNC/src/Machine/Homing.cpp index ef780b54b..644b19371 100644 --- a/FluidNC/src/Machine/Homing.cpp +++ b/FluidNC/src/Machine/Homing.cpp @@ -40,6 +40,16 @@ namespace Machine { std::queue Homing::_remainingCycles; uint32_t Homing::_settling_ms; + AxisMask Homing::_unhomed_axes; // Bitmap of axes whose position is unknown + + bool Homing::axis_is_homed(size_t axis) { return bitnum_is_false(_unhomed_axes, axis); } + void Homing::set_axis_homed(size_t axis) { clear_bitnum(_unhomed_axes, axis); } + void Homing::set_axis_unhomed(size_t axis) { set_bitnum(_unhomed_axes, axis); } + void Homing::set_all_axes_unhomed() { _unhomed_axes = Machine::Axes::homingMask; } + void Homing::set_all_axes_homed() { _unhomed_axes = 0; } + + AxisMask Homing::unhomed_axes() { return _unhomed_axes; } + const char* Homing::_phaseNames[] = { "None", "PrePulloff", "FastApproach", "Pulloff0", "SlowApproach", "Pulloff1", "Pulloff2", "CycleDone", }; @@ -319,9 +329,9 @@ namespace Machine { config->_stepping->endLowLatency(); - if (!sys.abort) { // Execute startup scripts after successful homing. - sys.state = State::Idle; // Set to IDLE when complete. - Stepper::go_idle(); // Set steppers to the settings idle state before returning. + if (!sys.abort) { + sys.state = unhomed_axes() ? State::Alarm : State::Idle; + Stepper::go_idle(); // Set steppers to the settings idle state before returning. } } @@ -389,6 +399,7 @@ namespace Machine { // Replace coordinates homed axes with the homing values. for (size_t axis = 0; axis < n_axis; axis++) { if (bitnum_is_true(_cycleAxes, axis)) { + set_axis_homed(axis); mpos[axis] = axes->_axis[axis]->_homing->_mpos; } } diff --git a/FluidNC/src/Machine/Homing.h b/FluidNC/src/Machine/Homing.h index d1a780c22..682e9678a 100644 --- a/FluidNC/src/Machine/Homing.h +++ b/FluidNC/src/Machine/Homing.h @@ -11,6 +11,8 @@ namespace Machine { class Homing : public Configuration::Configurable { + static AxisMask _unhomed_axes; + public: static enum Phase { None = 0, @@ -23,6 +25,14 @@ namespace Machine { CycleDone = 7, } _phase; + static AxisMask unhomed_axes(); + + static void set_axis_homed(size_t axis); + static void set_axis_unhomed(size_t axis); + static bool axis_is_homed(size_t axis); + static void set_all_axes_homed(); + static void set_all_axes_unhomed(); + Homing() = default; static const int AllCycles = 0; // Must be zero. @@ -44,7 +54,7 @@ namespace Machine { // The homing cycles are 1,2,3 etc. 0 means not homed as part of home-all, // but you can still home it manually with e.g. $HA - int _cycle = -1; // what auto-homing cycle does this axis home on? + int _cycle = 0; // what auto-homing cycle does this axis home on? bool _allow_single_axis = true; // Allow use of $H command on this axis bool _positiveDirection = true; float _mpos = 0.0f; // After homing this will be the mpos of the switch location diff --git a/FluidNC/src/Main.cpp b/FluidNC/src/Main.cpp index 0b7600c52..a00a18cc0 100644 --- a/FluidNC/src/Main.cpp +++ b/FluidNC/src/Main.cpp @@ -127,9 +127,11 @@ void setup() { // NOTE: The startup script will run after successful completion of the homing cycle, but // not after disabling the alarm locks. Prevents motion startup blocks from crashing into // things uncontrollably. Very bad. + Homing::set_all_axes_homed(); if (config->_start->_mustHome && Machine::Axes::homingMask) { + Homing::set_all_axes_unhomed(); // If there is an axis with homing configured, enter Alarm state on startup - sys.state = State::Alarm; + send_alarm(ExecAlarm::Unhomed); } for (auto s : config->_spindles) { s->init(); diff --git a/FluidNC/src/MotionControl.cpp b/FluidNC/src/MotionControl.cpp index fcb054391..b78999df2 100644 --- a/FluidNC/src/MotionControl.cpp +++ b/FluidNC/src/MotionControl.cpp @@ -109,165 +109,11 @@ static bool mc_linear_no_check(float* target, plan_line_data_t* pl_data, float* } bool mc_linear(float* target, plan_line_data_t* pl_data, float* position) { if (!pl_data->is_jog) { // soft limits for jogs have already been dealt with - limits_soft_check(target); - } - return mc_linear_no_check(target, pl_data, position); -} - -// Check that the arc does not exceed the soft limits using a fast -// algorithm that requires no transcendental functions. -// caxes[] depends on the plane selection via G17, G18, and G19. caxes[0] is the first -// circle plane axis, caxes[1] is the second circle plane axis, and caxes[2] is the -// orthogonal plane. So for G17 mode, caxes[] is { 0, 1, 2} for { X, Y, Z}. G18 is {2, 0, 1} i.e. {Z, X, Y}, and G19 is {1, 2, 0} i.e. {Y, Z, X} -void check_arc_limits(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) { - auto axes = config->_axes; - - // Handle the orthognal axis first to get it out of the way. - size_t the_axis = caxes[2]; - if (axes->_axis[the_axis]->_softLimits) { - float amin = std::min(position[the_axis], target[the_axis]); - if (amin < limitsMinPosition(the_axis)) { - limit_error(the_axis, amin); - return; - } - float amax = std::max(position[the_axis], target[the_axis]); - if (amax > limitsMaxPosition(the_axis)) { - limit_error(the_axis, amax); - return; - } - } - - bool limited[2] = { axes->_axis[caxes[0]]->_softLimits, axes->_axis[caxes[1]]->_softLimits }; - - // If neither axis of the circular plane has limits enabled, skip the computation - if (!(limited[0] || limited[1])) { - return; - } - - // The origin for this calculation's coordinate system is at the center of the arc. - // The 0 and 1 entries are for the circle plane - // and the 2 entry is the orthogonal (linear) direction - - float s[2], e[2]; // Start and end of arc in the circle plane, relative to center - - // Depending on the arc direction, set the arc start and end points relative - // to the arc center. Afterwards, end is always counterclockwise relative to - // start, thus simplifying the following decision tree. - if (is_clockwise_arc) { - s[0] = target[caxes[0]] - center[0]; - s[1] = target[caxes[1]] - center[1]; - e[0] = position[caxes[0]] - center[0]; - e[1] = position[caxes[1]] - center[1]; - } else { - s[0] = position[caxes[0]] - center[0]; - s[1] = position[caxes[1]] - center[1]; - e[0] = target[caxes[0]] - center[0]; - e[1] = target[caxes[1]] - center[1]; - } - - // Axis crossings - plus and minus caxes[0] and caxes[1] - bool p[2] = { false, false }; - bool m[2] = { false, false }; - - // The following decision tree determines whether the arc crosses - // the horizontal and vertical axes of the circular plane in the - // positive and negative half planes. There are ways to express - // it in fewer lines of code by converting to alternate - // representations like angles, but this way is computationally - // efficient since it avoids any use of transcendental functions. - // Every path through this decision tree is either 4 or 5 simple - // comparisons. - if (e[1] >= 0) { // End in upper half plane - if (e[0] > 0) { // End in quadrant 0 - X+ Y+ - if (s[1] >= 0) { // Start in upper half plane - if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ - if (s[0] <= e[0]) { // wraparound - p[0] = p[1] = m[0] = m[1] = true; - } - } else { // Start in quadrant 1 - X- Y+ - m[0] = m[1] = p[0] = true; - } - } else { // Start in lower half plane - if (s[0] > 0) { // Start in quadrant 3 - X+ Y- - p[0] = true; - } else { // Start in quadrant 2 - X- Y- - m[1] = p[0] = true; - } - } - } else { // End in quadrant 1 - X- Y+ - if (s[1] >= 0) { // Start in upper half plane - if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ - p[1] = true; - } else { // Start in quadrant 1 - X- Y+ - if (s[0] <= e[0]) { // wraparound - p[0] = p[1] = m[0] = m[1] = true; - } - } - } else { // Start in lower half plane - if (s[0] > 0) { // Start in quadrant 3 - X+ Y- - p[0] = p[1] = true; - } else { // Start in quadrant 2 - X- Y- - m[1] = p[0] = p[1] = true; - } - } - } - } else { // e[1] < 0 - end in lower half plane - if (e[0] > 0) { // End in quadrant 3 - X+ Y+ - if (s[1] >= 0) { // Start in upper half plane - if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ - p[1] = m[0] = m[1] = true; - } else { // Start in quadrant 1 - X- Y+ - m[0] = m[1] = true; - } - } else { // Start in lower half plane - if (s[0] > 0) { // Start in quadrant 3 - X+ Y- - if (s[0] >= e[0]) { // wraparound - p[0] = p[1] = m[0] = m[1] = true; - } - } else { // Start in quadrant 2 - X- Y- - m[1] = true; - } - } - } else { // End in quadrant 2 - X- Y+ - if (s[1] >= 0) { // Start in upper half plane - if (s[0] > 0) { // Start in quadrant 0 - X+ Y+ - p[1] = m[0] = true; - } else { // Start in quadrant 1 - X- Y+ - m[0] = true; - } - } else { // Start in lower half plane - if (s[0] > 0) { // Start in quadrant 3 - X+ Y- - p[0] = p[1] = m[0] = true; - } else { // Start in quadrant 2 - X- Y- - if (s[0] >= e[0]) { // wraparound - p[0] = p[1] = m[0] = m[1] = true; - } - } - } - } - } - - // Now check limits based on arc endpoints and axis crossings - for (size_t a = 0; a < 2; ++a) { - the_axis = caxes[a]; - if (limited[a]) { - // If we crossed the axis in the positive half plane, the - // maximum extent along that axis is at center + radius. - // Otherwise it is the maximum coordinate of the start and - // end positions. Similarly for the negative half plane - // and the minimum extent. - float amin = m[a] ? center[a] - radius : std::min(target[the_axis], position[the_axis]); - if (amin < limitsMinPosition(the_axis)) { - limit_error(the_axis, amin); - return; - } - float amax = p[a] ? center[a] + radius : std::max(target[the_axis], position[the_axis]); - if (amax > limitsMaxPosition(the_axis)) { - limit_error(the_axis, amax); - return; - } + if (config->_kinematics->invalid_line(target)) { + return false; } } + return mc_linear_no_check(target, pl_data, position); } // Execute an arc in offset mode format. position == current xyz, target == target xyz, @@ -291,7 +137,9 @@ void mc_arc(float* target, // The first two axes are the circle plane and the third is the orthogonal plane size_t caxes[3] = { axis_0, axis_1, axis_linear }; - check_arc_limits(target, position, center, radius, caxes, is_clockwise_arc); + if (config->_kinematics->invalid_arc(target, position, center, radius, caxes, is_clockwise_arc)) { + return; + } // Radius vector from center to current location float radii[2] = { -offset[axis_0], -offset[axis_1] }; diff --git a/FluidNC/src/Motors/StandardStepper.cpp b/FluidNC/src/Motors/StandardStepper.cpp index fa519ffcc..20b3aec2b 100644 --- a/FluidNC/src/Motors/StandardStepper.cpp +++ b/FluidNC/src/Motors/StandardStepper.cpp @@ -84,7 +84,9 @@ namespace MotorDrivers { _step_pin.setAttr(Pin::Attr::Output); } - _disable_pin.setAttr(Pin::Attr::Output); + if (_disable_pin.defined()) { + _disable_pin.setAttr(Pin::Attr::Output); + } } void StandardStepper::config_message() { diff --git a/FluidNC/src/Planner.cpp b/FluidNC/src/Planner.cpp index e42e2834f..c0b717448 100644 --- a/FluidNC/src/Planner.cpp +++ b/FluidNC/src/Planner.cpp @@ -306,8 +306,16 @@ bool plan_buffer_line(float* target, plan_line_data_t* pl_data) { int32_t target_steps[MAX_N_AXIS], position_steps[MAX_N_AXIS]; float unit_vec[MAX_N_AXIS], delta_mm; // Copy position data based on type of motion being planned. - copyAxes(position_steps, block->motion.systemMotion ? get_motor_steps() : pl.position); - + if (block->motion.systemMotion) { + copyAxes(position_steps, get_motor_steps()); + } else { + if (!block->is_jog && Homing::unhomed_axes()) { + log_info("Unhomed axes: " << config->_axes->maskToNames(Homing::unhomed_axes())); + send_alarm(ExecAlarm::Unhomed); + return false; + } + copyAxes(position_steps, pl.position); + } auto n_axis = config->_axes->_numberAxis; for (size_t idx = 0; idx < n_axis; idx++) { // Calculate target position in absolute steps, number of steps for each axis, and determine max step events. diff --git a/FluidNC/src/Planner.h b/FluidNC/src/Planner.h index 684c3104f..fe4d21e1f 100644 --- a/FluidNC/src/Planner.h +++ b/FluidNC/src/Planner.h @@ -12,6 +12,7 @@ #include "Config.h" // MAX_N_AXIS #include "SpindleDatatypes.h" // SpindleState #include "GCode.h" // CoolantState +#include "Types.h" // AxisMask #include diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index d73155f64..5e2991af1 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -286,6 +286,7 @@ static Error disable_alarm_lock(const char* value, WebUI::AuthenticationLevel au if (err != Error::Ok) { return err; } + Homing::set_all_axes_homed(); report_feedback_message(Message::AlarmUnlock); sys.state = State::Idle; diff --git a/FluidNC/src/Protocol.cpp b/FluidNC/src/Protocol.cpp index 587c818a5..18c87ef03 100644 --- a/FluidNC/src/Protocol.cpp +++ b/FluidNC/src/Protocol.cpp @@ -36,6 +36,7 @@ std::map AlarmNames = { { ExecAlarm::ControlPin, "Control Pin Initially On" }, { ExecAlarm::HomingAmbiguousSwitch, "Ambiguous Switch" }, { ExecAlarm::HardStop, "Hard Stop" }, + { ExecAlarm::Unhomed, "Unhomed" }, }; const char* alarmString(ExecAlarm alarmNumber) { diff --git a/FluidNC/src/Protocol.h b/FluidNC/src/Protocol.h index 2b44361dd..da11aa898 100644 --- a/FluidNC/src/Protocol.h +++ b/FluidNC/src/Protocol.h @@ -64,6 +64,7 @@ enum class ExecAlarm : uint8_t { ControlPin = 11, HomingAmbiguousSwitch = 12, HardStop = 13, + Unhomed = 14, }; extern volatile ExecAlarm lastAlarm; @@ -97,6 +98,7 @@ extern NoArgEvent motionCancelEvent; extern NoArgEvent sleepEvent; extern NoArgEvent rtResetEvent; extern NoArgEvent debugEvent; +extern NoArgEvent unhomedEvent; // extern NoArgEvent statusReportEvent; From f08f7c3ca13cb14499cf23f7d2d86808a68fd5a8 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Tue, 1 Aug 2023 10:56:09 -1000 Subject: [PATCH 05/11] Constraints to kinematics, post_homing macro, etc --- FluidNC/src/Jog.cpp | 4 +- FluidNC/src/Kinematics/Cartesian.cpp | 9 ++- FluidNC/src/Kinematics/Cartesian.h | 10 ++- FluidNC/src/Kinematics/Kinematics.cpp | 9 ++- FluidNC/src/Kinematics/Kinematics.h | 10 ++- FluidNC/src/Machine/Homing.cpp | 2 +- FluidNC/src/Machine/Macros.cpp | 31 ++++---- FluidNC/src/Machine/Macros.h | 8 +- FluidNC/src/Main.cpp | 54 +------------- FluidNC/src/MotionControl.cpp | 8 +- FluidNC/src/Parking.cpp | 1 + FluidNC/src/Planner.h | 15 ++-- FluidNC/src/ProcessSettings.cpp | 7 +- FluidNC/src/Protocol.cpp | 103 ++++++++++++++++++-------- FluidNC/src/Protocol.h | 5 ++ 15 files changed, 148 insertions(+), 128 deletions(-) diff --git a/FluidNC/src/Jog.cpp b/FluidNC/src/Jog.cpp index 7dc23136c..1768a8bdf 100644 --- a/FluidNC/src/Jog.cpp +++ b/FluidNC/src/Jog.cpp @@ -12,9 +12,7 @@ // Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog. // cancelledInflight will be set to true if was not added to parser due to a cancelJog. Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block, bool* cancelledInflight) { - if (config->_kinematics->invalid_line(gc_block->values.xyz)) { - config->_kinematics->constrain_line(gc_block->values.xyz, gc_state.position); - } + config->_kinematics->constrain_jog(gc_block->values.xyz, pl_data, gc_state.position); // Initialize planner data struct for jogging motions. // NOTE: Spindle and coolant are allowed to fully function with overrides during a jog. diff --git a/FluidNC/src/Kinematics/Cartesian.cpp b/FluidNC/src/Kinematics/Cartesian.cpp index 8735988e9..ff0ad1c8f 100644 --- a/FluidNC/src/Kinematics/Cartesian.cpp +++ b/FluidNC/src/Kinematics/Cartesian.cpp @@ -23,9 +23,11 @@ namespace Kinematics { // caxes[] depends on the plane selection via G17, G18, and G19. caxes[0] is the first // circle plane axis, caxes[1] is the second circle plane axis, and caxes[2] is the // orthogonal plane. So for G17 mode, caxes[] is { 0, 1, 2} for { X, Y, Z}. G18 is {2, 0, 1} i.e. {Z, X, Y}, and G19 is {1, 2, 0} i.e. {Y, Z, X} - bool Cartesian::invalid_arc(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) { - auto axes = config->_axes; + bool Cartesian::invalid_arc( + float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) { + pl_data->limits_checked = true; + auto axes = config->_axes; // Handle the orthognal axis first to get it out of the way. size_t the_axis = caxes[2]; if (axes->_axis[the_axis]->_softLimits) { @@ -174,7 +176,7 @@ namespace Kinematics { return false; } - void Cartesian::constrain_line(float* target, float* position) { + void Cartesian::constrain_jog(float* target, plan_line_data_t* pl_data, float* position) { auto axes = config->_axes; auto n_axis = config->_axes->_numberAxis; @@ -230,6 +232,7 @@ namespace Kinematics { log_debug("Jog constrained to axis range"); } } + pl_data->limits_checked = true; } bool Cartesian::invalid_line(float* cartesian) { diff --git a/FluidNC/src/Kinematics/Cartesian.h b/FluidNC/src/Kinematics/Cartesian.h index 2b026cd54..8ff0d660c 100644 --- a/FluidNC/src/Kinematics/Cartesian.h +++ b/FluidNC/src/Kinematics/Cartesian.h @@ -23,9 +23,15 @@ namespace Kinematics { // Kinematic Interface - virtual void constrain_line(float* cartesian, float* position) override; + virtual void constrain_jog(float* cartesian, plan_line_data_t* pl_data, float* position) override; virtual bool invalid_line(float* cartesian) override; - virtual bool invalid_arc(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) override; + virtual bool invalid_arc(float* target, + plan_line_data_t* pl_data, + float* position, + float center[3], + float radius, + size_t caxes[3], + bool is_clockwise_arc) override; virtual bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) override; virtual void init() override; diff --git a/FluidNC/src/Kinematics/Kinematics.cpp b/FluidNC/src/Kinematics/Kinematics.cpp index cb30a5ae9..d0180d466 100644 --- a/FluidNC/src/Kinematics/Kinematics.cpp +++ b/FluidNC/src/Kinematics/Kinematics.cpp @@ -8,9 +8,9 @@ #include "Cartesian.h" namespace Kinematics { - void Kinematics::constrain_line(float* target, float* position) { + void Kinematics::constrain_jog(float* target, plan_line_data_t* pl_data, float* position) { Assert(_system != nullptr, "No kinematic system"); - return _system->constrain_line(target, position); + return _system->constrain_jog(target, pl_data, position); } bool Kinematics::invalid_line(float* target) { @@ -18,9 +18,10 @@ namespace Kinematics { return _system->invalid_line(target); } - bool Kinematics::invalid_arc(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) { + bool Kinematics::invalid_arc( + float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) { Assert(_system != nullptr, "No kinematic system"); - return _system->invalid_arc(target, position, center, radius, caxes, is_clockwise_arc); + return _system->invalid_arc(target, pl_data, position, center, radius, caxes, is_clockwise_arc); } bool Kinematics::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { diff --git a/FluidNC/src/Kinematics/Kinematics.h b/FluidNC/src/Kinematics/Kinematics.h index 9add47c4c..87a7ad6a2 100644 --- a/FluidNC/src/Kinematics/Kinematics.h +++ b/FluidNC/src/Kinematics/Kinematics.h @@ -48,9 +48,10 @@ namespace Kinematics { void motors_to_cartesian(float* cartesian, float* motors, int n_axis); void transform_cartesian_to_motors(float* motors, float* cartesian); - void constrain_line(float* target, float* position); + void constrain_jog(float* target, plan_line_data_t* pl_data, float* position); bool invalid_line(float* target); - bool invalid_arc(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc); + bool invalid_arc( + float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc); bool canHome(AxisMask axisMask); void releaseMotors(AxisMask axisMask, MotorMask motors); @@ -74,9 +75,10 @@ namespace Kinematics { virtual void init() = 0; virtual void init_position() = 0; // used to set the machine position at init - virtual void constrain_line(float* cartesian, float* position) {} + virtual void constrain_jog(float* cartesian, plan_line_data_t* pl_data, float* position) {} virtual bool invalid_line(float* cartesian) { return false; } - virtual bool invalid_arc(float* target, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) { + virtual bool invalid_arc( + float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) { return false; } diff --git a/FluidNC/src/Machine/Homing.cpp b/FluidNC/src/Machine/Homing.cpp index 644b19371..cc6e2aaa5 100644 --- a/FluidNC/src/Machine/Homing.cpp +++ b/FluidNC/src/Machine/Homing.cpp @@ -59,7 +59,7 @@ namespace Machine { float target[config->_axes->_numberAxis]; axisVector(_phaseAxes, _phaseMotors, _phase, target, rate, _settling_ms); - plan_line_data_t plan_data; + plan_line_data_t plan_data = {}; plan_data.spindle_speed = 0; plan_data.motion = {}; plan_data.motion.systemMotion = 1; diff --git a/FluidNC/src/Machine/Macros.cpp b/FluidNC/src/Machine/Macros.cpp index a493af8ea..3d1042a09 100644 --- a/FluidNC/src/Machine/Macros.cpp +++ b/FluidNC/src/Machine/Macros.cpp @@ -7,14 +7,19 @@ #include "src/Machine/MachineConfig.h" // config void MacroEvent::run(void* arg) { + int n = int(arg); if (sys.state != State::Idle) { log_error("Macro can only be used in idle state"); return; } - log_debug("Macro " << _num); - config->_macros->run_macro(_num); + log_debug("Macro " << n); + config->_macros->run_macro(n); } +std::string Macros::_startup_line[n_startup_lines]; +std::string Macros::_macro[n_macros]; +std::string Macros::_post_homing_line; + MacroEvent macro0Event { 0 }; MacroEvent macro1Event { 1 }; MacroEvent macro2Event { 2 }; @@ -47,18 +52,18 @@ Cmd findOverride(std::string name) { return it == overrideCodes.end() ? Cmd::None : it->second; } -bool Macros::run_macro(size_t index) { - if (index >= n_macros) { - return false; - } - auto macro = _macro[index]; - if (macro == "") { +bool Macros::run_macro(size_t n) { + return run_macro(_macro[n]); +} + +bool Macros::run_macro(const std::string& s) { + if (s == "") { return true; } char c; - for (int i = 0; i < macro.length(); i++) { - c = macro[i]; + for (int i = 0; i < s.length(); i++) { + c = s[i]; switch (c) { case '&': // & is a proxy for newlines in macros, because you cannot @@ -66,13 +71,13 @@ bool Macros::run_macro(size_t index) { WebUI::inputBuffer.push('\n'); break; case '#': - if ((i + 3) > macro.length()) { + if ((i + 3) > s.length()) { log_error("Missing characters after # realtime escape in macro"); return false; } { - std::string s(macro.c_str() + i + 1, 2); - Cmd cmd = findOverride(s); + std::string s1(s.c_str() + i + 1, 2); + Cmd cmd = findOverride(s1); if (cmd == Cmd::None) { log_error("Bad #xx realtime escape in macro"); return false; diff --git a/FluidNC/src/Machine/Macros.h b/FluidNC/src/Machine/Macros.h index ac785b10f..825ba6acc 100644 --- a/FluidNC/src/Machine/Macros.h +++ b/FluidNC/src/Machine/Macros.h @@ -30,13 +30,16 @@ namespace Machine { static const int n_macros = 4; private: - std::string _startup_line[n_startup_lines]; - std::string _macro[n_macros]; + static std::string _startup_line[n_startup_lines]; + static std::string _macro[n_macros]; public: + static std::string _post_homing_line; + Macros() = default; bool run_macro(size_t index); + bool run_macro(const std::string& s); std::string startup_line(size_t index) { if (index >= n_startup_lines) { @@ -63,6 +66,7 @@ namespace Machine { handler.item("macro1", _macro[1]); handler.item("macro2", _macro[2]); handler.item("macro3", _macro[3]); + handler.item("post_homing", _post_homing_line); } ~Macros() {} diff --git a/FluidNC/src/Main.cpp b/FluidNC/src/Main.cpp index a00a18cc0..0035bf71b 100644 --- a/FluidNC/src/Main.cpp +++ b/FluidNC/src/Main.cpp @@ -107,32 +107,12 @@ void setup() { config->_control->init(); config->_kinematics->init(); + + limits_init(); } // Initialize system state. if (sys.state != State::ConfigAlarm) { - if (FORCE_INITIALIZATION_ALARM) { - // Force ALARM state upon a power-cycle or hard reset. - sys.state = State::Alarm; - } else { - sys.state = State::Idle; - } - - limits_init(); - - // Check for power-up and set system alarm if homing is enabled to force homing cycle - // by setting alarm state. Alarm locks out all g-code commands, including the - // startup scripts, but allows access to settings and internal commands. Only a homing - // cycle '$H' or kill alarm locks '$X' will disable the alarm. - // NOTE: The startup script will run after successful completion of the homing cycle, but - // not after disabling the alarm locks. Prevents motion startup blocks from crashing into - // things uncontrollably. Very bad. - Homing::set_all_axes_homed(); - if (config->_start->_mustHome && Machine::Axes::homingMask) { - Homing::set_all_axes_unhomed(); - // If there is an axis with homing configured, enter Alarm state on startup - send_alarm(ExecAlarm::Unhomed); - } for (auto s : config->_spindles) { s->init(); } @@ -154,40 +134,12 @@ void setup() { } allChannels.deregistration(&startupLog); -} - -static void reset_variables() { - // Reset primary systems. - system_reset(); - protocol_reset(); - gc_init(); // Set g-code parser to default state - // Spindle should be set either by the configuration - // or by the post-configuration fixup, but we test - // it anyway just for safety. We want to avoid any - // possibility of crashing at this point. - - plan_reset(); // Clear block buffer and planner variables - - if (sys.state != State::ConfigAlarm) { - if (spindle) { - spindle->stop(); - report_ovr_counter = 0; // Set to report change immediately - } - Stepper::reset(); // Clear stepper subsystem variables - } - - // Sync cleared gcode and planner positions to current system position. - plan_sync_position(); - gc_sync_position(); - allChannels.flushRx(); - report_init_message(allChannels); - mc_init(); + protocol_send_event(&startEvent); } void loop() { static int tries = 0; try { - reset_variables(); // Start the main loop. Processes program inputs and executes them. // This can exit on a system abort condition, in which case run_once() // is re-executed by an enclosing loop. It can also exit via a diff --git a/FluidNC/src/MotionControl.cpp b/FluidNC/src/MotionControl.cpp index b78999df2..86d69bb39 100644 --- a/FluidNC/src/MotionControl.cpp +++ b/FluidNC/src/MotionControl.cpp @@ -108,7 +108,7 @@ static bool mc_linear_no_check(float* target, plan_line_data_t* pl_data, float* return config->_kinematics->cartesian_to_motors(target, pl_data, position); } bool mc_linear(float* target, plan_line_data_t* pl_data, float* position) { - if (!pl_data->is_jog) { // soft limits for jogs have already been dealt with + if (!pl_data->is_jog && !pl_data->limits_checked) { // soft limits for jogs have already been dealt with if (config->_kinematics->invalid_line(target)) { return false; } @@ -137,7 +137,7 @@ void mc_arc(float* target, // The first two axes are the circle plane and the third is the orthogonal plane size_t caxes[3] = { axis_0, axis_1, axis_linear }; - if (config->_kinematics->invalid_arc(target, position, center, radius, caxes, is_clockwise_arc)) { + if (config->_kinematics->invalid_arc(target, pl_data, position, center, radius, caxes, is_clockwise_arc)) { return; } @@ -251,7 +251,7 @@ void mc_arc(float* target, position[i] += linear_per_segment[i]; } pl_data->feed_rate = original_feedrate; // This restores the feedrate kinematics may have altered - mc_linear_no_check(position, pl_data, previous_position); + mc_linear(position, pl_data, previous_position); previous_position[axis_0] = position[axis_0]; previous_position[axis_1] = position[axis_1]; previous_position[axis_linear] = position[axis_linear]; @@ -262,7 +262,7 @@ void mc_arc(float* target, } } // Ensure last segment arrives at target location. - mc_linear_no_check(target, pl_data, previous_position); + mc_linear(target, pl_data, previous_position); } // Execute dwell in seconds. diff --git a/FluidNC/src/Parking.cpp b/FluidNC/src/Parking.cpp index 2eaf390a6..a5f7584fb 100644 --- a/FluidNC/src/Parking.cpp +++ b/FluidNC/src/Parking.cpp @@ -64,6 +64,7 @@ void Parking::setup() { plan_data.motion.systemMotion = 1; plan_data.motion.noFeedOverride = 1; plan_data.line_number = PARKING_MOTION_LINE_NUMBER; + plan_data.is_jog = false; block = plan_get_current_block(); if (block) { diff --git a/FluidNC/src/Planner.h b/FluidNC/src/Planner.h index fe4d21e1f..888999734 100644 --- a/FluidNC/src/Planner.h +++ b/FluidNC/src/Planner.h @@ -62,13 +62,14 @@ struct plan_block_t { // Planner data prototype. Must be used when passing new motions to the planner. struct plan_line_data_t { - float feed_rate; // Desired feed rate for line motion. Value is ignored, if rapid motion. - SpindleSpeed spindle_speed; // Desired spindle speed through line motion. - PlMotion motion; // Bitflag variable to indicate motion conditions. See defines above. - SpindleState spindle; // Spindle enable state - CoolantState coolant; // Coolant state - int32_t line_number; // Desired line number to report when executing. - bool is_jog; // true if this was generated due to a jog command + float feed_rate; // Desired feed rate for line motion. Value is ignored, if rapid motion. + SpindleSpeed spindle_speed; // Desired spindle speed through line motion. + PlMotion motion; // Bitflag variable to indicate motion conditions. See defines above. + SpindleState spindle; // Spindle enable state + CoolantState coolant; // Coolant state + int32_t line_number; // Desired line number to report when executing. + bool is_jog; // true if this was generated due to a jog command + bool limits_checked; // true if soft limits already checked }; void plan_init(); diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index 5e2991af1..4f9b5fc48 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -329,7 +329,7 @@ static Error home(AxisMask axisMask) { protocol_execute_realtime(); } while (sys.state == State::Homing); - settings_execute_startup(); + config->_macros->run_macro(Macros::_post_homing_line); return Error::Ok; } @@ -988,6 +988,9 @@ Error settings_execute_line(char* line, Channel& out, WebUI::AuthenticationLevel } void settings_execute_startup() { + if (sys.state != State::Idle) { + return; + } Error status_code; for (int i = 0; i < config->_macros->n_startup_lines; i++) { auto str = config->_macros->startup_line(i); @@ -997,7 +1000,7 @@ void settings_execute_startup() { char gcline[256]; strncpy(gcline, str.c_str(), 255); status_code = gc_execute_line(gcline); - // Uart0 << ">" << gcline << ":"; + // Uart0 << ">" << gcline << "\n"; if (status_code != Error::Ok) { log_error("Startup line: " << errorString(status_code)); } diff --git a/FluidNC/src/Protocol.cpp b/FluidNC/src/Protocol.cpp index 18c87ef03..008136320 100644 --- a/FluidNC/src/Protocol.cpp +++ b/FluidNC/src/Protocol.cpp @@ -37,6 +37,7 @@ std::map AlarmNames = { { ExecAlarm::HomingAmbiguousSwitch, "Ambiguous Switch" }, { ExecAlarm::HardStop, "Hard Stop" }, { ExecAlarm::Unhomed, "Unhomed" }, + { ExecAlarm::Init, "Init" }, }; const char* alarmString(ExecAlarm alarmNumber) { @@ -234,39 +235,12 @@ static void alarm_msg(ExecAlarm alarm_code) { delay_ms(500); // Force delay to ensure message clears serial write buffer. } -static void check_startup_state() { - // Check for and report alarm state after a reset, error, or an initial power up. - // NOTE: Sleep mode disables the stepper drivers and position can't be guaranteed. - // Re-initialize the sleep state as an ALARM mode to ensure user homes or acknowledges. - if (sys.state == State::ConfigAlarm) { - report_error_message(Message::ConfigAlarmLock); - } else { - // Perform some machine checks to make sure everything is good to go. - if (config->_start->_checkLimits) { - if (config->_axes->hasHardLimits() && limits_get_state()) { - sys.state = State::Alarm; // Ensure alarm state is active. - alarm_msg(ExecAlarm::HardLimit); - report_error_message(Message::CheckLimits); - } - } - if (config->_control->startup_check()) { - send_alarm(ExecAlarm::ControlPin); - } else if (sys.state == State::Alarm || sys.state == State::Sleep) { - report_feedback_message(Message::AlarmLock); - sys.state = State::Alarm; // Ensure alarm state is set. - } else { - // All systems go! - sys.state = State::Idle; - settings_execute_startup(); // Execute startup script. - } - } -} +static void check_startup_state() {} const uint32_t heapWarnThreshold = 15000; uint32_t heapLowWater = UINT_MAX; void protocol_main_loop() { - check_startup_state(); start_polling(); // --------------------------------------------------------------------------------- @@ -368,9 +342,71 @@ void protocol_execute_realtime() { } } -// Executes run-time commands, when required. This function is the primary state -// machine that controls the various real-time features. -// NOTE: Do not alter this unless you know exactly what you are doing! +static void protocol_run_startup_lines() { + settings_execute_startup(); // Execute startup script. +} + +static void protocol_do_restart() { + // Reset primary systems. + system_reset(); + protocol_reset(); + gc_init(); // Set g-code parser to default state + // Spindle should be set either by the configuration + // or by the post-configuration fixup, but we test + // it anyway just for safety. We want to avoid any + // possibility of crashing at this point. + + plan_reset(); // Clear block buffer and planner variables + + if (sys.state != State::ConfigAlarm) { + if (spindle) { + spindle->stop(); + report_ovr_counter = 0; // Set to report change immediately + } + Stepper::reset(); // Clear stepper subsystem variables + } + + // Sync cleared gcode and planner positions to current system position. + plan_sync_position(); + gc_sync_position(); + allChannels.flushRx(); + report_init_message(allChannels); + mc_init(); + sys.state = State::Idle; + + // Check for and report alarm state after a reset, error, or an initial power up. + // NOTE: Sleep mode disables the stepper drivers and position can't be guaranteed. + // Re-initialize the sleep state as an ALARM mode to ensure user homes or acknowledges. + if (sys.state == State::ConfigAlarm) { + report_error_message(Message::ConfigAlarmLock); + return; + } + + // Perform some machine checks to make sure everything is good to go. + if (config->_start->_checkLimits && config->_axes->hasHardLimits() && limits_get_state()) { + mc_critical(ExecAlarm::HardLimit); + } else if (config->_control->startup_check()) { + send_alarm(ExecAlarm::ControlPin); + } +} + +static void protocol_do_start() { + protocol_send_event(&restartEvent); + sys.state = State::Critical; + if (FORCE_INITIALIZATION_ALARM) { + // Force ALARM state upon a power-cycle or hard reset. + send_alarm(ExecAlarm::Init); + return; + } + Homing::set_all_axes_homed(); + if (config->_start->_mustHome && Machine::Axes::homingMask) { + Homing::set_all_axes_unhomed(); + // If there is an axis with homing configured, enter Alarm state on startup + send_alarm(ExecAlarm::Unhomed); + } + protocol_send_event(&runStartupLinesEvent); +} + static void protocol_do_alarm(void* alarmVoid) { lastAlarm = (ExecAlarm)((int)alarmVoid); if (spindle->_off_on_alarm) { @@ -1008,7 +1044,7 @@ void protocol_do_rt_reset() { sys.state == State::Alarm; } protocol_do_late_reset(); - sys.abort = true; // Trigger system abort. + protocol_send_event(&restartEvent); } ArgEvent feedOverrideEvent { protocol_do_feed_override }; @@ -1027,6 +1063,9 @@ NoArgEvent cycleStopEvent { protocol_do_cycle_stop }; NoArgEvent motionCancelEvent { protocol_do_motion_cancel }; NoArgEvent sleepEvent { protocol_do_sleep }; NoArgEvent debugEvent { report_realtime_debug }; +NoArgEvent startEvent { protocol_do_start }; +NoArgEvent restartEvent { protocol_do_restart }; +NoArgEvent runStartupLinesEvent { protocol_run_startup_lines }; NoArgEvent rtResetEvent { protocol_do_rt_reset }; diff --git a/FluidNC/src/Protocol.h b/FluidNC/src/Protocol.h index da11aa898..331e5d677 100644 --- a/FluidNC/src/Protocol.h +++ b/FluidNC/src/Protocol.h @@ -65,6 +65,7 @@ enum class ExecAlarm : uint8_t { HomingAmbiguousSwitch = 12, HardStop = 13, Unhomed = 14, + Init = 15, }; extern volatile ExecAlarm lastAlarm; @@ -99,6 +100,10 @@ extern NoArgEvent sleepEvent; extern NoArgEvent rtResetEvent; extern NoArgEvent debugEvent; extern NoArgEvent unhomedEvent; +extern NoArgEvent startEvent; +extern NoArgEvent restartEvent; + +extern NoArgEvent runStartupLinesEvent; // extern NoArgEvent statusReportEvent; From 0495d9ccaaa831b0c57d6d8fcad63b0f6c6d0986 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 3 Aug 2023 09:48:07 -1000 Subject: [PATCH 06/11] Propagate Alarm state across ctrl-x resets --- FluidNC/src/Protocol.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/FluidNC/src/Protocol.cpp b/FluidNC/src/Protocol.cpp index 008136320..4235f8200 100644 --- a/FluidNC/src/Protocol.cpp +++ b/FluidNC/src/Protocol.cpp @@ -372,7 +372,6 @@ static void protocol_do_restart() { allChannels.flushRx(); report_init_message(allChannels); mc_init(); - sys.state = State::Idle; // Check for and report alarm state after a reset, error, or an initial power up. // NOTE: Sleep mode disables the stepper drivers and position can't be guaranteed. @@ -403,6 +402,8 @@ static void protocol_do_start() { Homing::set_all_axes_unhomed(); // If there is an axis with homing configured, enter Alarm state on startup send_alarm(ExecAlarm::Unhomed); + } else { + sys.state = State::Idle; } protocol_send_event(&runStartupLinesEvent); } @@ -1038,10 +1039,11 @@ void protocol_do_rt_reset() { Machine::Homing::fail(ExecAlarm::HomingFailReset); } else if (sys.state == State::Cycle || sys.state == State::Jog || sys.step_control.executeHold || sys.step_control.executeSysMotion) { Stepper::stop_stepping(); // Stop stepping immediately, possibly losing position - // Probably unnecessary because system will restart soon - // send_alarm(ExecAlarm::AbortCycle); + protocol_do_alarm((void*)ExecAlarm::AbortCycle); } else if (sys.state == State::Critical) { - sys.state == State::Alarm; + sys.state = State::Alarm; + } else { + sys.state = State::Idle; } protocol_do_late_reset(); protocol_send_event(&restartEvent); @@ -1091,7 +1093,6 @@ void protocol_send_event(Event* evt, void* arg) { void protocol_handle_events() { EventItem item; while (xQueueReceive(event_queue, &item, 0)) { - // log_debug("event"); item.event->run(item.arg); } } From 2f02a9f6b8d1e8c0f2706f24f49988d106e803ee Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 3 Aug 2023 10:12:25 -1000 Subject: [PATCH 07/11] Startup lines behave like other macros, with a log_info message --- FluidNC/src/Machine/Macros.cpp | 1 + FluidNC/src/Machine/Macros.h | 16 +--------------- FluidNC/src/ProcessSettings.cpp | 11 +++++++---- 3 files changed, 9 insertions(+), 19 deletions(-) diff --git a/FluidNC/src/Machine/Macros.cpp b/FluidNC/src/Machine/Macros.cpp index 3d1042a09..2a058375e 100644 --- a/FluidNC/src/Machine/Macros.cpp +++ b/FluidNC/src/Machine/Macros.cpp @@ -61,6 +61,7 @@ bool Macros::run_macro(const std::string& s) { return true; } + log_info("Running macro: " << s); char c; for (int i = 0; i < s.length(); i++) { c = s[i]; diff --git a/FluidNC/src/Machine/Macros.h b/FluidNC/src/Machine/Macros.h index 825ba6acc..ec3081d3d 100644 --- a/FluidNC/src/Machine/Macros.h +++ b/FluidNC/src/Machine/Macros.h @@ -30,10 +30,10 @@ namespace Machine { static const int n_macros = 4; private: - static std::string _startup_line[n_startup_lines]; static std::string _macro[n_macros]; public: + static std::string _startup_line[n_startup_lines]; static std::string _post_homing_line; Macros() = default; @@ -41,20 +41,6 @@ namespace Machine { bool run_macro(size_t index); bool run_macro(const std::string& s); - std::string startup_line(size_t index) { - if (index >= n_startup_lines) { - return ""; - } - auto 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. - std::replace(s.begin(), s.end(), '&', '\n'); - return s + "\n"; - } - // Configuration helpers: // TODO: We could validate the startup lines diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index 4f9b5fc48..9aad47233 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -440,9 +440,9 @@ static Error get_report_build_info(const char* value, WebUI::AuthenticationLevel } return Error::InvalidStatement; } -static Error report_startup_lines(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { +static Error show_startup_lines(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { for (int i = 0; i < config->_macros->n_startup_lines; i++) { - log_to(out, "$N", i << "=" << config->_macros->startup_line(i)); + log_to(out, "$N", i << "=" << config->_macros->_startup_line[i]); } return Error::Ok; } @@ -798,7 +798,7 @@ void make_user_commands() { new UserCommand("SLP", "System/Sleep", go_to_sleep, notIdleOrAlarm); new UserCommand("I", "Build/Info", get_report_build_info, notIdleOrAlarm); - new UserCommand("N", "GCode/StartupLines", report_startup_lines, notIdleOrAlarm); + new UserCommand("N", "GCode/StartupLines", show_startup_lines, notIdleOrAlarm); new UserCommand("RST", "Settings/Restore", restore_settings, notIdleOrAlarm, WA); new UserCommand("Heap", "Heap/Show", showHeap, anyState); @@ -993,8 +993,10 @@ void settings_execute_startup() { } Error status_code; for (int i = 0; i < config->_macros->n_startup_lines; i++) { - auto str = config->_macros->startup_line(i); + auto str = config->_macros->_startup_line[i]; if (str.length()) { + config->_macros->run_macro(str); +#if 0 // We have to copy this to a mutable array because // gc_execute_line modifies the line while parsing. char gcline[256]; @@ -1004,6 +1006,7 @@ void settings_execute_startup() { if (status_code != Error::Ok) { log_error("Startup line: " << errorString(status_code)); } +#endif } } } From 50f06ab2b0a1070cf8162d76f3f7537b9a417de3 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 3 Aug 2023 10:42:07 -1000 Subject: [PATCH 08/11] after_homing, after_unlock, after_reset macros --- FluidNC/src/Machine/Macros.cpp | 4 +++- FluidNC/src/Machine/Macros.h | 8 ++++++-- FluidNC/src/ProcessSettings.cpp | 4 ++-- FluidNC/src/Protocol.cpp | 4 ++++ 4 files changed, 15 insertions(+), 5 deletions(-) diff --git a/FluidNC/src/Machine/Macros.cpp b/FluidNC/src/Machine/Macros.cpp index 2a058375e..588c9f858 100644 --- a/FluidNC/src/Machine/Macros.cpp +++ b/FluidNC/src/Machine/Macros.cpp @@ -18,7 +18,9 @@ void MacroEvent::run(void* arg) { std::string Macros::_startup_line[n_startup_lines]; std::string Macros::_macro[n_macros]; -std::string Macros::_post_homing_line; +std::string Macros::_after_homing_line; +std::string Macros::_after_reset_line; +std::string Macros::_after_unlock_line; MacroEvent macro0Event { 0 }; MacroEvent macro1Event { 1 }; diff --git a/FluidNC/src/Machine/Macros.h b/FluidNC/src/Machine/Macros.h index ec3081d3d..f22cde18c 100644 --- a/FluidNC/src/Machine/Macros.h +++ b/FluidNC/src/Machine/Macros.h @@ -34,7 +34,9 @@ namespace Machine { public: static std::string _startup_line[n_startup_lines]; - static std::string _post_homing_line; + static std::string _after_homing_line; + static std::string _after_reset_line; + static std::string _after_unlock_line; Macros() = default; @@ -52,7 +54,9 @@ namespace Machine { handler.item("macro1", _macro[1]); handler.item("macro2", _macro[2]); handler.item("macro3", _macro[3]); - handler.item("post_homing", _post_homing_line); + handler.item("after_homing", _after_homing_line); + handler.item("after_reset", _after_reset_line); + handler.item("after_unlock", _after_unlock_line); } ~Macros() {} diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index 9aad47233..2d1f7014e 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -290,7 +290,7 @@ static Error disable_alarm_lock(const char* value, WebUI::AuthenticationLevel au report_feedback_message(Message::AlarmUnlock); sys.state = State::Idle; - // Don't run startup script. Prevents stored moves in startup from causing accidents. + config->_macros->run_macro(Macros::_after_unlock_line); } // Otherwise, no effect. return Error::Ok; } @@ -329,7 +329,7 @@ static Error home(AxisMask axisMask) { protocol_execute_realtime(); } while (sys.state == State::Homing); - config->_macros->run_macro(Macros::_post_homing_line); + config->_macros->run_macro(Macros::_after_homing_line); return Error::Ok; } diff --git a/FluidNC/src/Protocol.cpp b/FluidNC/src/Protocol.cpp index 4235f8200..efea0c3f8 100644 --- a/FluidNC/src/Protocol.cpp +++ b/FluidNC/src/Protocol.cpp @@ -386,6 +386,10 @@ static void protocol_do_restart() { mc_critical(ExecAlarm::HardLimit); } else if (config->_control->startup_check()) { send_alarm(ExecAlarm::ControlPin); + } else { + if (sys.state == State::Idle) { + config->_macros->run_macro(Macros::_after_reset_line); + } } } From 3b4cc9f5d1f790f51ee450d14c5f8337893758b2 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Fri, 4 Aug 2023 08:47:10 -1000 Subject: [PATCH 09/11] SoftLimits do not require $x --- FluidNC/src/Protocol.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/FluidNC/src/Protocol.cpp b/FluidNC/src/Protocol.cpp index efea0c3f8..cf1cc87ee 100644 --- a/FluidNC/src/Protocol.cpp +++ b/FluidNC/src/Protocol.cpp @@ -418,10 +418,16 @@ static void protocol_do_alarm(void* alarmVoid) { spindle->stop(); } alarm_msg(lastAlarm); - if (lastAlarm == ExecAlarm::HardLimit || lastAlarm == ExecAlarm::SoftLimit || lastAlarm == ExecAlarm::HardStop) { + if (lastAlarm == ExecAlarm::HardLimit || lastAlarm == ExecAlarm::HardStop) { sys.state = State::Critical; // Set system alarm state report_error_message(Message::CriticalEvent); protocol_disable_steppers(); + Homing::set_all_axes_unhomed(); + return; + } + if (lastAlarm == ExecAlarm::SoftLimit) { + sys.state = State::Critical; // Set system alarm state + report_error_message(Message::CriticalEvent); return; } sys.state = State::Alarm; @@ -1045,8 +1051,12 @@ void protocol_do_rt_reset() { Stepper::stop_stepping(); // Stop stepping immediately, possibly losing position protocol_do_alarm((void*)ExecAlarm::AbortCycle); } else if (sys.state == State::Critical) { - sys.state = State::Alarm; - } else { + if (Homing::unhomed_axes()) { + protocol_do_alarm((void*)ExecAlarm::Unhomed); + } else { + sys.state = State::Idle; + } + } else if (sys.state != State::Alarm) { sys.state = State::Idle; } protocol_do_late_reset(); From db1e68a696a8f3da7ef1bd1d94bb123380634242 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Sat, 5 Aug 2023 14:15:07 -1000 Subject: [PATCH 10/11] Created a Macro class for easier use of macros --- FluidNC/src/Machine/Macros.cpp | 32 +++++++++++-------------- FluidNC/src/Machine/Macros.h | 42 +++++++++++++++++---------------- FluidNC/src/ProcessSettings.cpp | 26 +++++--------------- FluidNC/src/Protocol.cpp | 2 +- 4 files changed, 43 insertions(+), 59 deletions(-) diff --git a/FluidNC/src/Machine/Macros.cpp b/FluidNC/src/Machine/Macros.cpp index 588c9f858..f58379e73 100644 --- a/FluidNC/src/Machine/Macros.cpp +++ b/FluidNC/src/Machine/Macros.cpp @@ -12,15 +12,14 @@ void MacroEvent::run(void* arg) { log_error("Macro can only be used in idle state"); return; } - log_debug("Macro " << n); - config->_macros->run_macro(n); + config->_macros->_macro[n].run(); } -std::string Macros::_startup_line[n_startup_lines]; -std::string Macros::_macro[n_macros]; -std::string Macros::_after_homing_line; -std::string Macros::_after_reset_line; -std::string Macros::_after_unlock_line; +Macro Macros::_startup_line[n_startup_lines] = { { "startup_line0" }, { "startup_line1" } }; +Macro Macros::_macro[n_macros] = { { "macro0" }, { "macro1" }, { "macro2" }, { "macro3" } }; +Macro Macros::_after_homing = { "after_homing" }; +Macro Macros::_after_reset = { "after_reset" }; +Macro Macros::_after_unlock = { "after_unlock" }; MacroEvent macro0Event { 0 }; MacroEvent macro1Event { 1 }; @@ -54,19 +53,16 @@ Cmd findOverride(std::string name) { return it == overrideCodes.end() ? Cmd::None : it->second; } -bool Macros::run_macro(size_t n) { - return run_macro(_macro[n]); -} - -bool Macros::run_macro(const std::string& s) { - if (s == "") { +bool Macro::run() { + const std::string& s = _gcode; + if (_gcode == "") { return true; } - log_info("Running macro: " << s); + log_info("Running macro " << _name << ": " << _gcode); char c; - for (int i = 0; i < s.length(); i++) { - c = s[i]; + for (int i = 0; i < _gcode.length(); i++) { + c = _gcode[i]; switch (c) { case '&': // & is a proxy for newlines in macros, because you cannot @@ -74,12 +70,12 @@ bool Macros::run_macro(const std::string& s) { WebUI::inputBuffer.push('\n'); break; case '#': - if ((i + 3) > s.length()) { + if ((i + 3) > _gcode.length()) { log_error("Missing characters after # realtime escape in macro"); return false; } { - std::string s1(s.c_str() + i + 1, 2); + std::string s1(_gcode.c_str() + i + 1, 2); Cmd cmd = findOverride(s1); if (cmd == Cmd::None) { log_error("Bad #xx realtime escape in macro"); diff --git a/FluidNC/src/Machine/Macros.h b/FluidNC/src/Machine/Macros.h index f22cde18c..a462bc0c6 100644 --- a/FluidNC/src/Machine/Macros.h +++ b/FluidNC/src/Machine/Macros.h @@ -24,39 +24,41 @@ extern MacroEvent macro2Event; extern MacroEvent macro3Event; namespace Machine { + class Macro { + public: + std::string _gcode; + std::string _name; + Macro(const char* name) : _name(name) {} + bool run(); + }; + class Macros : public Configuration::Configurable { public: static const int n_startup_lines = 2; static const int n_macros = 4; - private: - static std::string _macro[n_macros]; - - public: - static std::string _startup_line[n_startup_lines]; - static std::string _after_homing_line; - static std::string _after_reset_line; - static std::string _after_unlock_line; + static Macro _macro[n_macros]; + static Macro _startup_line[n_startup_lines]; + static Macro _after_homing; + static Macro _after_reset; + static Macro _after_unlock; Macros() = default; - bool run_macro(size_t index); - bool run_macro(const std::string& s); - // Configuration helpers: // TODO: We could validate the startup lines void group(Configuration::HandlerBase& handler) override { - handler.item("startup_line0", _startup_line[0]); - handler.item("startup_line1", _startup_line[1]); - handler.item("macro0", _macro[0]); - handler.item("macro1", _macro[1]); - handler.item("macro2", _macro[2]); - handler.item("macro3", _macro[3]); - handler.item("after_homing", _after_homing_line); - handler.item("after_reset", _after_reset_line); - handler.item("after_unlock", _after_unlock_line); + handler.item(_startup_line[0]._name.c_str(), _startup_line[0]._gcode); + handler.item(_startup_line[1]._name.c_str(), _startup_line[1]._gcode); + handler.item(_macro[0]._name.c_str(), _macro[0]._gcode); + handler.item(_macro[1]._name.c_str(), _macro[1]._gcode); + handler.item(_macro[2]._name.c_str(), _macro[2]._gcode); + handler.item(_macro[3]._name.c_str(), _macro[3]._gcode); + handler.item(_after_homing._name.c_str(), _after_homing._gcode); + handler.item(_after_reset._name.c_str(), _after_reset._gcode); + handler.item(_after_unlock._name.c_str(), _after_unlock._gcode); } ~Macros() {} diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index 2d1f7014e..fccfd91ff 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -290,7 +290,7 @@ static Error disable_alarm_lock(const char* value, WebUI::AuthenticationLevel au report_feedback_message(Message::AlarmUnlock); sys.state = State::Idle; - config->_macros->run_macro(Macros::_after_unlock_line); + config->_macros->_after_unlock.run(); } // Otherwise, no effect. return Error::Ok; } @@ -329,7 +329,7 @@ static Error home(AxisMask axisMask) { protocol_execute_realtime(); } while (sys.state == State::Homing); - config->_macros->run_macro(Macros::_after_homing_line); + config->_macros->_after_homing.run(); return Error::Ok; } @@ -442,7 +442,7 @@ static Error get_report_build_info(const char* value, WebUI::AuthenticationLevel } static Error show_startup_lines(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { for (int i = 0; i < config->_macros->n_startup_lines; i++) { - log_to(out, "$N", i << "=" << config->_macros->_startup_line[i]); + log_to(out, "$N", i << "=" << config->_macros->_startup_line[i]._gcode); } return Error::Ok; } @@ -597,9 +597,9 @@ static Error motors_init(const char* value, WebUI::AuthenticationLevel auth_leve static Error macros_run(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { if (value) { - log_info("Running macro " << *value); + log_info("Running macro" << *value); size_t macro_num = (*value) - '0'; - config->_macros->run_macro(macro_num); + config->_macros->_macro[macro_num].run(); return Error::Ok; } log_error("$Macros/Run requires a macro number argument"); @@ -993,21 +993,7 @@ void settings_execute_startup() { } Error status_code; for (int i = 0; i < config->_macros->n_startup_lines; i++) { - auto str = config->_macros->_startup_line[i]; - if (str.length()) { - config->_macros->run_macro(str); -#if 0 - // We have to copy this to a mutable array because - // gc_execute_line modifies the line while parsing. - char gcline[256]; - strncpy(gcline, str.c_str(), 255); - status_code = gc_execute_line(gcline); - // Uart0 << ">" << gcline << "\n"; - if (status_code != Error::Ok) { - log_error("Startup line: " << errorString(status_code)); - } -#endif - } + config->_macros->_startup_line[i].run(); } } diff --git a/FluidNC/src/Protocol.cpp b/FluidNC/src/Protocol.cpp index cf1cc87ee..62af92dee 100644 --- a/FluidNC/src/Protocol.cpp +++ b/FluidNC/src/Protocol.cpp @@ -388,7 +388,7 @@ static void protocol_do_restart() { send_alarm(ExecAlarm::ControlPin); } else { if (sys.state == State::Idle) { - config->_macros->run_macro(Macros::_after_reset_line); + config->_macros->_after_reset.run(); } } } From 164a09ab76164c9419bd561c897989cb3f77bdd5 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Sat, 5 Aug 2023 14:22:19 -1000 Subject: [PATCH 11/11] Run the after_unlock macro even if no unlock was necessary --- FluidNC/src/ProcessSettings.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index fccfd91ff..467556244 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -289,9 +289,9 @@ static Error disable_alarm_lock(const char* value, WebUI::AuthenticationLevel au Homing::set_all_axes_homed(); report_feedback_message(Message::AlarmUnlock); sys.state = State::Idle; - - config->_macros->_after_unlock.run(); - } // Otherwise, no effect. + } + // Run the after_unlock macro even if no unlock was necessary + config->_macros->_after_unlock.run(); return Error::Ok; } static Error report_ngc(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) {