WIP - Fixed POG Style single switch squaring

Grbl_Esp32/src/Machine/Axes.cpp

@@ -28,7 +28,7 @@ namespace Machine {
             _sharedStepperDisable.report("Shared stepper disable");
         }
 
-        release_all_motors();
+        unlock_all_motors();
 
         // certain motors need features to be turned on. Check them here
         for (uint8_t axis = X_AXIS; axis < _numberAxis; axis++) {
@@ -83,7 +83,7 @@ namespace Machine {
     // Put the motors in the given axes into homing mode, returning a
     // mask of which motors (considering gangs) can do homing.
     MotorMask Axes::set_homing_mode(AxisMask axisMask, bool isHoming) {
-        release_all_motors();  // On homing transitions, cancel all motor lockouts
+        unlock_all_motors();  // On homing transitions, cancel all motor lockouts
         MotorMask motorsCanHome = 0;
 
         for (uint8_t axis = X_AXIS; axis < _numberAxis; axis++) {
@@ -102,8 +102,10 @@ namespace Machine {
         return motorsCanHome;
     }
 
-    void Axes::release_all_motors() { _motorLockoutMask = 0xffffffff; }
+    void Axes::unlock_all_motors() { _motorLockoutMask = 0; }
-    void Axes::stop_motors(MotorMask mask) { clear_bits(_motorLockoutMask, mask); }
+    void Axes::lock_motors(MotorMask mask) { set_bits(_motorLockoutMask, mask); }
+    void Axes::unlock_motors(MotorMask mask) {clear_bits(_motorLockoutMask, mask); }
+        ;
 
         void IRAM_ATTR Axes::step(uint8_t step_mask, uint8_t dir_mask) {
             auto n_axis = _numberAxis;
@@ -136,10 +138,10 @@ namespace Machine {
                 if (bitnum_is_true(step_mask, axis)) {
                     auto a = _axis[axis];
 
-                if (bitnum_is_true(_motorLockoutMask, axis)) {
+                    if (bitnum_is_false(_motorLockoutMask, axis)) {
                         a->_gangs[0]->_motor->step();
                     }
-                if (bitnum_is_true(_motorLockoutMask, axis + 16)) {
+                    if (bitnum_is_false(_motorLockoutMask, axis + 16)) {
                         a->_gangs[1]->_motor->step();
                     }
                 }

Grbl_Esp32/src/Machine/Axes.h

@@ -33,7 +33,7 @@ namespace Machine {
 
         // During homing, this is used to stop stepping on motors that have
         // reached their limit switches, by clearing bits in the mask.
-        MotorMask _motorLockoutMask = 0xffffffff;
+        MotorMask _motorLockoutMask = 0;
 
     public:
         Axes();
@@ -90,8 +90,9 @@ namespace Machine {
         // These are used during homing cycles.
         // The return value is a bitmask of axes that can home
         MotorMask set_homing_mode(AxisMask homing_mask, bool isHoming);
-        void      release_all_motors();
+        void      unlock_all_motors();     
-        void      stop_motors(MotorMask motor_mask);
+        void      lock_motors(MotorMask motor_mask);
+        void      unlock_motors(MotorMask motor_mask);
 
         void set_disable(int axis, bool disable);
         void set_disable(bool disable);

Grbl_Esp32/src/Machine/Homing.cpp

@@ -144,7 +144,8 @@ namespace Machine {
 
         uint32_t settling_ms = plan_move(remainingMotors, approach, seek);
 
-        config->_axes->release_all_motors();
+        config->_axes->lock_motors(0xffffffff);
+        config->_axes->unlock_motors(remainingMotors);
 
         do {
             if (approach) {
@@ -152,7 +153,7 @@ namespace Machine {
                 // XXX do we check only the switch in the direction of motion?
                 MotorMask limitedMotors = Machine::Axes::posLimitMask | Machine::Axes::negLimitMask;
 
-                config->_axes->stop_motors(limitedMotors);
+                config->_axes->lock_motors(limitedMotors);
                 clear_bits(remainingMotors, limitedMotors);
             }
 
@@ -198,9 +199,9 @@ namespace Machine {
             // No axis has multiple motors
             return false;
         }
+
         auto axes   = config->_axes;
         auto n_axis = axes->_numberAxis;
-
         for (int axis = 0; axis < n_axis; axis++) {
             if (bitnum_is_false(squaredAxes, axis)) {
                 continue;
@@ -211,6 +212,7 @@ namespace Machine {
                 // Shared endstop on squared axis
                 return true;
             }
+            // check to see if at least one side is missing a switch
             endstop = axisConfig->_gangs[0]->_endstops;
             if (!endstop) {
                 // Missing endstop on gang 0
@@ -277,7 +279,6 @@ namespace Machine {
             if (squaredOneSwitch(motors)) {
                 run(motors & GANG0, true, false);   // Approach slowly
                 run(motors & GANG0, false, false);  // Pulloff
-
                 run(motors & GANG1, true, false);   // Approach slowly
                 run(motors & GANG1, false, false);  // Pulloff
             } else {