From 51915383a67d675f17baf715cc769f58eceaa181 Mon Sep 17 00:00:00 2001 From: Joacim Breiler Date: Sat, 16 Jul 2022 06:43:03 +0200 Subject: [PATCH 1/8] Disable CR-adding when transferring files via XModem (#528) * Disable CR-adding when transferring files via XModem The UART channel have the feature for adding extra CR on line feeds enabled which will break the XModem protocol. This fix will disable the extra CR adding and restore the setting after the file is sent. * Changed variable name to be consistent with other code. --- FluidNC/src/ProcessSettings.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index bb203d8de..cd25e4968 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -644,8 +644,10 @@ static Error xmodem_send(const char* value, WebUI::AuthenticationLevel auth_leve log_info("Cannot open " << value); return Error::DownloadFailed; } + bool oldCr = out.setCr(false); log_info("Sending " << value << " via XModem"); int size = xmodemTransmit(&out, infile); + out.setCr(oldCr); delete infile; if (size >= 0) { log_info("Sent " << size << " bytes"); From 4242c76992683088818a2c64b4b2067af06f0ea1 Mon Sep 17 00:00:00 2001 From: Joacim Breiler Date: Fri, 5 Aug 2022 09:26:44 +0200 Subject: [PATCH 2/8] Fixed problem with held packet length According to the xmodem-spec a packet may be 128 or 1024 bytes and can be switched at any time. In order to flush the packet we need to store the received packet length that is held until the next flush. --- FluidNC/src/xmodem.cpp | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/FluidNC/src/xmodem.cpp b/FluidNC/src/xmodem.cpp index 247569e8b..c9e6fca85 100644 --- a/FluidNC/src/xmodem.cpp +++ b/FluidNC/src/xmodem.cpp @@ -33,7 +33,6 @@ */ #include "xmodem.h" -// #include static Channel* serialPort; static Print* file; @@ -126,36 +125,35 @@ static void flushinput(void) { // control-Z's. Doing the control-Z removal only on the final // packet avoids removing interior control-Z's that happen to // land at the end of a packet. -static bool held = false; static uint8_t held_packet[1024]; +static size_t held_packet_len; static void flush_packet(size_t packet_len, size_t& total_len) { - if (held) { + if (held_packet_len > 0) { // Remove trailing ctrl-z's on the final packet - held = false; size_t count; - for (count = packet_len; count > 0; --count) { + for (count = held_packet_len; count > 0; --count) { if (held_packet[count - 1] != CTRLZ) { break; } } file->write(held_packet, count); total_len += count; + held_packet_len = 0; } } static void write_packet(uint8_t* buf, size_t packet_len, size_t& total_len) { - if (held) { - held = false; - file->write(held_packet, packet_len); - total_len += packet_len; + if (held_packet_len > 0) { + file->write(held_packet, held_packet_len); + total_len += held_packet_len; + held_packet_len = 0; } memcpy(held_packet, buf, packet_len); - held = true; + held_packet_len = packet_len; } int xmodemReceive(Channel* serial, FileStream* out) { - serialPort = serial; - file = out; - - held = false; + serialPort = serial; + file = out; + held_packet_len = 0; uint8_t xbuff[1030]; /* 1024 for XModem 1k + 3 head chars + 2 crc + nul */ uint8_t* p; From e18f866d0d695d622f877128264c6541da597241 Mon Sep 17 00:00:00 2001 From: redbeard Date: Fri, 5 Aug 2022 16:54:20 -0700 Subject: [PATCH 3/8] bug: Logic error: state cannot be both Cycle & Hold (#559) The logical conjunction introduced in the `cycleOrHold` function will always evaluate to false. --- FluidNC/src/Settings.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FluidNC/src/Settings.cpp b/FluidNC/src/Settings.cpp index 3830018e7..3b0297b32 100644 --- a/FluidNC/src/Settings.cpp +++ b/FluidNC/src/Settings.cpp @@ -21,7 +21,7 @@ bool notIdleOrAlarm() { return sys.state != State::Idle && sys.state != State::Alarm && sys.state != State::ConfigAlarm; } bool cycleOrHold() { - return sys.state == State::Cycle && sys.state == State::Hold; + return sys.state == State::Cycle || sys.state == State::Hold; } Word::Word(type_t type, permissions_t permissions, const char* description, const char* grblName, const char* fullName) : From ee744836066067dd3353427505bbf472a7817d3d Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Mon, 5 Sep 2022 18:09:31 -1000 Subject: [PATCH 4/8] Queue switch events v2 (#562) * Queue switch events v2 * Homing and further elmination of rtXXX variables * Fixed some obvious bugs * Homing working with a simple test * Reenable ISRs when exiting from homing * Settle after approach, stop motor on failure * Eliminated kinematics_[post_]homing() which are now unnecessary * Fixed hard limit vs homing behavior for limit pins * Implemented PrePulloff moves * Fixed double ~ after safety door * Avoid sending pointless cycle start events * Clarified can_park() code * Fixed stuck limit problems * CoreXY homing * Fix corexy limits for replan * Factor common homing code * Better fix for motor_steps CoreXY issue * Home in incremental distances instead of pre-zeroing motor steps * Mpos * Cartesian botch, duh * Home via mc_move_motors * Include syntax botch * FreeRTOS should be spelled as freertos in include paths FreeRTOS works on case insensitive filesystems but not on Linux * Abstracted the notion of event pins and added control pins to it * Fixed feedhold --- FluidNC/src/Config.h | 2 +- FluidNC/src/Control.cpp | 23 +- FluidNC/src/Control.h | 1 + FluidNC/src/ControlPin.cpp | 45 +-- FluidNC/src/ControlPin.h | 37 +- FluidNC/src/Event.h | 37 ++ FluidNC/src/GCode.cpp | 2 +- FluidNC/src/Kinematics/Cartesian.cpp | 55 ++- FluidNC/src/Kinematics/Cartesian.h | 9 +- FluidNC/src/Kinematics/CoreXY.cpp | 239 ++----------- FluidNC/src/Kinematics/CoreXY.h | 10 +- FluidNC/src/Kinematics/Kinematics.cpp | 29 +- FluidNC/src/Kinematics/Kinematics.h | 16 +- FluidNC/src/Kinematics/WallPlotter.cpp | 29 +- FluidNC/src/Kinematics/WallPlotter.h | 3 +- FluidNC/src/Limits.cpp | 2 +- FluidNC/src/Machine/EventPin.cpp | 101 ++++++ FluidNC/src/Machine/EventPin.h | 53 +++ FluidNC/src/Machine/Homing.cpp | 464 +++++++++++++++---------- FluidNC/src/Machine/Homing.h | 73 ++-- FluidNC/src/Machine/LimitPin.cpp | 76 ++-- FluidNC/src/Machine/LimitPin.h | 27 +- FluidNC/src/Machine/Macros.cpp | 20 ++ FluidNC/src/Machine/Macros.h | 20 +- FluidNC/src/Machine/Motor.cpp | 2 +- FluidNC/src/Machine/Motor.h | 1 + FluidNC/src/Main.cpp | 2 + FluidNC/src/MotionControl.cpp | 35 +- FluidNC/src/MotionControl.h | 3 - FluidNC/src/Pin.h | 19 +- FluidNC/src/ProcessSettings.cpp | 35 +- FluidNC/src/Protocol.cpp | 415 ++++++++++++---------- FluidNC/src/Protocol.h | 84 +++-- FluidNC/src/Report.cpp | 3 + FluidNC/src/Report.h | 2 - FluidNC/src/Serial.cpp | 83 ++--- FluidNC/src/Stepper.cpp | 7 +- FluidNC/src/System.cpp | 9 + FluidNC/src/System.h | 1 + FluidNC/src/WebUI/WebSettings.cpp | 2 +- FluidNC/test/Pins/ErrorPinTest.cpp | 3 +- FluidNC/test/Pins/Undefined.cpp | 3 +- 42 files changed, 1122 insertions(+), 960 deletions(-) create mode 100644 FluidNC/src/Event.h create mode 100644 FluidNC/src/Machine/EventPin.cpp create mode 100644 FluidNC/src/Machine/EventPin.h create mode 100644 FluidNC/src/Machine/Macros.cpp diff --git a/FluidNC/src/Config.h b/FluidNC/src/Config.h index 978fe20ca..6c0151fbf 100644 --- a/FluidNC/src/Config.h +++ b/FluidNC/src/Config.h @@ -96,7 +96,7 @@ namespace SpindleSpeedOverride { const int Min = 10; // Percent of programmed spindle speed (1-100). Usually 10%. const int CoarseIncrement = 10; // (1-99). Usually 10%. const int FineIncrement = 1; // (1-99). Usually 1%. -} +}; // When a M2 or M30 program end command is executed, most GCode states are restored to their defaults. // This compile-time option includes the restoring of the feed, rapid, and spindle speed override values diff --git a/FluidNC/src/Control.cpp b/FluidNC/src/Control.cpp index 5aac66cc0..e451c6d4c 100644 --- a/FluidNC/src/Control.cpp +++ b/FluidNC/src/Control.cpp @@ -3,18 +3,19 @@ #include "Control.h" -#include "Protocol.h" // rtSafetyDoor, etc +#include "Protocol.h" // *Event +#include "Machine/Macros.h" // macro0Event Control::Control() { // The SafetyDoor pin must be defined first because it is checked explicity in safety_door_ajar() - _pins.push_back(new ControlPin(rtSafetyDoor, "safety_door_pin", 'D')); - _pins.push_back(new ControlPin(rtReset, "reset_pin", 'R')); - _pins.push_back(new ControlPin(rtFeedHold, "feed_hold_pin", 'H')); - _pins.push_back(new ControlPin(rtCycleStart, "cycle_start_pin", 'S')); - _pins.push_back(new ControlPin(rtButtonMacro0, "macro0_pin", '0')); - _pins.push_back(new ControlPin(rtButtonMacro1, "macro1_pin", '1')); - _pins.push_back(new ControlPin(rtButtonMacro2, "macro2_pin", '2')); - _pins.push_back(new ControlPin(rtButtonMacro3, "macro3_pin", '3')); + _pins.push_back(new ControlPin(&safetyDoorEvent, "safety_door_pin", 'D')); + _pins.push_back(new ControlPin(&resetEvent, "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')); + _pins.push_back(new ControlPin(¯o1Event, "macro1_pin", '1')); + _pins.push_back(new ControlPin(¯o2Event, "macro2_pin", '2')); + _pins.push_back(new ControlPin(¯o3Event, "macro3_pin", '3')); } void Control::init() { @@ -25,7 +26,7 @@ void Control::init() { void Control::group(Configuration::HandlerBase& handler) { for (auto pin : _pins) { - handler.item(pin->_legend, pin->_pin); + handler.item(pin->_legend.c_str(), pin->_pin); } } @@ -33,7 +34,7 @@ String Control::report_status() { String ret = ""; for (auto pin : _pins) { if (pin->get()) { - ret += pin->_letter; + ret += pin->letter(); } } return ret; diff --git a/FluidNC/src/Control.h b/FluidNC/src/Control.h index af780bf5a..196603d72 100644 --- a/FluidNC/src/Control.h +++ b/FluidNC/src/Control.h @@ -7,6 +7,7 @@ #include "ControlPin.h" #include +using namespace Machine; class Control : public Configuration::Configurable { public: Control(); diff --git a/FluidNC/src/ControlPin.cpp b/FluidNC/src/ControlPin.cpp index bdb47127b..2d011ee14 100644 --- a/FluidNC/src/ControlPin.cpp +++ b/FluidNC/src/ControlPin.cpp @@ -1,41 +1,14 @@ #include "ControlPin.h" -#include "Report.h" // addPinReport -#include "Protocol.h" // ExecAlarm -#include // IRAM_ATTR -#include // CHANGE - -void IRAM_ATTR ControlPin::handleISR() { - bool pinState = _pin.read(); - _value = pinState; - - // Rate limit control pin events so switch bounce does not cause multiple events - if (pinState && (_debounceEnd == 0 || ((getCpuTicks() - _debounceEnd) >= 0))) { - _debounceEnd = usToEndTicks(debounceUs); - // We use 0 to mean that the debounce lockout is inactive, - // so if the end time happens to be 0, bump it up by one tick. - if (_debounceEnd == 0) { - _debounceEnd = 1; +namespace Machine { + void ControlPin::run(void* arg) { + if (get()) { + // log_debug(_legend); + block(); + startTimer(); + _event->run(arg); + } else { + reArm(); } - _rtVariable = true; - } -} - -void ControlPin::init() { - if (_pin.undefined()) { - return; - } - _pin.report(_legend); - auto attr = Pin::Attr::Input | Pin::Attr::ISR; - if (_pin.capabilities().has(Pins::PinCapabilities::PullUp)) { - attr = attr | Pin::Attr::PullUp; } - _pin.setAttr(attr); - _pin.attachInterrupt(ISRHandler, CHANGE, this); - _rtVariable = false; - _value = _pin.read(); -} - -ControlPin::~ControlPin() { - _pin.detachInterrupt(); } diff --git a/FluidNC/src/ControlPin.h b/FluidNC/src/ControlPin.h index e5693d651..032b182fb 100644 --- a/FluidNC/src/ControlPin.h +++ b/FluidNC/src/ControlPin.h @@ -2,32 +2,19 @@ #include "Pin.h" #include // IRAM_ATTR +#include "Machine/EventPin.h" +namespace Machine { + class ControlPin : public Machine::EventPin { + private: + const char _letter; // The name that appears in init() messages and the name of the configuration item -class ControlPin { -private: - bool _value; - volatile bool& _rtVariable; // The variable that is set when the pin is asserted - int32_t _debounceEnd = 0; + public: + ControlPin(Event* event, const char* legend, char letter) : EventPin(event, legend), _letter(letter) {} - void IRAM_ATTR handleISR(); - CreateISRHandlerFor(ControlPin, handleISR); + char letter() { return _letter; }; - // Interval during which we ignore repeated control pin activations - const int debounceUs = 10000; // 10000 us = 10 ms + void run(void* arg) override; -public: - const char* _legend; // The name that appears in init() messages and the name of the configuration item - const char _letter; // The letter that appears in status reports when the pin is active - - ControlPin(volatile bool& rtVariable, const char* legend, char letter) : - _value(false), _letter(letter), _rtVariable(rtVariable), _legend(legend) { - _rtVariable = _value; - } - - Pin _pin; - - void init(); - bool get() { return _value; } - - ~ControlPin(); -}; + ~ControlPin(); + }; +} diff --git a/FluidNC/src/Event.h b/FluidNC/src/Event.h new file mode 100644 index 000000000..daeceafa7 --- /dev/null +++ b/FluidNC/src/Event.h @@ -0,0 +1,37 @@ +// Copyright (c) 2022 - Mitch Bradley +// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file. + +#pragma once +#include // String + +// Objects derived from the Event base class are placed in the event queue. +// Protocol dequeues them and calls their run methods. +class Event { +public: + Event() {} + virtual void run(void* arg) = 0; +}; + +class NoArgEvent : public Event { + void (*_function)() = nullptr; + +public: + NoArgEvent(void (*function)()) : _function(function) {} + void run(void* arg) override { + if (_function) { + _function(); + } + } +}; + +class ArgEvent : public Event { + void (*_function)(void*) = nullptr; + +public: + ArgEvent(void (*function)(void*)) : _function(function) {} + void run(void* arg) override { + if (_function) { + _function(arg); + } + } +}; diff --git a/FluidNC/src/GCode.cpp b/FluidNC/src/GCode.cpp index f2b27dbec..575bb1a22 100644 --- a/FluidNC/src/GCode.cpp +++ b/FluidNC/src/GCode.cpp @@ -1628,7 +1628,7 @@ Error gc_execute_line(char* line, Channel& channel) { case ProgramFlow::Paused: protocol_buffer_synchronize(); // Sync and finish all remaining buffered motions before moving on. if (sys.state != State::CheckMode) { - rtFeedHold = true; // Use feed hold for program pause. + protocol_send_event(&feedHoldEvent); protocol_execute_realtime(); // Execute suspend. } break; diff --git a/FluidNC/src/Kinematics/Cartesian.cpp b/FluidNC/src/Kinematics/Cartesian.cpp index 1a9fbc0e6..bf8998619 100644 --- a/FluidNC/src/Kinematics/Cartesian.cpp +++ b/FluidNC/src/Kinematics/Cartesian.cpp @@ -1,19 +1,12 @@ #include "Cartesian.h" -#include "../Machine/MachineConfig.h" +#include "src/Machine/MachineConfig.h" +#include "src/Machine/Axes.h" // ambiguousLimit() +#include "src/Limits.h" namespace Kinematics { void Cartesian::init() { log_info("Kinematic system: " << name()); } - bool Cartesian::kinematics_homing(AxisMask cycle_mask) { - // Do nothing. - return false; - } - - void Cartesian::kinematics_post_homing() { - // Do nothing. - } - 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); @@ -24,6 +17,48 @@ namespace Kinematics { copyAxes(cartesian, motors); } + void Cartesian::transform_cartesian_to_motors(float* motors, float* cartesian) { + // Motor space is cartesian space, so we do no transform. + copyAxes(motors, cartesian); + } + + bool Cartesian::canHome(AxisMask axisMask) { + if (ambiguousLimit()) { + log_error("Ambiguous limit switch touching. Manually clear all switches"); + return false; + } + return true; + } + + bool Cartesian::limitReached(AxisMask& axisMask, MotorMask& motorMask, MotorMask limited) { + // For Cartesian, the limit switches are associated with individual motors, since + // an axis can have dual motors each with its own limit switch. We clear the motors in + // the mask whose limits have been reached. + clear_bits(motorMask, limited); + + // Set axisMask according to the motors that are still running. + axisMask = Machine::Axes::motors_to_axes(motorMask); + + // We do not have to stop until all motors have reached their limits + return !axisMask; + } + + void Cartesian::releaseMotors(AxisMask axisMask, MotorMask motors, Machine::Homing::Phase phase) { + auto axes = config->_axes; + auto n_axis = axes->_numberAxis; + for (int axis = 0; axis < n_axis; axis++) { + if (bitnum_is_true(axisMask, axis)) { + auto paxis = axes->_axis[axis]; + if (bitnum_is_true(motors, Machine::Axes::motor_bit(axis, 0))) { + paxis->_motors[0]->unlimit(); + } + if (bitnum_is_true(motors, Machine::Axes::motor_bit(axis, 1))) { + paxis->_motors[1]->unlimit(); + } + } + } + } + // Configuration registration namespace { KinematicsFactory::InstanceBuilder registration("Cartesian"); diff --git a/FluidNC/src/Kinematics/Cartesian.h b/FluidNC/src/Kinematics/Cartesian.h index 1de8c0d58..268a94dec 100644 --- a/FluidNC/src/Kinematics/Cartesian.h +++ b/FluidNC/src/Kinematics/Cartesian.h @@ -25,9 +25,12 @@ namespace Kinematics { virtual bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) override; virtual void init() override; - virtual bool kinematics_homing(AxisMask cycle_mask) override; - virtual void kinematics_post_homing() override; - virtual void motors_to_cartesian(float* cartesian, float* motors, int n_axis) override; + void motors_to_cartesian(float* cartesian, float* motors, int n_axis) override; + void transform_cartesian_to_motors(float* cartesian, float* motors) override; + + bool canHome(AxisMask axisMask) override; + void releaseMotors(AxisMask axisMask, MotorMask motors, Machine::Homing::Phase phase) override; + bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited) override; // Configuration handlers: void afterParse() override {} diff --git a/FluidNC/src/Kinematics/CoreXY.cpp b/FluidNC/src/Kinematics/CoreXY.cpp index 8f2af0104..44340cec4 100644 --- a/FluidNC/src/Kinematics/CoreXY.cpp +++ b/FluidNC/src/Kinematics/CoreXY.cpp @@ -37,230 +37,41 @@ namespace Kinematics { config->_axes->_axis[Y_AXIS]->_motors[0]->limitOtherAxis(X_AXIS); } - // plan a homing mve in motor space for the homing sequence - void CoreXY::plan_homing_move(AxisMask axisMask, bool approach, bool seek) { - float rate = 0; - float* target = get_mpos(); - float dist = 0; - uint8_t axis = X_AXIS; - - if (bitnum_is_true(axisMask, Y_AXIS)) { - axis = Y_AXIS; - } - - auto axisConf = config->_axes->_axis[axis]; - - if (seek) { - dist = axisConf->_maxTravel * axisConf->_homing->_seek_scaler; - rate = axisConf->_homing->_seekRate; - } else { - dist = axisConf->_motors[0]->_pulloff; - rate = axisConf->_homing->_feedRate; - if (!approach) { - dist *= -1.000; // backoff - } else { // approach - dist *= axisConf->_homing->_feed_scaler; // times scaler to make sure we hit - } - } - - if (!axisConf->_homing->_positiveDirection) { - dist *= -1.000; - } - - auto n_axis = config->_axes->_numberAxis; - - float move_to[n_axis] = { 0 }; - // zero all X&Y posiitons before each cycle - // leave other axes unchanged - for (int axis = X_AXIS; axis <= config->_axes->_numberAxis; axis++) { - if (axis < Z_AXIS) { - set_motor_steps(axis, 0); - target[axis] = 0.0; - } else { - move_to[axis] = target[axis]; - } - } - - //TODO Need to adjust the rate for CoreXY 1.414 - - (axis == X_AXIS) ? move_to[X_AXIS] = dist : move_to[Y_AXIS] = dist; - - transform_cartesian_to_motors(target, move_to); - - plan_line_data_t plan_data; - plan_data.spindle_speed = 0; - plan_data.motion = {}; - plan_data.motion.systemMotion = 1; - plan_data.motion.noFeedOverride = 1; - plan_data.spindle = SpindleState::Disable; - plan_data.coolant.Mist = 0; - plan_data.coolant.Flood = 0; - plan_data.line_number = 0; - plan_data.is_jog = false; - plan_data.feed_rate = rate; // Magnitude of homing rate vector - - plan_buffer_line(target, &plan_data); // Bypass mc_move_motors(). Directly plan homing motion. - - sys.step_control = {}; - sys.step_control.executeSysMotion = true; // Set to execute homing motion and clear existing flags. - Stepper::prep_buffer(); // Prep and fill segment buffer from newly planned block. - Stepper::wake_up(); // Initiate motion - - // The move has started. There are 2 good outcomes we are looking for - // On approach we are looking for a switch touch - // On backoff we are waiting for the motion to stop. - // All other outcomes are problems - - bool switch_touch = false; - do { - if (approach) { - switch_touch = bitnum_is_true((Machine::Axes::posLimitMask | Machine::Axes::negLimitMask), axis); - } - - Stepper::prep_buffer(); // Check and prep segment buffer. - - // This checks some of the events that would normally be handled - // by protocol_execute_realtime(). The homing loop is time-critical - // so we handle those events directly here, calling protocol_execute_realtime() - // only if one of those events is active. - if (rtReset) { - throw ExecAlarm::HomingFailReset; - } - if (rtSafetyDoor) { - throw ExecAlarm::HomingFailDoor; - } - if (rtCycleStop) { - //log_info("CoreXY Cyclestop"); - rtCycleStop = false; - if (approach) { - throw ExecAlarm::HomingFailApproach; - } - if (bitnum_is_true((Machine::Axes::posLimitMask | Machine::Axes::negLimitMask), axis)) { - // Homing failure: Limit switch still engaged after pull-off motion - throw ExecAlarm::HomingFailPulloff; - } - - switch_touch = true; // used to break out of the do loop - } - pollChannels(); - } while (!switch_touch); - - Stepper::reset(); // Immediately force kill steppers and reset step segment buffer. - delay_ms(axisConf->_homing->_settle_ms); - } - - bool CoreXY::kinematics_homing(AxisMask cycle_mask) { // TODO cycle_mask s/b axisMask...this is not about cycles + bool CoreXY::canHome(AxisMask axisMask) { // make sure there are no axes that are not in homingMask - if (cycle_mask && !(cycle_mask & Machine::Axes::homingMask)) { + if (axisMask && !(axisMask & Machine::Axes::homingMask)) { log_error("Not a homed axis:"); - return true; + return false; } if (ambiguousLimit()) { log_error("Ambiguous limit switch touching. Manually clear all switches"); - return true; - } - - if (cycle_mask != 0) { - if (bitnum_is_true(cycle_mask, X_AXIS) || bitnum_is_true(cycle_mask, Y_AXIS)) { - log_error("CoreXY cannot single axis home X or Y axes"); - // TODO: Set some Kinematics error or alarm - return true; - } - Machine::Homing::run_one_cycle(cycle_mask); - return true; - } - - // Multi-axis cycles not allowed with CoreXY because 2 motors are used for linear XY moves - // Check each cycle for multiple axes - for (int cycle = 1; cycle <= MAX_N_AXIS; cycle++) { - AxisMask axisMask = Machine::Homing::axis_mask_from_cycle(cycle); - uint8_t count = 0; - - for (int axis = 0; axis < MAX_N_AXIS; axis++) { - if (bitnum_is_true(axisMask, axis)) { - if (++count > 1) { // Error with any axis with more than one axis per cycle - log_error("CoreXY cannot multi-axis home. Check homing cycle:" << cycle); - // TODO: Set some Kinematics error or alarm - return true; - } - } - } + return false; } - - // run cycles - for (int cycle = 1; cycle <= MAX_N_AXIS; cycle++) { - AxisMask axisMask = Machine::Homing::axis_mask_from_cycle(cycle); - - if (!axisMask) - continue; - - // Only X and Y need a special homing sequence - if (!bitnum_is_true(axisMask, X_AXIS) && !bitnum_is_true(axisMask, Y_AXIS)) { - Machine::Homing::run_one_cycle(axisMask); - continue; - } else { - //log_info("CoreXY homing cycle:" << cycle << " axis:" << axisMask); - if (bitnum_is_true(axisMask, X_AXIS) || bitnum_is_true(axisMask, Y_AXIS)) { - try { - plan_homing_move(axisMask, true, true); // seek aproach - plan_homing_move(axisMask, false, false); // pulloff - plan_homing_move(axisMask, true, false); // feed aproach - plan_homing_move(axisMask, false, false); // pulloff - } catch (ExecAlarm alarm) { - rtAlarm = alarm; - config->_axes->set_homing_mode(axisMask, false); // tell motors homing is done...failed - log_error("Homing fail"); - mc_reset(); // Stop motors, if they are running. - protocol_execute_realtime(); // handle any pending rtXXX conditions - return true; - } - } - } - } - - auto n_axis = config->_axes->_numberAxis; - - float mpos[n_axis] = { 0 }; - - // Set machine positions for homed limit switches. Don't update non-homed axes. - for (int axis = 0; axis < n_axis; axis++) { - Machine::Axis* axisConf = config->_axes->_axis[axis]; - - if (axisConf->_homing) { - auto mpos_mm = axisConf->_homing->_mpos; - mpos[axis] = mpos_mm; - } - } - - float motors_mm[n_axis]; - transform_cartesian_to_motors(motors_mm, mpos); - - // the only single axis homing allowed is Z and above - if (cycle_mask >= 1 << Z_AXIS) { - for (int axis = Z_AXIS; axis < n_axis; axis++) { - if (bitnum_is_true(cycle_mask, axis)) { - // set the Z motor position - set_motor_steps(axis, mpos_to_steps(motors_mm[axis], axis)); - } - } - } else { - // set all of them - for (int axis = X_AXIS; axis < n_axis; axis++) { - set_motor_steps(axis, mpos_to_steps(motors_mm[axis], axis)); - } - } - - sys.step_control = {}; // Return step control to normal operation. - - gc_sync_position(); - plan_sync_position(); - return true; } - void CoreXY::kinematics_post_homing() { - // Do nothing. + bool CoreXY::limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited) { + // For CoreXY, the limit switches are associated with axes, not motors (multiple motors + // must run to perform a move parallel to an axis). When a limit switch is hit, we + // clear the associated axis bit and stop motion. The homing code will then replan + // a new move along the remaining axes. + MotorMask toClear = axisMask & limited; + auto axes = config->_axes; + + clear_bits(axisMask, limited); + clear_bits(motors, limited); + // clear_bits(Machine::Axes::posLimitMask, toClear); + // clear_bits(Machine::Axes::negLimitMask, toClear); + return bool(toClear); + } + + void CoreXY::releaseMotors(AxisMask axisMask, MotorMask motors, Machine::Homing::Phase phase) { + auto axes = config->_axes; + auto n_axis = axes->_numberAxis; + for (size_t axis = X_AXIS; axis < n_axis; axis++) { + axes->_axis[axis]->_motors[0]->unlimit(); + } } /* diff --git a/FluidNC/src/Kinematics/CoreXY.h b/FluidNC/src/Kinematics/CoreXY.h index 6d4a9dfda..7157aed3a 100644 --- a/FluidNC/src/Kinematics/CoreXY.h +++ b/FluidNC/src/Kinematics/CoreXY.h @@ -28,16 +28,20 @@ namespace Kinematics { // Kinematic Interface virtual void init() override; - bool kinematics_homing(AxisMask cycle_mask) override; - void kinematics_post_homing() override; bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) override; void motors_to_cartesian(float* cartesian, float* motors, int n_axis) override; + bool canHome(AxisMask axisMask) override; + void releaseMotors(AxisMask axisMask, MotorMask motors, Machine::Homing::Phase phase) override; + bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited); + // Configuration handlers: void validate() const override {} virtual void group(Configuration::HandlerBase& handler) override; void afterParse() override {} + void transform_cartesian_to_motors(float* motors, float* cartesian) override; + // Name of the configurable. Must match the name registered in the cpp file. virtual const char* name() const override { return "CoreXY"; } @@ -49,8 +53,6 @@ namespace Kinematics { void plan_homing_move(AxisMask axisMask, bool approach, bool seek); - void transform_cartesian_to_motors(float* motors, float* cartesian); - protected: float _x_scaler = 1.0; }; diff --git a/FluidNC/src/Kinematics/Kinematics.cpp b/FluidNC/src/Kinematics/Kinematics.cpp index ad1c0ed26..ff941b365 100644 --- a/FluidNC/src/Kinematics/Kinematics.cpp +++ b/FluidNC/src/Kinematics/Kinematics.cpp @@ -4,35 +4,42 @@ #include "Kinematics.h" -#include "../Config.h" +#include "src/Config.h" #include "Cartesian.h" namespace Kinematics { - bool Kinematics::kinematics_homing(AxisMask cycle_mask) { + bool Kinematics::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { Assert(_system != nullptr, "No kinematic system"); - return _system->kinematics_homing(cycle_mask); + return _system->cartesian_to_motors(target, pl_data, position); } - void Kinematics::kinematics_post_homing() { + void Kinematics::motors_to_cartesian(float* cartesian, float* motors, int n_axis) { Assert(_system != nullptr, "No kinematic system"); - return _system->kinematics_post_homing(); + return _system->motors_to_cartesian(cartesian, motors, n_axis); } - bool Kinematics::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { + bool Kinematics::canHome(AxisMask axisMask) { Assert(_system != nullptr, "No kinematic system"); - return _system->cartesian_to_motors(target, pl_data, position); + return _system->canHome(axisMask); } - void Kinematics::motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + void Kinematics::releaseMotors(AxisMask axisMask, MotorMask motors, Machine::Homing::Phase phase) { Assert(_system != nullptr, "No kinematic system"); - return _system->motors_to_cartesian(cartesian, motors, n_axis); + _system->releaseMotors(axisMask, motors, phase); + } + + bool Kinematics::limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited) { + Assert(_system != nullptr, "No kinematics system."); + return _system->limitReached(axisMask, motors, limited); } - void Kinematics::group(Configuration::HandlerBase& handler) { - ::Kinematics::KinematicsFactory::factory(handler, _system); + void Kinematics::transform_cartesian_to_motors(float* motors, float* cartesian) { Assert(_system != nullptr, "No kinematics system."); + return _system->transform_cartesian_to_motors(motors, cartesian); } + void Kinematics::group(Configuration::HandlerBase& handler) { ::Kinematics::KinematicsFactory::factory(handler, _system); } + void Kinematics::afterParse() { if (_system == nullptr) { _system = new ::Kinematics::Cartesian(); diff --git a/FluidNC/src/Kinematics/Kinematics.h b/FluidNC/src/Kinematics/Kinematics.h index baf09cf1f..6ce56a58e 100644 --- a/FluidNC/src/Kinematics/Kinematics.h +++ b/FluidNC/src/Kinematics/Kinematics.h @@ -6,6 +6,7 @@ #include "../MotionControl.h" #include "../Planner.h" #include "../Types.h" +#include "src/Machine/Homing.h" /* Special types @@ -42,10 +43,13 @@ namespace Kinematics { void init(); void config_kinematics(); - bool kinematics_homing(AxisMask cycle_mask); - void kinematics_post_homing(); bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position); void motors_to_cartesian(float* cartesian, float* motors, int n_axis); + void transform_cartesian_to_motors(float* motors, float* cartesian); + + bool canHome(AxisMask axisMask); + void releaseMotors(AxisMask axisMask, MotorMask motors, Machine::Homing::Phase phase); + bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited); private: ::Kinematics::KinematicSystem* _system = nullptr; @@ -63,10 +67,14 @@ namespace Kinematics { // Kinematic system interface. virtual bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) = 0; virtual void init() = 0; - virtual bool kinematics_homing(AxisMask cycle_mask) = 0; - virtual void kinematics_post_homing() = 0; virtual void motors_to_cartesian(float* cartesian, float* motors, int n_axis) = 0; + virtual void transform_cartesian_to_motors(float* motors, float* cartesian) = 0; + + virtual bool canHome(AxisMask axisMask) { return false; } + virtual void releaseMotors(AxisMask axisMask, MotorMask motors, Machine::Homing::Phase phase) {} + virtual bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited) { return false; } + // Configuration interface. void afterParse() override {} void group(Configuration::HandlerBase& handler) override {} diff --git a/FluidNC/src/Kinematics/WallPlotter.cpp b/FluidNC/src/Kinematics/WallPlotter.cpp index 5e06249b5..38440ed15 100644 --- a/FluidNC/src/Kinematics/WallPlotter.cpp +++ b/FluidNC/src/Kinematics/WallPlotter.cpp @@ -29,13 +29,8 @@ namespace Kinematics { last_z = 0; } - bool WallPlotter::kinematics_homing(AxisMask cycle_mask) { - // Do nothing. - return false; - } - - void WallPlotter::kinematics_post_homing() { - // Do nothing. + void WallPlotter::transform_cartesian_to_motors(float* cartesian, float* motors) { + log_error("WallPlotter::transform_cartesian_to_motors is broken"); } /* @@ -49,25 +44,25 @@ namespace Kinematics { position = an MAX_N_AXIS array of where the machine is starting from for this move */ bool WallPlotter::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { - float dx, dy, dz; // segment distances in each cartesian axis - uint32_t segment_count; // number of segments the move will be broken in to. + float dx, dy, dz; // segment distances in each cartesian axis + uint32_t segment_count; // number of segments the move will be broken in to. // calculate the total X,Y axis move distance // Z axis is the same in both coord systems, so it does not undergo conversion float dist = vector_distance(target, position, Y_AXIS); // Segment our G1 and G0 moves based on yaml file. If we choose a small enough _segment_length we can hide the nonlinearity segment_count = dist / _segment_length; - if (segment_count < 1) { // Make sure there is at least one segment, even if there is no movement + if (segment_count < 1) { // Make sure there is at least one segment, even if there is no movement // We need to do this to make sure other things like S and M codes get updated properly by // the planner even if there is no movement?? - segment_count = 1; + segment_count = 1; } // Calc distance of individual segments - dx = (target[X_AXIS] - position[X_AXIS])/segment_count; - dy = (target[Y_AXIS] - position[Y_AXIS])/segment_count; - dz = (target[Z_AXIS] - position[Z_AXIS])/segment_count; - // Current cartesian end point of the segment - float seg_x = position[X_AXIS]; + dx = (target[X_AXIS] - position[X_AXIS]) / segment_count; + dy = (target[Y_AXIS] - position[Y_AXIS]) / segment_count; + dz = (target[Z_AXIS] - position[Z_AXIS]) / segment_count; + // Current cartesian end point of the segment + float seg_x = position[X_AXIS]; float seg_y = position[Y_AXIS]; float seg_z = position[Z_AXIS]; for (uint32_t segment = 1; segment <= segment_count; segment++) { @@ -75,7 +70,7 @@ namespace Kinematics { seg_x += dx; seg_y += dy; seg_z += dz; - float seg_left, seg_right; + float seg_left, seg_right; xy_to_lengths(seg_x, seg_y, seg_left, seg_right); // TODO: Need to adjust cartesian feedrate to motor/plotter space, just leave them alone for now diff --git a/FluidNC/src/Kinematics/WallPlotter.h b/FluidNC/src/Kinematics/WallPlotter.h index c984f9211..d234661a7 100644 --- a/FluidNC/src/Kinematics/WallPlotter.h +++ b/FluidNC/src/Kinematics/WallPlotter.h @@ -24,10 +24,9 @@ namespace Kinematics { // Kinematic Interface void init() override; - bool kinematics_homing(AxisMask cycle_mask) override; - void kinematics_post_homing() override; bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) override; void motors_to_cartesian(float* cartesian, float* motors, int n_axis) override; + void transform_cartesian_to_motors(float* cartesian, float* motors) override; // Configuration handlers: void validate() const override {} diff --git a/FluidNC/src/Limits.cpp b/FluidNC/src/Limits.cpp index e86c767d8..ef3bdf4c1 100644 --- a/FluidNC/src/Limits.cpp +++ b/FluidNC/src/Limits.cpp @@ -96,7 +96,7 @@ void limits_soft_check(float* cartesian) { // workspace volume so just come to a controlled stop so position is not lost. When complete // enter alarm mode. if (sys.state == State::Cycle) { - rtFeedHold = true; + protocol_send_event(&feedHoldEvent); do { pollChannels(); protocol_execute_realtime(); diff --git a/FluidNC/src/Machine/EventPin.cpp b/FluidNC/src/Machine/EventPin.cpp new file mode 100644 index 000000000..4a6bb45c8 --- /dev/null +++ b/FluidNC/src/Machine/EventPin.cpp @@ -0,0 +1,101 @@ +#include "EventPin.h" + +#include "src/Report.h" // addPinReport +#include "src/Protocol.h" // event_queue +#include "src/System.h" // sys +#include // IRAM_ATTR + +#include "soc/soc.h" +#include "soc/gpio_periph.h" +#include "hal/gpio_hal.h" + +namespace Machine { + std::list EventPin::_blockedPins; + + TimerHandle_t EventPin::_eventPinTimer = 0; + + EventPin::EventPin(Event* event, const char* legend) : _event(event), _legend(legend) { + if (_eventPinTimer == 0) { + _eventPinTimer = xTimerCreate("eventPinTimer", pdMS_TO_TICKS(200), false, NULL, eventPinTimerCallback); + } + } + EventPin::EventPin(Event* event, const char* legend, Pin& pin) : EventPin(event, legend) { + _pin.swap(pin); + } + bool EventPin::get() { + return _pin.read(); + } + void EventPin::eventPinTimerCallback(void*) { + check(); + } + void EventPin::run(void* arg) { + log_debug("EP run"); + // Since we do not trust the ISR to always trigger precisely, + // we check the pin state before calling the event handler + if (get()) { + block(); + _event->run(arg); + } else { + reArm(); + } + } + void EventPin::check() { + _blockedPins.remove_if(inactive); + } + + void EventPin::block() { + _blockedPins.emplace_back(this); + } + + bool EventPin::inactive(EventPin* pin) { + bool value = pin->get(); + + pin->update(value); + if (value) { + if (sys.state != State::Homing) { + xTimerStart(_eventPinTimer, 0); + } + } else { + pin->reArm(); + } + return !value; + } + + void IRAM_ATTR EventPin::handleISR() { + // This is the body of gpio_hal_intr_disable() which is not IRAM_ATTR + gpio_num_t gpio_num = gpio_num_t(_gpio); + gpio_dev_t* dev = GPIO_LL_GET_HW(GPIO_PORT_0); + gpio_ll_intr_disable(dev, gpio_num); + if (gpio_num < 32) { + gpio_ll_clear_intr_status(dev, BIT(gpio_num)); + } else { + gpio_ll_clear_intr_status_high(dev, BIT(gpio_num - 32)); + } + protocol_send_event_from_ISR(this, this); + } + void EventPin::startTimer() { + xTimerStart(_eventPinTimer, 0); + } + + void EventPin::reArm() { + gpio_intr_enable(gpio_num_t(_gpio)); + } + void EventPin::init() { + if (_pin.undefined()) { + return; + } + + _pin.report(_legend); + auto attr = Pin::Attr::Input | Pin::Attr::ISR; + if (_pin.capabilities().has(Pins::PinCapabilities::PullUp)) { + attr = attr | Pin::Attr::PullUp; + } + _pin.setAttr(attr); + _pin.attachInterrupt(ISRHandler, Pin::EITHER_EDGE, this); + _gpio = _pin.getNative(Pin::Capabilities::Input | Pin::Capabilities::ISR); + } + + EventPin::~EventPin() { + _pin.detachInterrupt(); + } +}; diff --git a/FluidNC/src/Machine/EventPin.h b/FluidNC/src/Machine/EventPin.h new file mode 100644 index 000000000..205345f3f --- /dev/null +++ b/FluidNC/src/Machine/EventPin.h @@ -0,0 +1,53 @@ +#pragma once + +#include "src/Pin.h" +#include "src/Event.h" +#include "src/Logging.h" + +#include // TimerHandle_t +#include + +namespace Machine { + class EventPin : public Event { + private: + static std::list _blockedPins; + + protected: + void handleISR(); + CreateISRHandlerFor(EventPin, handleISR); + + Event* _event = nullptr; // Subordinate event that is called conditionally + + pinnum_t _gpio; + + static bool inactive(EventPin* pin); + + static void eventPinTimerCallback(void*); + + public: + static TimerHandle_t _eventPinTimer; + + String _legend; // The name that appears in init() messages and the name of the configuration item + + EventPin(Event* event, const char* legend); + EventPin(Event* event, const char* legend, Pin& pin); + + Pin _pin; + + void init(); + bool get(); + void run(void* arg) override; + void block(); + + static void startTimer(); + static void check(); + + virtual void update(bool state) {}; + + // After the ISR triggers it is turned off until reArm() is called, + // thus preventing unwanted retriggering due to switch noise or held buttons + virtual void reArm(); + + ~EventPin(); + }; +}; diff --git a/FluidNC/src/Machine/Homing.cpp b/FluidNC/src/Machine/Homing.cpp index 2f4a5815a..cb68e505c 100644 --- a/FluidNC/src/Machine/Homing.cpp +++ b/FluidNC/src/Machine/Homing.cpp @@ -4,7 +4,8 @@ #include "../NutsBolts.h" // set_bitnum, etc #include "../System.h" // sys.* #include "../Stepper.h" // st_wake -#include "../Protocol.h" // protocol_execute_realtime +#include "../Protocol.h" // protocol_handle_events +#include "../Limits.h" // ambiguousLimit #include "../Machine/Axes.h" #include "../Machine/MachineConfig.h" // config @@ -30,24 +31,97 @@ namespace Machine { // Then we scale the travel distances for the other axes so they would complete // at the same time. - volatile bool Homing::_approach = false; - const uint32_t MOTOR0 = 0xffff; const uint32_t MOTOR1 = 0xffff0000; - // The return value is the setting time - uint32_t Homing::plan_move(MotorMask motors, HomingPhase phase) { - float maxSeekTime = 0.0; - float limitingRate = 0.0; - uint32_t settle = 0; - float rate = 0.0; + Homing::Phase Homing::_phase = Phase::None; + AxisMask Homing::_cycleAxes = 0; + AxisMask Homing::_phaseAxes = 0; + MotorMask Homing::_cycleMotors = 0; + MotorMask Homing::_phaseMotors; + std::queue Homing::_remainingCycles; + uint32_t Homing::_settling_ms; - auto axes = config->_axes; - auto n_axis = axes->_numberAxis; - float* target = get_mpos(); - float rates[n_axis]; + const char* Homing::_phaseNames[] = { + "None", "PrePulloff", "FastApproach", "Pulloff0", "SlowApproach", "Pulloff1", "Pulloff2", "CycleDone", + }; + + void Homing::startMove(float* target, float rate) { + plan_line_data_t plan_data; + plan_data.spindle_speed = 0; + plan_data.motion = {}; + plan_data.motion.systemMotion = 1; + plan_data.motion.noFeedOverride = 1; + plan_data.spindle = SpindleState::Disable; + plan_data.coolant.Mist = 0; + plan_data.coolant.Flood = 0; + plan_data.line_number = REPORT_LINE_NUMBER; + plan_data.is_jog = false; + plan_data.feed_rate = rate; // Magnitude of homing rate vector + + mc_move_motors(target, &plan_data); + + protocol_send_event(&cycleStartEvent); + } + + static MotorMask limited() { return Machine::Axes::posLimitMask | Machine::Axes::negLimitMask; } + + void Homing::cycleStop() { + if (approach()) { + // Cycle stop while approaching means that we did not hit + // a limit switch in the programmed distance + fail(ExecAlarm::HomingFailApproach); + return; + } + Machine::EventPin::check(); + + // Cycle stop in pulloff is success unless + // the limit switches are still active. + if (limited() & _phaseMotors) { + // Homing failure: Limit switch still engaged after pull-off motion + fail(ExecAlarm::HomingFailPulloff); + return; + } + // Normal termination for pulloff cycle + _phaseMotors = 0; + + // Advance to next cycle + Stepper::reset(); // Stop steppers and reset step segment buffer + delay_ms(_settling_ms); // Delay to allow transient dynamics to dissipate. + + nextPhase(); + } + + void Homing::nextPhase() { + _phase = static_cast(static_cast(_phase) + 1); + log_debug("Homing nextPhase " << phaseName(_phase)); + if (_phase == CycleDone || (_phase == Phase::Pulloff2 && !needsPulloff2(_cycleMotors))) { + set_mpos(); + nextCycle(); + } else { + runPhase(); + } + } - bool seeking = phase == HomingPhase::FastApproach; + void Homing::axisVector(AxisMask axisMask, MotorMask motors, Machine::Homing::Phase phase, float* target, float& rate, uint32_t& settle_ms) { + log_debug("Starting from " << target[0] << "," << target[1] << "," << target[2]); + + float maxSeekTime = 0.0; + float limitingRate = 0.0; + float ratesq = 0.0; + + settle_ms = 0; + + // log_debug("Cartesian homing " << int(axisMask) << " motors " << int(motors)); + + auto axes = config->_axes; + auto n_axis = axes->_numberAxis; + + float rates[n_axis] = { 0 }; + float distance[n_axis] = { 0 }; + + bool seeking = phase == Machine::Homing::Phase::FastApproach; + bool approach = seeking || phase == Machine::Homing::Phase::SlowApproach; AxisMask axesMask = 0; // Find the axis that will take the longest @@ -59,29 +133,26 @@ namespace Machine { // Record active axes for the next phase set_bitnum(axesMask, axis); - // Set target location for active axes and setup computation for homing rate. - set_motor_steps(axis, 0); - auto axisConfig = axes->_axis[axis]; auto homing = axisConfig->_homing; - settle = std::max(settle, homing->_settle_ms); + settle_ms = std::max(settle_ms, homing->_settle_ms); float axis_rate; float travel; switch (phase) { - case HomingPhase::FastApproach: + case Machine::Homing::Phase::FastApproach: axis_rate = homing->_seekRate; travel = axisConfig->_maxTravel; break; - case HomingPhase::PrePulloff: - case HomingPhase::SlowApproach: - case HomingPhase::Pulloff0: - case HomingPhase::Pulloff1: + case Machine::Homing::Phase::PrePulloff: + case Machine::Homing::Phase::SlowApproach: + case Machine::Homing::Phase::Pulloff0: + case Machine::Homing::Phase::Pulloff1: axis_rate = homing->_feedRate; travel = axisConfig->commonPulloff(); break; - case HomingPhase::Pulloff2: + case Machine::Homing::Phase::Pulloff2: axis_rate = homing->_feedRate; travel = axisConfig->extraPulloff(); if (travel < 0) { @@ -96,125 +167,189 @@ namespace Machine { break; } + // Set target direction based on various factors + switch (phase) { + case Machine::Homing::Phase::PrePulloff: { + // For PrePulloff, the motion depends on which switches are active. + 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 (posLimited && negLimited) { + log_error("Both positive and negative limit switches are active for axis " << axes->axisName(axis)); + // xxx need to abort somehow + return; + } + if (posLimited) { + distance[axis] = -travel; + } else if (negLimited) { + distance[axis] = travel; + } else { + distance[axis] = 0; + } + } break; + + case Machine::Homing::Phase::FastApproach: + case Machine::Homing::Phase::SlowApproach: + distance[axis] = homing->_positiveDirection ? travel : -travel; + break; + + case Machine::Homing::Phase::Pulloff0: + case Machine::Homing::Phase::Pulloff1: + case Machine::Homing::Phase::Pulloff2: + distance[axis] = homing->_positiveDirection ? -travel : travel; + break; + } + // Accumulate the squares of the homing rates for later use // in computing the aggregate feed rate. - rate += (axis_rate * axis_rate); + ratesq += (axis_rate * axis_rate); - // First we compute the maximum-time-to-completion vector; later we will - // convert it back to positions after we determine the limiting axis. - // Set target direction based on cycle mask and homing cycle approach state. - auto seekTime = travel / axis_rate; - - target[axis] = (homing->_positiveDirection ^ _approach) ? -travel : travel; - rates[axis] = axis_rate; + rates[axis] = axis_rate; + auto seekTime = travel / axis_rate; if (seekTime > maxSeekTime) { maxSeekTime = seekTime; limitingRate = axis_rate; } } - // Scale the target array, currently in units of time, back to positions + // Scale the distance array, currently in units of time, back to positions // When approaching add a fudge factor (scaler) to ensure that the limit is reached - // but no fudge factor when pulling off. for (int axis = 0; axis < n_axis; axis++) { if (bitnum_is_true(axesMask, axis)) { - auto homing = axes->_axis[axis]->_homing; - auto scaler = _approach ? (seeking ? homing->_seek_scaler : homing->_feed_scaler) : 1.0; - target[axis] *= scaler; - if (phase == HomingPhase::FastApproach) { + auto paxis = axes->_axis[axis]; + auto homing = paxis->_homing; + auto scaler = approach ? (seeking ? homing->_seek_scaler : homing->_feed_scaler) : 1.0; + distance[axis] *= scaler; + if (phase == Machine::Homing::Phase::FastApproach) { // For fast approach the vector direction is determined by the rates - target[axis] *= rates[axis] / limitingRate; + distance[axis] *= rates[axis] / limitingRate; } - log_debug(Axes::_names[axis] << " target " << target[axis] << " rate " << rates[axis]); + target[axis] += distance[axis]; + log_debug(axes->axisName(axis) << " target " << target[axis] << " rate " << rates[axis]); } } - plan_line_data_t plan_data; - plan_data.spindle_speed = 0; - plan_data.motion = {}; - plan_data.motion.systemMotion = 1; - plan_data.motion.noFeedOverride = 1; - plan_data.spindle = SpindleState::Disable; - plan_data.coolant.Mist = 0; - plan_data.coolant.Flood = 0; - plan_data.line_number = REPORT_LINE_NUMBER; - plan_data.is_jog = false; + rate = sqrtf(ratesq); // Magnitude of homing rate vector + log_debug("planned move to " << target[0] << "," << target[1] << "," << target[2] << " @ " << rate); + } + + void Homing::runPhase() { + _phaseAxes = _cycleAxes; + _phaseMotors = _cycleMotors; - plan_data.feed_rate = sqrtf(rate); // Magnitude of homing rate vector - plan_buffer_line(target, &plan_data); // Bypass mc_move_motors(). Directly plan homing motion. + if (_phase == Phase::PrePulloff) { + Machine::EventPin::check(); + if (!(limited() & _phaseMotors)) { + // No initial pulloff needed + nextPhase(); + return; + } + } - sys.step_control = {}; - sys.step_control.executeSysMotion = true; // Set to execute homing motion and clear existing flags. - Stepper::prep_buffer(); // Prep and fill segment buffer from newly planned block. - Stepper::wake_up(); // Initiate motion + if (approach()) { + Machine::EventPin::check(); + } - return settle; + config->_kinematics->releaseMotors(_phaseAxes, _phaseMotors, _phase); + + float* target = get_mpos(); + float rate; + axisVector(_phaseAxes, _phaseMotors, _phase, target, rate, _settling_ms); + startMove(target, rate); } - void Homing::run(MotorMask remainingMotors, HomingPhase phase) { - // See if any motors are left. This could be 0 if none of the motors specified - // by the original value of axes is capable of standard homing. - if (remainingMotors == 0) { + void Homing::limitReached() { + // As limit bits are set, let the kinematics system figure out what that + // means in terms of axes, motors, and whether to stop and replan + MotorMask limited = Machine::Axes::posLimitMask | Machine::Axes::negLimitMask; + + log_debug("Homing limited " << config->_axes->maskToNames(limited)); + + if (!approach()) { + // We are not supposed to see a limitReached event while pulling off + fail(ExecAlarm::HomingFailPulloff); return; } - if (phase == HomingPhase::PrePulloff) { - // Pulloff to clear switches - if (!((Machine::Axes::posLimitMask | Machine::Axes::negLimitMask) & remainingMotors)) { - return; + bool stop = config->_kinematics->limitReached(_phaseAxes, _phaseMotors, limited); + + // stop tells us whether we have to halt the motion and replan a new move to + // complete the homing cycle for this set of axes. + + if (stop) { + Stepper::reset(); // Stop moving + + if (_phaseAxes) { + log_debug("Homing replan with " << config->_axes->maskToNames(_phaseAxes)); + + config->_kinematics->releaseMotors(_phaseAxes, _phaseMotors, _phase); + + // If there are any axes that have not yet hit their limits, replan with + // the remaining axes. + float* target = get_mpos(); + float rate; + axisVector(_phaseAxes, _phaseMotors, _phase, target, rate, _settling_ms); + startMove(target, rate); + } else { + // If all axes have hit their limits, this phase is complete and + // we can start the next one + delay_ms(_settling_ms); // Delay to allow transient dynamics to dissipate. + nextPhase(); } } + } - _approach = (phase == HomingPhase::FastApproach || phase == HomingPhase::SlowApproach); + void Homing::done() { + log_debug("Homing done"); - uint32_t settling_ms = plan_move(remainingMotors, phase); + if (sys.abort) { + return; // Did not complete. Alarm state set by mc_alarm. + } + // Homing cycle complete! Setup system for normal operation. + // ------------------------------------------------------------------------------------- + // Sync gcode parser and planner positions to homed position. + gc_sync_position(); + plan_sync_position(); - do { - if (_approach) { - // As limit bits are set, remove the corresponding bits from remaingMotors. - // The stepping ISR code takes care of stopping the motors when limit bits are set - MotorMask limitedMotors = Machine::Axes::posLimitMask | Machine::Axes::negLimitMask; - clear_bits(remainingMotors, limitedMotors); - } + Machine::EventPin::check(); + config->_stepping->endLowLatency(); - Stepper::prep_buffer(); // Check and prep segment buffer. + 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. + } + } - // This checks some of the events that would normally be handled - // by protocol_execute_realtime(). The homing loop is time-critical - // so we handle those events directly here, calling protocol_execute_realtime() - // only if one of those events is active. - if (rtStatusReport) { - rtStatusReport = false; - report_realtime_status(allChannels); - } - if (rtReset) { - // Homing failure: Reset issued during cycle. - throw ExecAlarm::HomingFailReset; - } - if (rtSafetyDoor) { - // Homing failure: Safety door was opened. - throw ExecAlarm::HomingFailDoor; - } - if (rtCycleStop) { - rtCycleStop = false; - if (_approach) { - // Homing failure: Limit switch not found during approach. - throw ExecAlarm::HomingFailApproach; - } - // Pulloff - if ((Machine::Axes::posLimitMask | Machine::Axes::negLimitMask) & remainingMotors) { - // Homing failure: Limit switch still engaged after pull-off motion - throw ExecAlarm::HomingFailPulloff; - } - // Normal termination for pulloff cycle - remainingMotors = 0; + void Homing::nextCycle() { + // Start the next cycle in the queue + if (sys.state == State::Alarm) { + while (!_remainingCycles.empty()) { + _remainingCycles.pop(); } - pollChannels(); - } while (remainingMotors); + return; + } + if (_remainingCycles.empty()) { + done(); + return; + } + _cycleAxes = _remainingCycles.front(); + _remainingCycles.pop(); + + log_debug("Homing Cycle " << config->_axes->maskToNames(_cycleAxes)); + + _cycleAxes &= Machine::Axes::homingMask; + _cycleMotors = config->_axes->set_homing_mode(_cycleAxes, true); + + _phase = Phase::PrePulloff; + runPhase(); + } - Stepper::reset(); // Immediately force kill steppers and reset step segment buffer. - delay_ms(settling_ms); // Delay to allow transient dynamics to dissipate. - _approach = false; + void Homing::fail(ExecAlarm alarm) { + Stepper::reset(); // Stop moving + Machine::EventPin::check(); + rtAlarm = alarm; + config->_axes->set_homing_mode(_cycleAxes, false); // tell motors homing is done...failed } bool Homing::needsPulloff2(MotorMask motors) { @@ -240,42 +375,34 @@ namespace Machine { return false; } - // Homes the specified cycle axes, sets the machine position, and performs a pull-off motion after - // completing. Homing is a special motion case, which involves rapid uncontrolled stops to locate - // the trigger point of the limit switches. The rapid stops are handled by a system level axis lock - // mask, which prevents the stepper algorithm from executing step pulses. Homing motions typically - // circumvent the processes for executing motions in normal operation. - // NOTE: Only the abort realtime command can interrupt this process. - - // axes cannot be 0. The 0 case - run all cycles - is - // handled by the caller mc_homing_cycle() - - void Homing::set_mpos(AxisMask axisMask) { - // The active cycle axes should now be homed and machine limits have been located. By - // default, as with most CNCs, machine space is all negative, but that can be changed. - // Since limit switches - // can be on either side of an axes, check and set axes machine zero appropriately. Also, - // set up pull-off maneuver from axes limit switches that have been homed. This provides - // some initial clearance off the switches and should also help prevent them from falsely - // triggering when hard limits are enabled or when more than one axes shares a limit pin. - + void Homing::set_mpos() { auto axes = config->_axes; auto n_axis = axes->_numberAxis; - // Set machine positions for homed limit switches. Don't update non-homed axes. - for (int axis = 0; axis < n_axis; axis++) { - if (bitnum_is_true(axisMask, axis)) { - set_motor_steps(axis, mpos_to_steps(axes->_axis[axis]->_homing->_mpos, axis)); + float* mpos = get_mpos(); + + log_debug("mpos was " << mpos[0] << "," << mpos[1] << "," << mpos[2]); + // Replace coordinates homed axes with the homing values. + for (size_t axis = 0; axis < n_axis; axis++) { + if (bitnum_is_true(_cycleAxes, axis)) { + mpos[axis] = axes->_axis[axis]->_homing->_mpos; } } - sys.step_control = {}; // Return step control to normal operation. - axes->set_homing_mode(axisMask, false); // tell motors homing is done + log_debug("mpos becomes " << mpos[0] << "," << mpos[1] << "," << mpos[2]); + + set_motor_steps_from_mpos(mpos); + + mpos = get_mpos(); + log_debug("mpos transformed " << mpos[0] << "," << mpos[1] << "," << mpos[2]); + + sys.step_control = {}; // Return step control to normal operation. + axes->set_homing_mode(_cycleAxes, false); // tell motors homing is done } static String axisNames(AxisMask axisMask) { String retval = ""; auto n_axis = config->_axes->_numberAxis; - for (int axis = 0; axis < n_axis; axis++) { + for (size_t axis = 0; axis < n_axis; axis++) { if (bitnum_is_true(axisMask, axis)) { retval += Machine::Axes::_names[axis]; } @@ -283,69 +410,46 @@ namespace Machine { return retval; } - void Homing::run_one_cycle(AxisMask axisMask) { - axisMask &= Machine::Axes::homingMask; - log_debug("Homing " << config->_axes->maskToNames(axisMask)); - - MotorMask motors = config->_axes->set_homing_mode(axisMask, true); - - try { - log_debug("PrePulloff"); - run(motors, HomingPhase::PrePulloff); // Approach slowly - log_debug("Fast approach"); - run(motors, HomingPhase::FastApproach); // Approach slowly - log_debug("Pulloff0"); - run(motors, HomingPhase::Pulloff0); - log_debug("Slow approach"); - run(motors, HomingPhase::SlowApproach); // Approach slowly - log_debug("Pulloff1"); - run(motors, HomingPhase::Pulloff1); - if (needsPulloff2(motors)) { - log_debug("Differential Pulloff"); - // At least one axis has multiple motors with different pulloffs - run(motors, HomingPhase::Pulloff2); - } - } catch (ExecAlarm alarm) { - rtAlarm = alarm; - config->_axes->set_homing_mode(axisMask, false); // tell motors homing is done...failed - log_error("Homing fail"); - mc_reset(); // Stop motors, if they are running. - // protocol_execute_realtime() will handle any pending rtXXX conditions - protocol_execute_realtime(); + // Construct a list of homing cycles to run. If there are any + // such cycles, enter Homing state and begin running the first + // cycle. The protocol loop will then respond to events and advance + // the homing state machine through its phases. + void Homing::run_cycles(AxisMask axisMask) { + if (!config->_kinematics->canHome(axisMask)) { + log_error("This kinematic system cannot do homing"); + sys.state = State::Alarm; return; } - set_mpos(axisMask); - } + while (!_remainingCycles.empty()) { + _remainingCycles.pop(); + } - // XXX this should return Error or at least set ExecAlarm::HomingNoCycles (not yet defined) - void Homing::run_cycles(AxisMask axisMask) { - // ------------------------------------------------------------------------------------- if (axisMask != AllCycles) { - run_one_cycle(axisMask); + _remainingCycles.push(axisMask); } else { // Run all homing cycles bool someAxisHomed = false; for (int cycle = 1; cycle <= MAX_N_AXIS; cycle++) { - // home() sets state to Homing. If a cycle fails, we skip following cycles - if (sys.state == State::Alarm) { - return; - } // Set axisMask to the axes that home on this cycle - axisMask = axis_mask_from_cycle(cycle); if (axisMask) { // if there are some axes in this cycle - someAxisHomed = true; - run_one_cycle(axisMask); + _remainingCycles.push(axisMask); } } - if (!someAxisHomed) { - log_error("No homing cycles defined"); - sys.state = State::Alarm; - } } + + if (_remainingCycles.empty()) { + log_error("No homing cycles defined"); + sys.state = State::Alarm; + return; + } + config->_stepping->beginLowLatency(); + + sys.state = State::Homing; + nextCycle(); } AxisMask Homing::axis_mask_from_cycle(int cycle) { diff --git a/FluidNC/src/Machine/Homing.h b/FluidNC/src/Machine/Homing.h index d9e3d777b..af51ff36c 100644 --- a/FluidNC/src/Machine/Homing.h +++ b/FluidNC/src/Machine/Homing.h @@ -4,40 +4,42 @@ #pragma once -#include "../Configuration/Configurable.h" -#include "../System.h" // AxisMask, MotorMask +#include "src/Configuration/Configurable.h" +#include "src/System.h" // AxisMask, MotorMask +#include "src/Protocol.h" // ExecAlarm +#include namespace Machine { class Homing : public Configuration::Configurable { - static void set_mpos(AxisMask axisMask); - - static const int REPORT_LINE_NUMBER = 0; - - static bool needsPulloff2(MotorMask motors); - - enum class HomingPhase { - PrePulloff, - FastApproach, - Pulloff0, - SlowApproach, - Pulloff1, - Pulloff2, - }; - - static uint32_t plan_move(MotorMask motors, HomingPhase phase); - public: + static enum Phase { + None = 0, + PrePulloff = 1, + FastApproach = 2, + Pulloff0 = 3, + SlowApproach = 4, + Pulloff1 = 5, + Pulloff2 = 6, + CycleDone = 7, + } _phase; + Homing() = default; static const int AllCycles = 0; // Must be zero. - static volatile bool _approach; + static bool approach() { return _phase == FastApproach || _phase == SlowApproach; } + + static void fail(ExecAlarm alarm); + static void cycleStop(); static void run_cycles(AxisMask axisMask); static void run_one_cycle(AxisMask axisMask); static AxisMask axis_mask_from_cycle(int cycle); - static void run(MotorMask remainingMotors, HomingPhase phase); + static void run(MotorMask remainingMotors, Phase phase); + + static void startMove(float* target, float rate); + static void axisVector(AxisMask axisMask, MotorMask motors, Phase phase, float* target, float& rate, uint32_t& settle_ms); // 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 @@ -67,6 +69,33 @@ namespace Machine { } void init() {} - }; + static void set_mpos(); + + static const int REPORT_LINE_NUMBER = 0; + + static bool needsPulloff2(MotorMask motors); + + static void limitReached(); + + private: + static uint32_t planMove(AxisMask axisMask, MotorMask motors, Phase phase, float* target, float& rate); + + static void done(); + static void runPhase(); + static void nextPhase(); + static void nextCycle(); + + static MotorMask _cycleMotors; // Motors for this cycle + static MotorMask _phaseMotors; // Motors still running in this phase + static AxisMask _cycleAxes; // Axes for this cycle + static AxisMask _phaseAxes; // Axes still active in this phase + + static std::queue _remainingCycles; + + static uint32_t _settling_ms; + + static const char* _phaseNames[]; + static const char* phaseName(Phase phase) { return _phaseNames[static_cast(phase)]; } + }; } diff --git a/FluidNC/src/Machine/LimitPin.cpp b/FluidNC/src/Machine/LimitPin.cpp index 89182dee3..563f484da 100644 --- a/FluidNC/src/Machine/LimitPin.cpp +++ b/FluidNC/src/Machine/LimitPin.cpp @@ -1,17 +1,20 @@ -#include "LimitPin.h" -#include "Axes.h" -#include "MachineConfig.h" // config +#include "src/Machine/EventPin.h" +#include "src/Machine/Axes.h" +#include "src/Machine/MachineConfig.h" // config -#include "../NutsBolts.h" // set_bitnum etc -#include "../MotionControl.h" // mc_reset -#include "../Limits.h" -#include "../Protocol.h" // rtAlarm +#include "src/NutsBolts.h" // set_bitnum etc +#include "src/MotionControl.h" // mc_reset +#include "src/Limits.h" +#include "src/Protocol.h" // protocol_send_event_from_ISR() -#include // CHANGE +#include "soc/soc.h" +#include "soc/gpio_periph.h" +#include "hal/gpio_hal.h" namespace Machine { + static void foo(void* bar) {}; LimitPin::LimitPin(Pin& pin, int axis, int motor, int direction, bool& pHardLimits, bool& pLimited) : - _axis(axis), _motorNum(motor), _value(false), _pHardLimits(pHardLimits), _pLimited(pLimited), _pin(pin) { + EventPin(&limitEvent, "Limit", pin), _axis(axis), _motorNum(motor), _value(false), _pHardLimits(pHardLimits), _pLimited(pLimited) { String sDir; // Select one or two bitmask variables to receive the switch data switch (direction) { @@ -43,34 +46,13 @@ namespace Machine { _legend = String(" " + sDir + " Limit"); } - void IRAM_ATTR LimitPin::handleISR() { - read(); - if (sys.state != State::Alarm && sys.state != State::ConfigAlarm && sys.state != State::Homing) { - if (_pHardLimits && rtAlarm == ExecAlarm::None) { -#if 0 + void IRAM_ATTR LimitPin::update(bool value) { + _pLimited = value; - if (config->_softwareDebounceMs) { - // send a message to wakeup the task that rechecks the switches after a small delay - int evt; - xQueueSendFromISR(limit_sw_queue, &evt, NULL); - return; - } -#endif - - // log_debug("Hard limits"); // This might not work from ISR context - mc_reset(); // Initiate system kill. - rtAlarm = ExecAlarm::HardLimit; // Indicate hard limit critical event - } - } - } - - void IRAM_ATTR LimitPin::read() { - _value = _pin.read(); - _pLimited = _value; if (_pExtraLimited != nullptr) { - *_pExtraLimited = _value; + *_pExtraLimited = value; } - if (_value) { + if (value) { if (_posLimits != nullptr) { set_bits(*_posLimits, _bitmask); } @@ -87,28 +69,14 @@ namespace Machine { } } - void LimitPin::init() { - if (_pin.undefined()) { - return; - } - set_bitnum(Axes::limitMask, _axis); - _pin.report(_legend.c_str()); - auto attr = Pin::Attr::Input | Pin::Attr::ISR; - if (_pin.capabilities().has(Pins::PinCapabilities::PullUp)) { - attr = attr | Pin::Attr::PullUp; - } - _pin.setAttr(attr); - _pin.attachInterrupt(ISRHandler, CHANGE, this); - - read(); - } - // Make this switch act like an axis level switch. Both motors will report the same // This should be called from a higher level object, that has the logic to figure out // if this belongs to a dual motor, single switch axis - void LimitPin::makeDualMask() { _bitmask = Axes::axes_to_motors(Axes::motors_to_axes(_bitmask)); } - - void LimitPin::setExtraMotorLimit(int axis, int motorNum) { _pExtraLimited = &config->_axes->_axis[axis]->_motors[motorNum]->_limited; } + void LimitPin::makeDualMask() { + _bitmask = Axes::axes_to_motors(Axes::motors_to_axes(_bitmask)); + } - LimitPin::~LimitPin() { _pin.detachInterrupt(); } + void LimitPin::setExtraMotorLimit(int axis, int motorNum) { + _pExtraLimited = &config->_axes->_axis[axis]->_motors[motorNum]->_limited; + } } diff --git a/FluidNC/src/Machine/LimitPin.h b/FluidNC/src/Machine/LimitPin.h index 509907c25..6439aa3f3 100644 --- a/FluidNC/src/Machine/LimitPin.h +++ b/FluidNC/src/Machine/LimitPin.h @@ -1,15 +1,10 @@ #pragma once -#include "../Pin.h" - -#include // IRAM_ATTR +#include "EventPin.h" namespace Machine { - class LimitPin { + class LimitPin : public EventPin { private: - int _axis; - int _motorNum; - bool _value = 0; uint32_t _bitmask = 0; @@ -28,24 +23,18 @@ namespace Machine { volatile uint32_t* _posLimits = nullptr; volatile uint32_t* _negLimits = nullptr; - void IRAM_ATTR handleISR(); - - CreateISRHandlerFor(LimitPin, handleISR); - - void read(); - public: LimitPin(Pin& pin, int axis, int motorNum, int direction, bool& phardLimits, bool& pLimited); - Pin& _pin; - - String _legend; + void update(bool value) override; - void init(); - bool get() { return _value; } + //void init(); void makeDualMask(); // makes this a mask for motor0 and motor1 void setExtraMotorLimit(int axis, int motorNum); - ~LimitPin(); + bool isHard() { return _pHardLimits; } + + int _axis; + int _motorNum; }; } diff --git a/FluidNC/src/Machine/Macros.cpp b/FluidNC/src/Machine/Macros.cpp new file mode 100644 index 000000000..0dedb6eab --- /dev/null +++ b/FluidNC/src/Machine/Macros.cpp @@ -0,0 +1,20 @@ +// Copyright (c) 2022 - Mitch Bradley +// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file. + +#include "Macros.h" +#include "src/System.h" // sys +#include "src/Machine/MachineConfig.h" // config + +void MacroEvent::run(void* 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); +} + +MacroEvent macro0Event { 0 }; +MacroEvent macro1Event { 1 }; +MacroEvent macro2Event { 2 }; +MacroEvent macro3Event { 3 }; diff --git a/FluidNC/src/Machine/Macros.h b/FluidNC/src/Machine/Macros.h index 71ba27258..869082b2d 100644 --- a/FluidNC/src/Machine/Macros.h +++ b/FluidNC/src/Machine/Macros.h @@ -4,9 +4,23 @@ #pragma once -#include "../Configuration/Configurable.h" -#include "../WebUI/InputBuffer.h" // WebUI::inputBuffer -#include "../Uart.h" +#include "src/Configuration/Configurable.h" +#include "src/WebUI/InputBuffer.h" // WebUI::inputBuffer +#include "src/Uart.h" +#include "src/Event.h" + +class MacroEvent : public Event { + int _num; + +public: + MacroEvent(int num) : _num(num) {} + void run(void*) override; +}; + +extern MacroEvent macro0Event; +extern MacroEvent macro1Event; +extern MacroEvent macro2Event; +extern MacroEvent macro3Event; namespace Machine { class Macros : public Configuration::Configurable { diff --git a/FluidNC/src/Machine/Motor.cpp b/FluidNC/src/Machine/Motor.cpp index c86009b5c..1b575222d 100644 --- a/FluidNC/src/Machine/Motor.cpp +++ b/FluidNC/src/Machine/Motor.cpp @@ -69,7 +69,7 @@ namespace Machine { void IRAM_ATTR Motor::step(bool reverse) { // Skip steps based on limit pins - if (_blocked || (_limited && (Homing::_approach || (sys.state != State::Homing && _hardLimits)))) { + if (_blocked || (_limited && (Homing::approach() || (sys.state != State::Homing && _hardLimits)))) { return; } _driver->step(); diff --git a/FluidNC/src/Machine/Motor.h b/FluidNC/src/Machine/Motor.h index 90674fd45..fb14faca7 100644 --- a/FluidNC/src/Machine/Motor.h +++ b/FluidNC/src/Machine/Motor.h @@ -52,6 +52,7 @@ namespace Machine { void unstep(); void block() { _blocked = true; } void unblock() { _blocked = false; } + void unlimit() { _limited = false; } ~Motor(); }; } diff --git a/FluidNC/src/Main.cpp b/FluidNC/src/Main.cpp index f3c443031..f5cc037c2 100644 --- a/FluidNC/src/Main.cpp +++ b/FluidNC/src/Main.cpp @@ -41,6 +41,8 @@ void setup() { display_init(); + protocol_init(); + // Load settings from non-volatile storage settings_init(); // requires config diff --git a/FluidNC/src/MotionControl.cpp b/FluidNC/src/MotionControl.cpp index 45ff79c25..c8ca27b66 100644 --- a/FluidNC/src/MotionControl.cpp +++ b/FluidNC/src/MotionControl.cpp @@ -252,39 +252,6 @@ bool mc_dwell(int32_t milliseconds) { return delay_msec(milliseconds, DwellMode::Dwell); } -// Perform homing cycle to locate and set machine zero. Only '$H' executes this command. -// NOTE: There should be no motions in the buffer and the system must be in idle state before -// executing the homing cycle. This prevents incorrect buffered plans after homing. -void mc_homing_cycle(AxisMask axis_mask) { - if (config->_kinematics->kinematics_homing(axis_mask)) { - // Allow kinematics to replace homing. - // TODO: Better integrate this logic. - return; - } - - // Abort homing cycle if an axis has limit switches engaged on both ends, - // or if it is impossible to tell which end is engaged. In that situation - // we do not know the pulloff direction. - if (ambiguousLimit()) { - return; - } - - // Might set an alarm; if so protocol_execute_realtime will handle it - Machine::Homing::run_cycles(axis_mask); - - protocol_execute_realtime(); // Check for reset and set system abort. - if (sys.abort) { - return; // Did not complete. Alarm state set by mc_alarm. - } - // Homing cycle complete! Setup system for normal operation. - // ------------------------------------------------------------------------------------- - // Sync gcode parser and planner positions to homed position. - gc_sync_position(); - plan_sync_position(); - // This give kinematics a chance to do something after normal homing - config->_kinematics->kinematics_post_homing(); -} - volatile ProbeState probeState; bool probe_succeeded = false; @@ -324,7 +291,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, bool away, // Activate the probing state monitor in the stepper module. probeState = ProbeState::Active; // Perform probing cycle. Wait here until probe is triggered or motion completes. - rtCycleStart = true; + protocol_send_event(&cycleStartEvent); do { pollChannels(); protocol_execute_realtime(); diff --git a/FluidNC/src/MotionControl.h b/FluidNC/src/MotionControl.h index eaebb68be..d90a9c8ec 100644 --- a/FluidNC/src/MotionControl.h +++ b/FluidNC/src/MotionControl.h @@ -64,6 +64,3 @@ void mc_reset(); void mc_cancel_jog(); void mc_init(); - -bool kinematics_pre_homing(AxisMask cycle_mask); -void kinematics_post_homing(); diff --git a/FluidNC/src/Pin.h b/FluidNC/src/Pin.h index c20b01a4e..d3fb5c135 100644 --- a/FluidNC/src/Pin.h +++ b/FluidNC/src/Pin.h @@ -25,14 +25,16 @@ class String; // with ISRHandler. // // Usage: -// - In header file (private / protected members) or in cpp file in anonymous namespace (public members) +// - In header file (private / protected members) or in cpp file in anonymous namespace (public members) // CreateISRHandlerFor(LimitPin, handleISR); -// - When attaching an ISR: _pin.attachInterrupt(ISRHandler, CHANGE, this); +// - When attaching an ISR: _pin.attachInterrupt(ISRHandler, EITHER_EDGE, this); // // I'd rather not use any defines, but templates... but apparently there's no choice here. Let's just make it as safe // as possible... #define CreateISRHandlerFor(className, methodName) \ - static void IRAM_ATTR ISRHandler(void* data) { static_cast(data)->methodName(); } + static void IRAM_ATTR ISRHandler(void* data) { \ + static_cast(data)->methodName(); \ + } // Pin class. A pin is basically a thing that can 'output', 'input' or do both. GPIO on an ESP32 comes to mind, // but there are way more possible pins. Think about I2S/I2C/SPI extenders, RS485 driven pin devices and even @@ -93,6 +95,14 @@ class Pin { static const bool On = true; static const bool Off = false; + static const int NO_INTERRUPT = 0; + static const int RISING_EDGE = 1; + static const int FALLING_EDGE = 2; + static const int EITHER_EDGE = 3; + + static const int ASSERTING = 0x10; + static const int DEASSERTING = 0x11; + // inline static Pins::PinDetail* create(const char* str) { return create(StringRange(str)); }; static Pin create(const char* str) { return create(StringRange(str)); } // ensure it's not ambiguous @@ -107,7 +117,7 @@ class Pin { inline Pin(Pin&& o) : _detail(nullptr) { std::swap(_detail, o._detail); } inline Pin& operator=(const Pin& o) = delete; - inline Pin& operator =(Pin&& o) { + inline Pin& operator=(Pin&& o) { std::swap(_detail, o._detail); return *this; } @@ -153,6 +163,7 @@ class Pin { inline String name() const { return _detail->toString(); } void report(const char* legend); + void report(String legend) { report(legend.c_str()); } inline void swap(Pin& o) { std::swap(o._detail, _detail); } diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index cd25e4968..7789e41ce 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -284,6 +284,10 @@ static Error disable_alarm_lock(const char* value, WebUI::AuthenticationLevel au } report_feedback_message(Message::AlarmUnlock); sys.state = State::Idle; + + // Turn event pin ISRs back on; they could be off due to hard limit triggering + Machine::EventPin::check(); + // Don't run startup script. Prevents stored moves in startup from causing accidents. } // Otherwise, no effect. return Error::Ok; @@ -292,12 +296,12 @@ static Error report_ngc(const char* value, WebUI::AuthenticationLevel auth_level report_ngc_parameters(out); return Error::Ok; } -static Error home(int cycle) { - if (cycle != 0) { // if not AllCycles we need to make sure the cycle is not prohibited +static Error home(AxisMask axisMask) { + if (axisMask != Machine::Homing::AllCycles) { // if not AllCycles we need to make sure the cycle is not prohibited // if there is a cycle it is the axis from $H auto n_axis = config->_axes->_numberAxis; for (int axis = 0; axis < n_axis; axis++) { - if (bitnum_is_true(cycle, axis)) { + if (bitnum_is_true(axisMask, axis)) { auto axisConfig = config->_axes->_axis[axis]; auto homing_allowed = axisConfig->_homing->_allow_single_axis; if (!homing_allowed) @@ -317,21 +321,15 @@ static Error home(int cycle) { return Error::CheckDoor; // Block if safety door is ajar. } - sys.state = State::Homing; // Set system state variable + Machine::Homing::run_cycles(axisMask); - config->_stepping->beginLowLatency(); - - mc_homing_cycle(cycle); + do { + pollChannels(); + protocol_execute_realtime(); + } while (sys.state == State::Homing); - config->_stepping->endLowLatency(); + settings_execute_startup(); - 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 (cycle == Machine::Homing::AllCycles) { - settings_execute_startup(); - } - } return Error::Ok; } static Error home_all(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { @@ -415,6 +413,7 @@ static Error show_limits(const char* value, WebUI::AuthenticationLevel auth_leve out << " PosLimitPins NegLimitPins\n"; const TickType_t interval = 500; TickType_t limit = xTaskGetTickCount(); + runLimitLoop = true; do { TickType_t thisTime = xTaskGetTickCount(); if (((long)(thisTime - limit)) > 0) { @@ -426,14 +425,14 @@ static Error show_limits(const char* value, WebUI::AuthenticationLevel auth_leve limit = thisTime + interval; } vTaskDelay(1); + protocol_handle_events(); pollChannels(); - } while (!rtFeedHold); - rtFeedHold = false; + } while (runLimitLoop); out << '\n'; return Error::Ok; } static Error go_to_sleep(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { - rtSleep = true; + protocol_send_event(&sleepEvent); return Error::Ok; } static Error get_report_build_info(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { diff --git a/FluidNC/src/Protocol.cpp b/FluidNC/src/Protocol.cpp index 579f26c3d..223b801f9 100644 --- a/FluidNC/src/Protocol.cpp +++ b/FluidNC/src/Protocol.cpp @@ -8,18 +8,18 @@ */ #include "Protocol.h" +#include "Event.h" #include "Machine/MachineConfig.h" +#include "Machine/Homing.h" #include "Report.h" // report_feedback_message #include "Limits.h" // limits_get_state, soft_limit #include "Planner.h" // plan_get_current_block #include "MotionControl.h" // PARKING_MOTION_LINE_NUMBER #include "Settings.h" // settings_execute_startup #include "InputFile.h" // infile - -#ifdef DEBUG_REPORT_REALTIME -volatile bool rtExecDebug; -#endif +#include "Logging.h" +#include "Machine/LimitPin.h" volatile ExecAlarm rtAlarm; // Global realtime executor bitflag variable for setting various alarms. @@ -39,23 +39,11 @@ std::map AlarmNames = { { ExecAlarm::HomingAmbiguousSwitch, "Ambiguous Switch" }, }; -volatile Accessory rtAccessoryOverride; // Global realtime executor bitflag variable for spindle/coolant overrides. -volatile Percent rtFOverride; // Global realtime executor feedrate override percentage -volatile Percent rtROverride; // Global realtime executor rapid override percentage -volatile Percent rtSOverride; // Global realtime executor spindle override percentage - -volatile bool rtStatusReport; -volatile bool rtCycleStart; -volatile bool rtFeedHold; volatile bool rtReset; -volatile bool rtSafetyDoor; -volatile bool rtMotionCancel; -volatile bool rtSleep; -volatile bool rtCycleStop; // For state transitions, instead of bitflag -volatile bool rtButtonMacro0; -volatile bool rtButtonMacro1; -volatile bool rtButtonMacro2; -volatile bool rtButtonMacro3; + +static volatile bool rtSafetyDoor; + +volatile bool runLimitLoop; // Interface to show_limits() static void protocol_exec_rt_suspend(); @@ -81,31 +69,31 @@ static SpindleStop spindle_stop_ovr; bool can_park() { if (spindle->isRateAdjusted()) { + // No parking in Laser mode return false; } - if (config->_enableParkingOverrideControl) { - return sys.override_ctrl == Override::ParkingMotion && Machine::Axes::homingMask; - } else { - return Machine::Axes::homingMask; + if (!Machine::Axes::homingMask) { + // No parking without homing + return false; } + if (!config->_enableParkingOverrideControl) { + // _enableParkingOverrideControl adds M56 whereby you can + // disable parking via GCode. If that feature is not present, + // parking is enabled subject to the preceding tests. + return true; + } + // If the M56 feature is present, M56 controls the value + // of sys.override_ctrl, thus letting you disable parking + // by saying M56 P0 + return sys.override_ctrl == Override::ParkingMotion; } void protocol_reset() { - probeState = ProbeState::Off; - soft_limit = false; - rtStatusReport = false; - rtCycleStart = false; - rtFeedHold = false; - rtReset = false; - rtSafetyDoor = false; - rtMotionCancel = false; - rtSleep = false; - rtCycleStop = false; - rtAccessoryOverride.value = 0; - rtFOverride = FeedOverride::Default; - rtROverride = RapidOverride::Default; - rtSOverride = SpindleSpeedOverride::Default; - spindle_stop_ovr.value = 0; + 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; @@ -118,6 +106,10 @@ static int32_t idleEndTime = 0; */ Channel* exclusiveChannel = nullptr; +static void request_safety_door() { + rtSafetyDoor = true; +} + void protocol_main_loop() { // 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. @@ -143,7 +135,7 @@ void protocol_main_loop() { // Check if the safety door is open. sys.state = State::Idle; if (config->_control->safety_door_ajar()) { - rtSafetyDoor = true; + request_safety_door(); protocol_execute_realtime(); // Enter safety door mode. Should return as IDLE state. } // All systems go! @@ -248,8 +240,9 @@ void protocol_buffer_synchronize() { // is finished, single commands), a command that needs to wait for the motions in the buffer to // execute calls a buffer sync, or the planner buffer is full and ready to go. void protocol_auto_cycle_start() { - if (plan_get_current_block() != NULL) { // Check if there are any blocks in the buffer. - rtCycleStart = true; // If so, execute them! + if (plan_get_current_block() != NULL && sys.state != State::Cycle && + sys.state != State::Hold) { // Check if there are any blocks in the buffer. + protocol_send_event(&cycleStartEvent); // If so, execute them } } @@ -292,6 +285,7 @@ static void protocol_do_alarm() { 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 @@ -328,10 +322,9 @@ static void protocol_hold_complete() { } static void protocol_do_motion_cancel() { + // log_debug("protocol_do_motion_cancel " << state_name()); // Execute and flag a motion cancel with deceleration and return to idle. Used primarily by probing cycle // to halt and cancel the remainder of the motion. - rtMotionCancel = false; - rtCycleStart = false; // Cancel any pending start // MOTION_CANCEL only occurs during a CYCLE, but a HOLD and SAFETY_DOOR may have been initiated // beforehand. Motion cancel affects only a single planner block motion, while jog cancel @@ -356,9 +349,10 @@ static void protocol_do_motion_cancel() { // When jogging, we do not set motionCancel, hence return not break return; + case State::Homing: + // XXX maybe motion cancel should stop homing case State::Sleep: case State::Hold: - case State::Homing: case State::SafetyDoor: break; } @@ -366,9 +360,8 @@ static void protocol_do_motion_cancel() { } static void protocol_do_feedhold() { + // log_debug("protocol_do_feedhold " << state_name()); // Execute a feed hold with deceleration, if required. Then, suspend system. - rtFeedHold = false; - rtCycleStart = false; // Cancel any pending start switch (sys.state) { case State::ConfigAlarm: case State::Alarm: @@ -377,11 +370,13 @@ static void protocol_do_feedhold() { case State::Sleep: return; // Do not change the state to Hold - case State::Hold: case State::Homing: + // XXX maybe feedhold should stop homing + case State::Hold: break; case State::Idle: + runLimitLoop = false; // Hack to stop show_limits() protocol_hold_complete(); break; @@ -398,11 +393,11 @@ static void protocol_do_feedhold() { } static void protocol_do_safety_door() { + // log_debug("protocol_do_safety_door " << int(sys.state)); // Execute a safety door stop with a feed hold and disable spindle/coolant. // NOTE: Safety door differs from feed holds by stopping everything no matter state, disables powered // devices (spindle/coolant), and blocks resuming until switch is re-engaged. - rtCycleStart = false; // Cancel any pending start report_feedback_message(Message::SafetyDoorAjar); switch (sys.state) { case State::ConfigAlarm: @@ -416,7 +411,7 @@ static void protocol_do_safety_door() { case State::Hold: break; case State::Homing: - rtAlarm = ExecAlarm::HomingFailDoor; + Machine::Homing::fail(ExecAlarm::HomingFailDoor); break; case State::SafetyDoor: if (!sys.suspend.bit.jogCancel && sys.suspend.bit.initiateRestore) { // Actively restoring @@ -457,7 +452,7 @@ static void protocol_do_safety_door() { } static void protocol_do_sleep() { - rtSleep = false; + // log_debug("protocol_do_sleep " << state_name()); switch (sys.state) { case State::ConfigAlarm: case State::Alarm: @@ -491,6 +486,7 @@ void protocol_cancel_disable_steppers() { } static void protocol_do_initiate_cycle() { + // log_debug("protocol_do_initiate_cycle " << state_name()); // Start cycle only if queued motions exist in planner buffer and the motion is not canceled. sys.step_control = {}; // Restore step control to normal operation if (plan_get_current_block() && !sys.suspend.bit.motionCancel) { @@ -498,31 +494,40 @@ static void protocol_do_initiate_cycle() { sys.state = State::Cycle; Stepper::prep_buffer(); // Initialize step segment buffer before beginning cycle. Stepper::wake_up(); - } else { // Otherwise, do nothing. Set and resume IDLE state. + } else { // Otherwise, do nothing. Set and resume IDLE state. + sys.suspend.value = 0; // Break suspend state. sys.state = State::Idle; } } +static void protocol_initiate_homing_cycle() { + // log_debug("protocol_initiate_homing_cycle " << state_name()); + sys.step_control = {}; // Restore step control to normal operation + sys.suspend.value = 0; // Break suspend state. + sys.step_control.executeSysMotion = true; // Set to execute homing motion and clear existing flags. + Stepper::prep_buffer(); // Initialize step segment buffer before beginning cycle. + Stepper::wake_up(); +} -// The handlers for rtFeedHold, rtMotionCancel, and rtsDafetyDoor clear rtCycleStart to -// ensure that auto-cycle-start does not resume a hold without explicit user input. static void protocol_do_cycle_start() { - rtCycleStart = false; - + // log_debug("protocol_do_cycle_start " << state_name()); // Execute a cycle start by starting the stepper interrupt to begin executing the blocks in queue. // Resume door state when parking motion has retracted and door has been closed. switch (sys.state) { case State::SafetyDoor: if (!sys.suspend.bit.safetyDoorAjar) { + // If the safety door is still open, do not restart yet if (sys.suspend.bit.restoreComplete) { - sys.state = State::Idle; // Set to IDLE to immediately resume the cycle. + // Restore is complete. Set the state to IDLE and resume normal motion + sys.state = State::Idle; + protocol_do_initiate_cycle(); } else if (sys.suspend.bit.retractComplete) { - // Flag to re-energize powered components and restore original position, if disabled by SAFETY_DOOR. - // NOTE: For a safety door to resume, the switch must be closed, as indicated by HOLD state, and - // the retraction execution is complete, which implies the initial feed hold is not active. To - // restore normal operation, the restore procedures must be initiated by the following flag. Once, - // they are complete, it will call CYCLE_START automatically to resume and exit the suspend. + // retractComplete means that all of the retraction operations that were + // initiated by the safety door opening, such as spindle stop and parking, + // are done. Thus we can respond to this cycle start by "restoring", + // i.e. undoing those retraction operations. Afterwards, restoreComplete + // will be set and another cycle start event will be issued automatically. sys.suspend.bit.initiateRestore = true; } } @@ -530,6 +535,9 @@ static void protocol_do_cycle_start() { case State::Idle: protocol_do_initiate_cycle(); break; + case State::Homing: + protocol_initiate_homing_cycle(); + break; case State::Hold: // Cycle start only when IDLE or when a hold is complete and ready to resume. if (sys.suspend.bit.holdComplete) { @@ -545,7 +553,6 @@ static void protocol_do_cycle_start() { case State::CheckMode: case State::Sleep: case State::Cycle: - case State::Homing: case State::Jog: break; } @@ -580,8 +587,7 @@ void protocol_disable_steppers() { } void protocol_do_cycle_stop() { - rtCycleStop = false; - + // log_debug("protocol_do_cycle_stop " << state_name()); protocol_disable_steppers(); switch (sys.state) { @@ -592,7 +598,7 @@ void protocol_do_cycle_stop() { // realtime command execution in the main program, ensuring that the planner re-plans safely. // NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper // cycle reinitializations. The stepper path should continue exactly as if nothing has happened. - // NOTE: rtCycleStop is set by the stepper subsystem when a cycle or feed hold completes. + // NOTE: cycleStopEvent is set by the stepper subsystem when a cycle or feed hold completes. if (!soft_limit && !sys.suspend.bit.jogCancel) { // Hold complete. Set to indicate ready to resume. Remain in HOLD or DOOR states until user // has issued a resume command or reset. @@ -610,7 +616,6 @@ void protocol_do_cycle_stop() { case State::CheckMode: case State::Idle: case State::Cycle: - case State::Homing: case State::Jog: // Motion complete. Includes CYCLE/JOG/HOMING states and jog cancel/motion cancel/soft limit events. // NOTE: Motion and jog cancel both immediately return to idle after the hold completes. @@ -630,64 +635,16 @@ void protocol_do_cycle_stop() { sys.state = State::Idle; } break; + case State::Homing: + Machine::Homing::cycleStop(); + break; } } -static void protocol_execute_overrides() { - // Execute overrides. - if ((rtFOverride != sys.f_override) || (rtROverride != sys.r_override)) { - sys.f_override = rtFOverride; - sys.r_override = rtROverride; - report_ovr_counter = 0; // Set to report change immediately - plan_update_velocity_profile_parameters(); - plan_cycle_reinitialize(); - } - - // NOTE: Unlike motion overrides, spindle overrides do not require a planner reinitialization. - if (rtSOverride != sys.spindle_speed_ovr) { - sys.step_control.updateSpindleSpeed = true; - sys.spindle_speed_ovr = rtSOverride; - report_ovr_counter = 0; // Set to report change immediately - - // If spindle is on, tell it the RPM has been overridden - // When moving, the override is handled by the stepping code - if (gc_state.modal.spindle != SpindleState::Disable && !inMotionState()) { - spindle->setState(gc_state.modal.spindle, gc_state.spindle_speed); - report_ovr_counter = 0; // Set to report change immediately - } - } - - if (rtAccessoryOverride.bit.spindleOvrStop) { - rtAccessoryOverride.bit.spindleOvrStop = false; - // Spindle stop override allowed only while in HOLD state. - if (sys.state == State::Hold) { - if (spindle_stop_ovr.value == 0) { - spindle_stop_ovr.bit.initiate = true; - } else if (spindle_stop_ovr.bit.enabled) { - spindle_stop_ovr.bit.restore = true; - } - } - } - - // NOTE: Since coolant state always performs a planner sync whenever it changes, the current - // run state can be determined by checking the parser state. - if (rtAccessoryOverride.bit.coolantFloodOvrToggle) { - rtAccessoryOverride.bit.coolantFloodOvrToggle = false; - if (config->_coolant->hasFlood() && (sys.state == State::Idle || sys.state == State::Cycle || sys.state == State::Hold)) { - gc_state.modal.coolant.Flood = !gc_state.modal.coolant.Flood; - config->_coolant->set_state(gc_state.modal.coolant); - report_ovr_counter = 0; // Set to report change immediately - } - } - if (rtAccessoryOverride.bit.coolantMistOvrToggle) { - rtAccessoryOverride.bit.coolantMistOvrToggle = false; - - if (config->_coolant->hasMist() && (sys.state == State::Idle || sys.state == State::Cycle || sys.state == State::Hold)) { - gc_state.modal.coolant.Mist = !gc_state.modal.coolant.Mist; - config->_coolant->set_state(gc_state.modal.coolant); - report_ovr_counter = 0; // Set to report change immediately - } - } +static void update_velocities() { + report_ovr_counter = 0; // Set to report change immediately + plan_update_velocity_profile_parameters(); + plan_cycle_reinitialize(); } // This is the final phase of the shutdown activity that is initiated by mc_reset(). @@ -717,22 +674,12 @@ static void protocol_do_late_reset() { } } -void protocol_do_macro(int macro_num) { - // must be in Idle - if (sys.state != State::Idle) { - log_error("Macro button only permitted in idle"); - return; - } - - config->_macros->run_macro(macro_num); -} - void protocol_exec_rt_system() { protocol_do_alarm(); // If there is a hard or soft limit, this will block until rtReset is set if (rtReset) { if (sys.state == State::Homing) { - rtAlarm = ExecAlarm::HomingFailReset; + Machine::Homing::fail(ExecAlarm::HomingFailReset); } protocol_do_late_reset(); // Trigger system abort. @@ -740,60 +687,12 @@ void protocol_exec_rt_system() { return; // Nothing else to do but exit. } - if (rtStatusReport) { - rtStatusReport = false; - report_realtime_status(allChannels); - } - - if (rtMotionCancel) { - protocol_do_motion_cancel(); - } - - if (rtFeedHold) { - protocol_do_feedhold(); - } - if (rtSafetyDoor) { protocol_do_safety_door(); } - if (rtSleep) { - protocol_do_sleep(); - } - - if (rtCycleStart) { - protocol_do_cycle_start(); - } + protocol_handle_events(); - if (rtCycleStop) { - protocol_do_cycle_stop(); - } - - if (rtButtonMacro0) { - rtButtonMacro0 = false; - protocol_do_macro(0); - } - if (rtButtonMacro1) { - rtButtonMacro1 = false; - protocol_do_macro(1); - } - if (rtButtonMacro2) { - rtButtonMacro2 = false; - protocol_do_macro(2); - } - if (rtButtonMacro3) { - rtButtonMacro3 = false; - protocol_do_macro(3); - } - - protocol_execute_overrides(); - -#ifdef DEBUG_PROTOCOL - if (rtExecDebug) { - rtExecDebug = false; - report_realtime_debug(); - } -#endif // Reload step segment buffer switch (sys.state) { case State::ConfigAlarm: @@ -843,7 +742,7 @@ static void protocol_exec_rt_suspend() { restore_spindle_speed = block->spindle_speed; } if (spindle->isRateAdjusted()) { - rtAccessoryOverride.bit.spindleOvrStop = true; + protocol_send_event(&accessoryOverrideEvent, (void*)AccessoryOverride::SpindleStopOvr); } while (sys.suspend.value) { @@ -980,7 +879,7 @@ static void protocol_exec_rt_suspend() { } if (!sys.suspend.bit.restartRetract) { sys.suspend.bit.restoreComplete = true; - rtCycleStart = true; // Set to resume program. + protocol_send_event(&cycleStartEvent); // Resume program. } } } @@ -1011,7 +910,7 @@ static void protocol_exec_rt_suspend() { } } if (spindle_stop_ovr.bit.restoreCycle) { - rtCycleStart = true; // Set to resume program. + protocol_send_event(&cycleStartEvent); // Resume program. } spindle_stop_ovr.value = 0; // Clear stop override state } @@ -1029,3 +928,149 @@ static void protocol_exec_rt_suspend() { protocol_exec_rt_system(); } } + +static void protocol_do_feed_override(void* incrementvp) { + int increment = int(incrementvp); + int percent; + if (increment == FeedOverride::Default) { + percent = FeedOverride::Default; + } else { + percent = sys.f_override + increment; + if (percent > FeedOverride::Max) { + percent = FeedOverride::Max; + } else if (percent < FeedOverride::Min) { + percent = FeedOverride::Min; + } + } + if (percent != sys.f_override) { + sys.f_override = percent; + update_velocities(); + } +} + +static void protocol_do_rapid_override(void* percentvp) { + int percent = int(percentvp); + if (percent != sys.r_override) { + sys.r_override = percent; + update_velocities(); + } +} + +static void protocol_do_spindle_override(void* incrementvp) { + int percent; + int increment = int(incrementvp); + if (increment == SpindleSpeedOverride::Default) { + percent = SpindleSpeedOverride::Default; + } else { + percent = sys.spindle_speed_ovr + increment; + if (percent > SpindleSpeedOverride::Max) { + percent = SpindleSpeedOverride::Max; + } else if (percent < SpindleSpeedOverride::Min) { + percent = SpindleSpeedOverride::Min; + } + } + if (percent != sys.spindle_speed_ovr) { + sys.spindle_speed_ovr = percent; + sys.step_control.updateSpindleSpeed = true; + report_ovr_counter = 0; // Set to report change immediately + + // If spindle is on, tell it the RPM has been overridden + // When moving, the override is handled by the stepping code + if (gc_state.modal.spindle != SpindleState::Disable && !inMotionState()) { + spindle->setState(gc_state.modal.spindle, gc_state.spindle_speed); + report_ovr_counter = 0; // Set to report change immediately + } + } +} + +static void protocol_do_accessory_override(void* type) { + switch (int(type)) { + case AccessoryOverride::SpindleStopOvr: + // Spindle stop override allowed only while in HOLD state. + if (sys.state == State::Hold) { + if (spindle_stop_ovr.value == 0) { + spindle_stop_ovr.bit.initiate = true; + } else if (spindle_stop_ovr.bit.enabled) { + spindle_stop_ovr.bit.restore = true; + } + report_ovr_counter = 0; // Set to report change immediately + } + break; + case AccessoryOverride::FloodToggle: + // NOTE: Since coolant state always performs a planner sync whenever it changes, the current + // run state can be determined by checking the parser state. + if (config->_coolant->hasFlood() && (sys.state == State::Idle || sys.state == State::Cycle || sys.state == State::Hold)) { + gc_state.modal.coolant.Flood = !gc_state.modal.coolant.Flood; + config->_coolant->set_state(gc_state.modal.coolant); + report_ovr_counter = 0; // Set to report change immediately + } + break; + case AccessoryOverride::MistToggle: + if (config->_coolant->hasMist() && (sys.state == State::Idle || sys.state == State::Cycle || sys.state == State::Hold)) { + gc_state.modal.coolant.Mist = !gc_state.modal.coolant.Mist; + config->_coolant->set_state(gc_state.modal.coolant); + report_ovr_counter = 0; // Set to report change immediately + } + break; + default: + break; + } +} + +static void protocol_do_limit(void* arg) { + Machine::LimitPin* limit = (Machine::LimitPin*)arg; + limit->update(limit->get()); + if (sys.state == State::Homing) { + Machine::Homing::limitReached(); + return; + } + if (sys.state == State::Cycle) { + if (limit->isHard() && rtAlarm == ExecAlarm::None) { + log_debug("Hard limits"); + mc_reset(); // Initiate system kill. + rtAlarm = ExecAlarm::HardLimit; // Indicate hard limit critical event + } + return; + } + log_debug("Limit switch tripped for " << config->_axes->axisName(limit->_axis) << " motor " << limit->_motorNum); + Machine::EventPin::startTimer(); +} +ArgEvent feedOverrideEvent { protocol_do_feed_override }; +ArgEvent rapidOverrideEvent { protocol_do_rapid_override }; +ArgEvent spindleOverrideEvent { protocol_do_spindle_override }; +ArgEvent accessoryOverrideEvent { protocol_do_accessory_override }; +ArgEvent limitEvent { protocol_do_limit }; + +ArgEvent reportStatusEvent { (void (*)(void*))report_realtime_status }; + +NoArgEvent safetyDoorEvent { request_safety_door }; +NoArgEvent feedHoldEvent { protocol_do_feedhold }; +NoArgEvent cycleStartEvent { protocol_do_cycle_start }; +NoArgEvent cycleStopEvent { protocol_do_cycle_stop }; +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 }; + +// The problem is that report_realtime_status needs a channel argument +// Event statusReportEvent { protocol_do_status_report(XXX) }; + +xQueueHandle event_queue; + +void protocol_init() { + event_queue = xQueueCreate(10, sizeof(EventItem)); +} + +void protocol_send_event(Event* evt, void* arg) { + EventItem item { evt, arg }; + xQueueSend(event_queue, &item, 0); +} +void protocol_handle_events() { + EventItem item; + while (xQueueReceive(event_queue, &item, 0)) { + // log_debug("event"); + item.event->run(item.arg); + } +} diff --git a/FluidNC/src/Protocol.h b/FluidNC/src/Protocol.h index af4e93bd1..31489dd7e 100644 --- a/FluidNC/src/Protocol.h +++ b/FluidNC/src/Protocol.h @@ -7,6 +7,9 @@ #include "Types.h" +#include +#include + // Line buffer size from the serial input stream to be executed.Also, governs the size of // each of the startup blocks, as they are each stored as a string of this size. // @@ -20,6 +23,8 @@ const int LINE_BUFFER_SIZE = 256; void protocol_reset(); +void protocol_init(); + // Starts the main loop. It handles all incoming characters from the serial port and executes // them as they complete. It is also responsible for finishing the initialization procedures. void protocol_main_loop(); @@ -38,42 +43,10 @@ void protocol_buffer_synchronize(); void protocol_disable_steppers(); void protocol_cancel_disable_steppers(); -extern volatile bool rtStatusReport; -extern volatile bool rtCycleStart; -extern volatile bool rtFeedHold; extern volatile bool rtReset; -extern volatile bool rtSafetyDoor; -extern volatile bool rtMotionCancel; -extern volatile bool rtSleep; extern volatile bool rtCycleStop; -extern volatile bool rtButtonMacro0; -extern volatile bool rtButtonMacro1; -extern volatile bool rtButtonMacro2; -extern volatile bool rtButtonMacro3; - -#ifdef DEBUG_REPORT_REALTIME -extern volatile bool rtExecDebug; -#endif - -// Override bit maps. Realtime bitflags to control feed, rapid, spindle, and coolant overrides. -// Spindle/coolant and feed/rapids are separated into two controlling flag variables. - -struct AccessoryBits { - uint8_t spindleOvrStop : 1; - uint8_t coolantFloodOvrToggle : 1; - uint8_t coolantMistOvrToggle : 1; -}; - -union Accessory { - uint8_t value; - AccessoryBits bit; -}; -extern volatile Accessory rtAccessoryOverride; // Global realtime executor bitflag variable for spindle/coolant overrides. - -extern volatile Percent rtFOverride; // Feed override value in percent -extern volatile Percent rtROverride; // Rapid feed override value in percent -extern volatile Percent rtSOverride; // Spindle override value in percent +extern volatile bool runLimitLoop; // Alarm codes. enum class ExecAlarm : uint8_t { @@ -96,3 +69,48 @@ extern volatile ExecAlarm rtAlarm; // Global realtime executor variable for set #include extern std::map AlarmNames; + +#include "Event.h" +enum AccessoryOverride { + SpindleStopOvr = 1, + FloodToggle = 2, + MistToggle = 3, +}; + +extern ArgEvent feedOverrideEvent; +extern ArgEvent rapidOverrideEvent; +extern ArgEvent spindleOverrideEvent; +extern ArgEvent accessoryOverrideEvent; +extern ArgEvent limitEvent; + +extern ArgEvent reportStatusEvent; + +extern NoArgEvent safetyDoorEvent; +extern NoArgEvent feedHoldEvent; +extern NoArgEvent cycleStartEvent; +extern NoArgEvent cycleStopEvent; +extern NoArgEvent motionCancelEvent; +extern NoArgEvent sleepEvent; +extern NoArgEvent resetEvent; +extern NoArgEvent debugEvent; + +// extern NoArgEvent statusReportEvent; + +extern xQueueHandle event_queue; + +struct EventItem { + Event* event; + void* arg; +}; + +void protocol_send_event(Event*, void* arg = 0); +void protocol_handle_events(); + +inline void protocol_send_event(Event* evt, int arg) { + protocol_send_event(evt, (void*)arg); +} + +inline void protocol_send_event_from_ISR(Event* evt, void* arg = 0) { + EventItem item { evt, arg }; + xQueueSendFromISR(event_queue, &item, NULL); +} diff --git a/FluidNC/src/Report.cpp b/FluidNC/src/Report.cpp index c4cbbb5b9..e8b0f50e8 100644 --- a/FluidNC/src/Report.cpp +++ b/FluidNC/src/Report.cpp @@ -577,6 +577,9 @@ static void pinString(Print& channel) { } } +// Define this to do something if a debug request comes in over serial +void report_realtime_debug() {} + // Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram // and the actual location of the CNC machine. Users may change the following function to their // specific needs, but the desired real-time data report must be as short as possible. This is diff --git a/FluidNC/src/Report.h b/FluidNC/src/Report.h index 1e8adc46a..8d0c1a0af 100644 --- a/FluidNC/src/Report.h +++ b/FluidNC/src/Report.h @@ -91,9 +91,7 @@ void report_gcode_modes(Print& channel); // Prints build info and user info void report_build_info(const char* line, Print& channel); -#ifdef DEBUG_REPORT_REALTIME void report_realtime_debug(); -#endif void reportTaskStackSize(UBaseType_t& saved); diff --git a/FluidNC/src/Serial.cpp b/FluidNC/src/Serial.cpp index d4132d4c7..fe2bc91c6 100644 --- a/FluidNC/src/Serial.cpp +++ b/FluidNC/src/Serial.cpp @@ -46,7 +46,7 @@ #include "MotionControl.h" #include "Report.h" #include "System.h" -#include "Protocol.h" // rtSafetyDoor etc +#include "Protocol.h" // *Event #include "InputFile.h" #include "WebUI/InputBuffer.h" // XXX could this be a StringStream ? #include "Main.h" // display() @@ -85,116 +85,91 @@ void execute_realtime_command(Cmd command, Channel& channel) { switch (command) { case Cmd::Reset: log_debug("Cmd::Reset"); - mc_reset(); // Call motion control reset routine. + protocol_send_event(&resetEvent); break; case Cmd::StatusReport: report_realtime_status(channel); // direct call instead of setting flag + // protocol_send_event(&reportStatusEvent, int(&channel)); break; case Cmd::CycleStart: - rtCycleStart = true; + protocol_send_event(&cycleStartEvent); break; case Cmd::FeedHold: - rtFeedHold = true; + protocol_send_event(&feedHoldEvent); break; case Cmd::SafetyDoor: - rtSafetyDoor = true; + protocol_send_event(&safetyDoorEvent); break; case Cmd::JogCancel: if (sys.state == State::Jog) { // Block all other states from invoking motion cancel. - rtMotionCancel = true; + protocol_send_event(&motionCancelEvent); } break; case Cmd::DebugReport: -#ifdef DEBUG_REPORT_REALTIME - rtExecDebug = true; -#endif + protocol_send_event(&debugEvent); break; case Cmd::SpindleOvrStop: - rtAccessoryOverride.bit.spindleOvrStop = 1; + protocol_send_event(&accessoryOverrideEvent, AccessoryOverride::SpindleStopOvr); break; case Cmd::FeedOvrReset: - rtFOverride = FeedOverride::Default; + protocol_send_event(&feedOverrideEvent, FeedOverride::Default); break; case Cmd::FeedOvrCoarsePlus: - rtFOverride += FeedOverride::CoarseIncrement; - if (rtFOverride > FeedOverride::Max) { - rtFOverride = FeedOverride::Max; - } + protocol_send_event(&feedOverrideEvent, FeedOverride::CoarseIncrement); break; case Cmd::FeedOvrCoarseMinus: - rtFOverride -= FeedOverride::CoarseIncrement; - if (rtFOverride < FeedOverride::Min) { - rtFOverride = FeedOverride::Min; - } + protocol_send_event(&feedOverrideEvent, -FeedOverride::CoarseIncrement); break; case Cmd::FeedOvrFinePlus: - rtFOverride += FeedOverride::FineIncrement; - if (rtFOverride > FeedOverride::Max) { - rtFOverride = FeedOverride::Max; - } + protocol_send_event(&feedOverrideEvent, FeedOverride::FineIncrement); break; case Cmd::FeedOvrFineMinus: - rtFOverride -= FeedOverride::FineIncrement; - if (rtFOverride < FeedOverride::Min) { - rtFOverride = FeedOverride::Min; - } + protocol_send_event(&feedOverrideEvent, -FeedOverride::FineIncrement); break; case Cmd::RapidOvrReset: - rtROverride = RapidOverride::Default; + protocol_send_event(&rapidOverrideEvent, RapidOverride::Default); break; case Cmd::RapidOvrMedium: - rtROverride = RapidOverride::Medium; + protocol_send_event(&rapidOverrideEvent, RapidOverride::Medium); break; case Cmd::RapidOvrLow: - rtROverride = RapidOverride::Low; + protocol_send_event(&rapidOverrideEvent, RapidOverride::Low); break; case Cmd::RapidOvrExtraLow: - rtROverride = RapidOverride::ExtraLow; + protocol_send_event(&rapidOverrideEvent, RapidOverride::ExtraLow); break; case Cmd::SpindleOvrReset: - rtSOverride = SpindleSpeedOverride::Default; + protocol_send_event(&spindleOverrideEvent, SpindleSpeedOverride::Default); break; case Cmd::SpindleOvrCoarsePlus: - rtSOverride += SpindleSpeedOverride::CoarseIncrement; - if (rtSOverride > SpindleSpeedOverride::Max) { - rtSOverride = SpindleSpeedOverride::Max; - } + protocol_send_event(&spindleOverrideEvent, SpindleSpeedOverride::CoarseIncrement); break; case Cmd::SpindleOvrCoarseMinus: - rtSOverride -= SpindleSpeedOverride::CoarseIncrement; - if (rtSOverride < SpindleSpeedOverride::Min) { - rtSOverride = SpindleSpeedOverride::Min; - } + protocol_send_event(&spindleOverrideEvent, -SpindleSpeedOverride::CoarseIncrement); break; case Cmd::SpindleOvrFinePlus: - rtSOverride += SpindleSpeedOverride::FineIncrement; - if (rtSOverride > SpindleSpeedOverride::Max) { - rtSOverride = SpindleSpeedOverride::Max; - } + protocol_send_event(&spindleOverrideEvent, SpindleSpeedOverride::FineIncrement); break; case Cmd::SpindleOvrFineMinus: - rtSOverride -= SpindleSpeedOverride::FineIncrement; - if (rtSOverride < SpindleSpeedOverride::Min) { - rtSOverride = SpindleSpeedOverride::Min; - } + protocol_send_event(&spindleOverrideEvent, -SpindleSpeedOverride::FineIncrement); break; case Cmd::CoolantFloodOvrToggle: - rtAccessoryOverride.bit.coolantFloodOvrToggle = 1; + protocol_send_event(&accessoryOverrideEvent, AccessoryOverride::FloodToggle); break; case Cmd::CoolantMistOvrToggle: - rtAccessoryOverride.bit.coolantMistOvrToggle = 1; + protocol_send_event(&accessoryOverrideEvent, AccessoryOverride::MistToggle); break; case Cmd::Macro0: - rtButtonMacro0 = true; + protocol_send_event(¯o0Event); break; case Cmd::Macro1: - rtButtonMacro1 = true; + protocol_send_event(¯o1Event); break; case Cmd::Macro2: - rtButtonMacro2 = true; + protocol_send_event(¯o2Event); break; case Cmd::Macro3: - rtButtonMacro3 = true; + protocol_send_event(¯o3Event); break; } } diff --git a/FluidNC/src/Stepper.cpp b/FluidNC/src/Stepper.cpp index 86b3e7a1e..268a29a6c 100644 --- a/FluidNC/src/Stepper.cpp +++ b/FluidNC/src/Stepper.cpp @@ -241,8 +241,9 @@ bool IRAM_ATTR Stepper::pulse_func() { spindle->setSpeedfromISR(0); } } - rtCycleStop = true; - awake = false; + + protocol_send_event_from_ISR(&cycleStopEvent); + awake = false; return false; // Nothing to do but exit. } } @@ -255,7 +256,7 @@ bool IRAM_ATTR Stepper::pulse_func() { auto m = axes->_axis[axis]->_motors[0]; probe_steps[axis] = m ? m->_steps : 0; } - rtMotionCancel = true; + protocol_send_event_from_ISR(&motionCancelEvent); } // Reset step out bits. diff --git a/FluidNC/src/System.cpp b/FluidNC/src/System.cpp index a610dcc99..3326e5061 100644 --- a/FluidNC/src/System.cpp +++ b/FluidNC/src/System.cpp @@ -58,6 +58,15 @@ void set_motor_steps(size_t axis, int32_t steps) { } } +void set_motor_steps_from_mpos(float* mpos) { + auto n_axis = config->_axes->_numberAxis; + float motor_steps[n_axis]; + config->_kinematics->transform_cartesian_to_motors(motor_steps, mpos); + for (size_t axis = 0; axis < n_axis; axis++) { + set_motor_steps(axis, mpos_to_steps(motor_steps[axis], axis)); + } +} + int32_t get_axis_motor_steps(size_t axis) { auto m = config->_axes->_axis[axis]->_motors[0]; return m ? m->_steps : 0; diff --git a/FluidNC/src/System.h b/FluidNC/src/System.h index 6cbc4090a..6a8fbb1aa 100644 --- a/FluidNC/src/System.h +++ b/FluidNC/src/System.h @@ -83,6 +83,7 @@ int32_t mpos_to_steps(float mpos, size_t axis); int32_t get_axis_motor_steps(size_t axis); void set_motor_steps(size_t axis, int32_t steps); +void set_motor_steps_from_mpos(float* mpos); int32_t* get_motor_steps(); // Updates a machine position array from a steps array diff --git a/FluidNC/src/WebUI/WebSettings.cpp b/FluidNC/src/WebUI/WebSettings.cpp index d2223593b..f081c84b9 100644 --- a/FluidNC/src/WebUI/WebSettings.cpp +++ b/FluidNC/src/WebUI/WebSettings.cpp @@ -224,7 +224,7 @@ namespace WebUI { } static Error listSettings(char* parameter, AuthenticationLevel auth_level, Channel& out) { // ESP400 - JSONencoder j(false, out); + JSONencoder j(true, out); j.begin(); j.begin_array("EEPROM"); diff --git a/FluidNC/test/Pins/ErrorPinTest.cpp b/FluidNC/test/Pins/ErrorPinTest.cpp index c20bb8232..d673296f6 100644 --- a/FluidNC/test/Pins/ErrorPinTest.cpp +++ b/FluidNC/test/Pins/ErrorPinTest.cpp @@ -1,7 +1,6 @@ #include "../TestFramework.h" #include -#include // CHANGE namespace Pins { Test(Error, Pins) { @@ -17,7 +16,7 @@ namespace Pins { AssertThrow(errorPin.write(true)); AssertThrow(errorPin.read()); - AssertThrow(errorPin.attachInterrupt([](void* arg) {}, CHANGE)); + AssertThrow(errorPin.attachInterrupt([](void* arg) {}, EITHER_EDGE)); AssertThrow(errorPin.detachInterrupt()); Assert(errorPin.capabilities() == Pin::Capabilities::Error, "Incorrect caps"); diff --git a/FluidNC/test/Pins/Undefined.cpp b/FluidNC/test/Pins/Undefined.cpp index 388c46407..8d68c27a5 100644 --- a/FluidNC/test/Pins/Undefined.cpp +++ b/FluidNC/test/Pins/Undefined.cpp @@ -1,7 +1,6 @@ #include "../TestFramework.h" #include -#include // CHANGE namespace Pins { Test(Undefined, Pins) { @@ -22,7 +21,7 @@ namespace Pins { Assert(0 == result, "Result value incorrect"); } - AssertThrow(unassigned.attachInterrupt([](void* arg) {}, CHANGE)); + AssertThrow(unassigned.attachInterrupt([](void* arg) {}, EITHER_EDGE)); AssertThrow(unassigned.detachInterrupt()); Assert(unassigned.capabilities().has(Pin::Capabilities::Void)); From 3cb7a1de3d89065dd935149c63341e1b3bef0f51 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Mon, 5 Sep 2022 18:49:11 -1000 Subject: [PATCH 5/8] Rebase of #575 onto Devt, Thanks to grittershub (#595) --- FluidNC/src/Motors/TMC2208Driver.cpp | 12 +++++++++++- FluidNC/src/Motors/TMC2209Driver.cpp | 22 +++++++++++++++------- FluidNC/src/Motors/TrinamicBase.cpp | 2 ++ FluidNC/src/Motors/TrinamicBase.h | 1 + 4 files changed, 29 insertions(+), 8 deletions(-) diff --git a/FluidNC/src/Motors/TMC2208Driver.cpp b/FluidNC/src/Motors/TMC2208Driver.cpp index 97d2a5db7..72c5d402f 100644 --- a/FluidNC/src/Motors/TMC2208Driver.cpp +++ b/FluidNC/src/Motors/TMC2208Driver.cpp @@ -64,7 +64,17 @@ namespace MotorDrivers { } } - bool TMC2208Driver::test() { return TrinamicBase::reportTest(tmc2208->test_connection()); } + bool TMC2208Driver::test() { + uint8_t ifcnt_before = tmc2208->IFCNT(); + tmc2208->GSTAT(0); // clear GSTAT to increase ifcnt + uint8_t ifcnt_after = tmc2208->IFCNT(); + bool okay = ((ifcnt_before + 1) & 0xff) == ifcnt_after; + if (!okay) { + TrinamicBase::reportCommsFailure(); + return false; + } + return true; + } // Configuration registration namespace { diff --git a/FluidNC/src/Motors/TMC2209Driver.cpp b/FluidNC/src/Motors/TMC2209Driver.cpp index 92e1207cd..d5be4657a 100644 --- a/FluidNC/src/Motors/TMC2209Driver.cpp +++ b/FluidNC/src/Motors/TMC2209Driver.cpp @@ -48,11 +48,11 @@ namespace MotorDrivers { // The TMCStepper library uses the value 0 to mean 1x microstepping int usteps = _microsteps == 1 ? 0 : _microsteps; tmc2209->microsteps(usteps); - tmc2209->pdn_disable(true); // powerdown pin is disabled. uses ihold. + tmc2209->pdn_disable(true); // powerdown pin is disabled. uses ihold. switch (_mode) { case TrinamicMode ::StealthChop: - log_debug(axisName() << " StealthChop"); + log_debug(axisName() << " StealthChop"); tmc2209->en_spreadCycle(false); tmc2209->pwm_autoscale(true); break; @@ -65,7 +65,7 @@ namespace MotorDrivers { { auto axisConfig = config->_axes->_axis[this->axis_index()]; auto homingFeedRate = (axisConfig->_homing != nullptr) ? axisConfig->_homing->_feedRate : 200; - log_debug(axisName() << " Stallguard"); + log_debug(axisName() << " Stallguard"); tmc2209->en_spreadCycle(false); tmc2209->pwm_autoscale(false); tmc2209->TCOOLTHRS(calc_tstep(homingFeedRate, 150.0)); @@ -87,10 +87,8 @@ namespace MotorDrivers { } float feedrate = Stepper::get_realtime_rate(); //* settings.microsteps[axis_index] / 60.0 ; // convert mm/min to Hz - // TMC2208 does not have StallGuard if (tmc2209) { - log_info(axisName() << " SG_Val: " << tmc2209->SG_RESULT() << " Rate: " << feedrate - << " mm/min SG_Setting:" << _stallguard); + log_info(axisName() << " SG_Val: " << tmc2209->SG_RESULT() << " Rate: " << feedrate << " mm/min SG_Setting:" << _stallguard); } } @@ -102,7 +100,17 @@ namespace MotorDrivers { } } - bool TMC2209Driver::test() { return TrinamicBase::reportTest(tmc2209->test_connection()); } + bool TMC2209Driver::test() { + uint8_t ifcnt_before = tmc2209->IFCNT(); + tmc2209->GSTAT(0); // clear GSTAT to increase ifcnt + uint8_t ifcnt_after = tmc2209->IFCNT(); + bool okay = ((ifcnt_before + 1) & 0xff) == ifcnt_after; + if (!okay) { + TrinamicBase::reportCommsFailure(); + return false; + } + return true; + } // Configuration registration namespace { diff --git a/FluidNC/src/Motors/TrinamicBase.cpp b/FluidNC/src/Motors/TrinamicBase.cpp index a404063a8..9edb05e8f 100644 --- a/FluidNC/src/Motors/TrinamicBase.cpp +++ b/FluidNC/src/Motors/TrinamicBase.cpp @@ -120,6 +120,8 @@ namespace MotorDrivers { } } + void TrinamicBase::reportCommsFailure(void) { log_info(axisName() << " communications check failed"); } + bool TrinamicBase::startDisable(bool disable) { if (_has_errors) { return false; diff --git a/FluidNC/src/Motors/TrinamicBase.h b/FluidNC/src/Motors/TrinamicBase.h index 77c5a1c00..7db414110 100644 --- a/FluidNC/src/Motors/TrinamicBase.h +++ b/FluidNC/src/Motors/TrinamicBase.h @@ -60,6 +60,7 @@ namespace MotorDrivers { bool set_homing_mode(bool isHoming); virtual void set_registers(bool isHoming) {} bool reportTest(uint8_t result); + void reportCommsFailure(void); bool startDisable(bool disable); virtual void config_motor(); From b409975720876e24e8adf1a40ef0cc1d16373ee5 Mon Sep 17 00:00:00 2001 From: redbeard Date: Mon, 5 Sep 2022 21:52:43 -0700 Subject: [PATCH 6/8] bug: Resolve broken $I command on serial console (#551) * bug: Resolve broken $I command on serial console Presently the `$I` command fails to reset the contents of the variable `result` due to a static definition in the funciton `WiFiConfig::info()`. Calling the function repeatedly causes repetitions in the output: ``` $I [VER:3.5 FluidNC v3.5.0:] [OPT:MPHS] [MSG: Machine: CNC_xPRO_V5] [MSG: Mode=STA:SSID=2e:Status=Connected:IP=192.168.132.163:MAC=98-CD-AC-B4-10-78] ok $I [VER:3.5 FluidNC v3.5.0:] [OPT:MPHS] [MSG: Machine: CNC_xPRO_V5] [MSG: Mode=STA:SSID=2e:Status=Connected:IP=192.168.132.163:MAC=98-CD-AC-B4-10-78Mode=STA:SSID=2e:Status=Connected:IP=192.168.132.163:MAC=98-CD-AC-B4-10-78] ok $I [VER:3.5 FluidNC v3.5.0:] [OPT:MPHS] [MSG: Machine: CNC_xPRO_V5] [MSG: Mode=STA:SSID=2e:Status=Connected:IP=192.168.132.163:MAC=98-CD-AC-B4-10-78Mode=STA:SSID=2e:Status=Connected:IP=192.168.132.163:MAC=98-CD-AC-B4-10-78Mode=STA:SSID=2e:Status=Connected:IP=192.168.132.163:MAC=98-CD-AC-B4-10-78] ``` Removing `static` will change the scope of `result` to a single instance and not share the value across all instances. * format: Run `clang-format` Running `clang-format` to ensure proper formatting. In this case it seems `clang-format` may not have been run on this file previously. As the option `AllowShortFunctionsOnASingleLine` is set to the value `Inline` and we are not assigning these values as a part of the class definition, it is expanding the function definitions to a multi-line syntax. --- FluidNC/src/WebUI/WifiConfig.cpp | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/FluidNC/src/WebUI/WifiConfig.cpp b/FluidNC/src/WebUI/WifiConfig.cpp index 2610b5550..3d8a740f5 100644 --- a/FluidNC/src/WebUI/WifiConfig.cpp +++ b/FluidNC/src/WebUI/WifiConfig.cpp @@ -63,7 +63,9 @@ namespace WebUI { { "Static", STATIC_MODE }, }; - static void print_mac(const char* s, String mac, Channel& out) { out << s << " (" << mac << ")\n"; } + static void print_mac(const char* s, String mac, Channel& out) { + out << s << " (" << mac << ")\n"; + } static Error showIP(char* parameter, AuthenticationLevel auth_level, Channel& out) { // ESP111 out << parameter << (WiFi.getMode() == WIFI_STA ? WiFi.localIP() : WiFi.softAPIP()).toString() << '\n'; @@ -322,8 +324,8 @@ namespace WebUI { } String WiFiConfig::info() { - static String result; - String tmp; + String result; + String tmp; if ((WiFi.getMode() == WIFI_MODE_STA) || (WiFi.getMode() == WIFI_MODE_APSTA)) { result += "Mode=STA:SSID="; @@ -723,7 +725,9 @@ namespace WebUI { /** * End WiFi */ - void WiFiConfig::end() { StopWiFi(); } + void WiFiConfig::end() { + StopWiFi(); + } /** * Reset ESP @@ -743,12 +747,16 @@ namespace WebUI { } log_info("WiFi reset done"); } - bool WiFiConfig::isOn() { return !(WiFi.getMode() == WIFI_MODE_NULL); } + bool WiFiConfig::isOn() { + return !(WiFi.getMode() == WIFI_MODE_NULL); + } /** * Handle not critical actions that must be done in sync environment */ - void WiFiConfig::handle() { wifi_services.handle(); } + void WiFiConfig::handle() { + wifi_services.handle(); + } Error WiFiConfig::listAPs(char* parameter, AuthenticationLevel auth_level, Channel& out) { // ESP410 JSONencoder j(false, out); @@ -787,6 +795,8 @@ namespace WebUI { return Error::Ok; } - WiFiConfig::~WiFiConfig() { end(); } + WiFiConfig::~WiFiConfig() { + end(); + } } #endif From 2504ed78dd9b5a65592309b2a2e22d601ae62b0e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lukas=20Go=C3=9Fmann?= <98282939+LukasGossmann@users.noreply.github.com> Date: Tue, 6 Sep 2022 06:53:35 +0200 Subject: [PATCH 7/8] Fixed mist cooling always being reported as active when mist pin is defined (#538) --- FluidNC/src/Report.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/FluidNC/src/Report.cpp b/FluidNC/src/Report.cpp index e8b0f50e8..2519d0508 100644 --- a/FluidNC/src/Report.cpp +++ b/FluidNC/src/Report.cpp @@ -433,7 +433,7 @@ void report_build_info(const char* line, Print& channel) { channel << "[VER:" << grbl_version << " FluidNC " << git_info << ":" << line << "]\n"; channel << "[OPT:"; if (config->_coolant->hasMist()) { - channel << "M"; // TODO Need to deal with M8...it could be disabled + channel << "M"; } channel << "PH"; if (ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES) { @@ -679,11 +679,10 @@ void report_realtime_status(Channel& channel) { } auto coolant = coolant_state; - // XXX WMB why .Flood in one case and ->hasMist() in the other? also see above if (coolant.Flood) { channel << "F"; } - if (config->_coolant->hasMist()) { + if (coolant.Mist) { channel << "M"; } } From 491e66f187d18ff7726c6441bc2af5c5131585ea Mon Sep 17 00:00:00 2001 From: forbin1ab <82760649+forbin1ab@users.noreply.github.com> Date: Mon, 5 Sep 2022 23:57:46 -0500 Subject: [PATCH 8/8] Wallplotter feedrate logic (#576) * Segmentation bug. Added cartesian feedrate logic. * Wallplotter BESC servo speedmap convert to 32bit Co-authored-by: Mitch Bradley --- FluidNC/src/Kinematics/WallPlotter.cpp | 41 ++++++++++++++++---------- FluidNC/src/Spindles/BESCSpindle.cpp | 19 ++++++------ FluidNC/src/Spindles/BESCSpindle.h | 4 +-- FluidNC/src/Spindles/PWMSpindle.h | 2 +- example_configs/WallPlotter.yaml | 4 +-- 5 files changed, 40 insertions(+), 30 deletions(-) diff --git a/FluidNC/src/Kinematics/WallPlotter.cpp b/FluidNC/src/Kinematics/WallPlotter.cpp index 38440ed15..d6d50ab27 100644 --- a/FluidNC/src/Kinematics/WallPlotter.cpp +++ b/FluidNC/src/Kinematics/WallPlotter.cpp @@ -49,7 +49,7 @@ namespace Kinematics { // calculate the total X,Y axis move distance // Z axis is the same in both coord systems, so it does not undergo conversion - float dist = vector_distance(target, position, Y_AXIS); + float dist = vector_distance(target, position, 2); // Only compute distance for both axes. X and Y // Segment our G1 and G0 moves based on yaml file. If we choose a small enough _segment_length we can hide the nonlinearity segment_count = dist / _segment_length; if (segment_count < 1) { // Make sure there is at least one segment, even if there is no movement @@ -57,23 +57,24 @@ namespace Kinematics { // the planner even if there is no movement?? segment_count = 1; } - // Calc distance of individual segments - dx = (target[X_AXIS] - position[X_AXIS]) / segment_count; - dy = (target[Y_AXIS] - position[Y_AXIS]) / segment_count; - dz = (target[Z_AXIS] - position[Z_AXIS]) / segment_count; - // Current cartesian end point of the segment - float seg_x = position[X_AXIS]; + // Calc distance of individual cartesian segments + dx = (target[X_AXIS] - position[X_AXIS])/segment_count; + dy = (target[Y_AXIS] - position[Y_AXIS])/segment_count; + dz = (target[Z_AXIS] - position[Z_AXIS])/segment_count; + // Current cartesian end point of the segment + float seg_x = position[X_AXIS]; float seg_y = position[Y_AXIS]; float seg_z = position[Z_AXIS]; + // Calculate desired cartesian feedrate distance ratio. Same for each seg. + float vdcart_ratio = pl_data->feed_rate/(dist/segment_count); for (uint32_t segment = 1; segment <= segment_count; segment++) { // calc next cartesian end point of the next segment seg_x += dx; seg_y += dy; seg_z += dz; - float seg_left, seg_right; + // Convert cartesian space coords to motor space + float seg_left, seg_right; xy_to_lengths(seg_x, seg_y, seg_left, seg_right); - // TODO: Need to adjust cartesian feedrate to motor/plotter space, just leave them alone for now - #ifdef USE_CHECKED_KINEMATICS // Check the inverse computation. float cx, cy; @@ -84,18 +85,26 @@ namespace Kinematics { // FIX: Produce an alarm state? } #endif // end USE_CHECKED_KINEMATICS - - // mc_move_motors() returns false if a jog is cancelled. - // In that case we stop sending segments to the planner. - + // Set interpolated feedrate in motor space for this segment to + // get desired feedrate in cartesian space + if (!pl_data->motion.rapidMotion) { // Rapid motions ignore feedrate. Don't convert. + // T=D/V, Tcart=Tmotor, Dcart/Vcart=Dmotor/Vmotor + // Vmotor = Dmotor*(Vcart/Dcart) + pl_data->feed_rate = hypot_f(last_left-seg_left,last_right-seg_right)*vdcart_ratio; + } + // TODO: G93 pl_data->motion.inverseTime logic?? Does this even make sense for wallplotter? + // Remember absolute motor lengths for next time last_left = seg_left; last_right = seg_right; last_z = seg_z; - + // Initiate motor movement with converted feedrate and converted position + // mc_move_motors() returns false if a jog is cancelled. + // In that case we stop sending segments to the planner. // Note that the left motor runs backward. // TODO: It might be better to adjust motor direction in .yaml file by inverting direction pin?? - float cables[MAX_N_AXIS] = { 0 - (last_left - zero_left), 0 + (last_right - zero_right), seg_z }; + float cables[MAX_N_AXIS] = { 0 - (seg_left - zero_left), 0 + (seg_right - zero_right), seg_z }; if (!mc_move_motors(cables, pl_data)) { + // TODO fixup last_left last_right?? What is position state when jog is cancelled? return false; } } diff --git a/FluidNC/src/Spindles/BESCSpindle.cpp b/FluidNC/src/Spindles/BESCSpindle.cpp index 9835be241..417ab621b 100644 --- a/FluidNC/src/Spindles/BESCSpindle.cpp +++ b/FluidNC/src/Spindles/BESCSpindle.cpp @@ -52,11 +52,9 @@ namespace Spindles { shelfSpeeds(4000, 20000); } - // We set the dev_speed scale in the speed map to the full PWM period. - // Then, in set_output, we map the dev_speed range of 0..64K to the pulse - // length range of ~1ms .. 2ms - setupSpeeds(_pwm->period()); - + // Use yaml speed_map to setup speed map for "spindle speed" conversion to timer counts used by PWM controller + //setupSpeeds(_pulse_span_counts); // Map the counts for just the part of the pulse that changes to keep math inside 32bits later... + setupSpeeds(_pwm->period()); // Map the entire pulse width period in counts stop(); config_message(); } @@ -73,19 +71,22 @@ namespace Spindles { _current_pwm_duty = duty; - // This maps the dev_speed range of 0.._pwm->period() into the pulse length + // This maps the dev_speed range of 0..(1<<_pwm_precision) into the pulse length // where _min_pulse_counts represents off and (_min_pulse_counts + _pulse_span_counts) // represents full on. Typically the off value is a 1ms pulse length and the // full on value is a 2ms pulse. - uint32_t pulse_counts = _min_pulse_counts + ((duty * _pulse_span_counts) / _pwm->period()); + // uint32_t pulse_counts = _min_pulse_counts + (_pulse_span_counts * (uint64_t) duty)/_pwm->period(); + _pwm->setDuty(_min_pulse_counts + (_pulse_span_counts * (uint64_t) duty)/_pwm->period()); + // _pwm->setDuty(_min_pulse_counts+duty); // More efficient by keeping math within 32bits?? + // log_info(name() << " duty:" << duty << " _min_pulse_counts:" << _min_pulse_counts + // << " _pulse_span_counts:" << _pulse_span_counts << " pulse_counts" << pulse_counts); - _pwm->setDuty(pulse_counts); } // prints the startup message of the spindle config void BESC::config_message() { log_info(name() << " Spindle Out:" << _output_pin.name() << " Min:" << _min_pulse_us << "us Max:" << _max_pulse_us - << "us Freq:" << _pwm->frequency() << "Hz Period:" << _pwm->period()); + << "us Freq:" << _pwm->frequency() << "Hz Full Period count:" << _pwm->period()); } // Configuration registration diff --git a/FluidNC/src/Spindles/BESCSpindle.h b/FluidNC/src/Spindles/BESCSpindle.h index 9bfec6a38..912635738 100644 --- a/FluidNC/src/Spindles/BESCSpindle.h +++ b/FluidNC/src/Spindles/BESCSpindle.h @@ -34,8 +34,8 @@ namespace Spindles { const uint32_t besc_pwm_max_freq = 2000; // 50 Hz // Calculated - uint16_t _pulse_span_counts; // In counts of a 16-bit counter - uint16_t _min_pulse_counts; // In counts of a 16-bit counter + uint32_t _pulse_span_counts; // In counts of a 32-bit counter. ESP32 uses up to 20bits + uint32_t _min_pulse_counts; // In counts of a 32-bit counter ESP32 uses up to 20bits protected: // Configurable diff --git a/FluidNC/src/Spindles/PWMSpindle.h b/FluidNC/src/Spindles/PWMSpindle.h index 273590759..7e0f3385c 100644 --- a/FluidNC/src/Spindles/PWMSpindle.h +++ b/FluidNC/src/Spindles/PWMSpindle.h @@ -59,7 +59,7 @@ namespace Spindles { virtual ~PWM() {} protected: - int32_t _current_pwm_duty = 0; + uint32_t _current_pwm_duty = 0; PwmPin* _pwm = nullptr; // Configurable diff --git a/example_configs/WallPlotter.yaml b/example_configs/WallPlotter.yaml index 777c2f456..82452986c 100644 --- a/example_configs/WallPlotter.yaml +++ b/example_configs/WallPlotter.yaml @@ -104,7 +104,7 @@ besc: spindown_ms: 2000 tool_num: 100 speed_map: 0=0.000% 255=100.000% - min_pulse_us: 600 - max_pulse_us: 2300 + min_pulse_us: 700 + max_pulse_us: 2200