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