Merge pull request #981 from bdring/NewAlarms

New alarms

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(&macro0Event, "macro0_pin", '0'));

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 = {};

FluidNC/src/Jog.cpp

@@ -12,15 +12,15 @@
 // 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;
     pl_data->motion.noFeedOverride = 1;
     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;
     }

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);

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;

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);

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;