From 25a61e9061dd4e47cb9a728c5e2f278029c83c2d Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sat, 15 Apr 2017 04:23:20 -0500
Subject: [PATCH] Improve motion-based endstop triggering code

---
 Marlin/endstops.cpp | 254 +++++++++++++++++++++-----------------------
 1 file changed, 123 insertions(+), 131 deletions(-)

diff --git a/Marlin/endstops.cpp b/Marlin/endstops.cpp
index d5b152502..8cb04979c 100644
--- a/Marlin/endstops.cpp
+++ b/Marlin/endstops.cpp
@@ -278,12 +278,16 @@ void Endstops::update() {
     }
   #endif
 
-    #if ENABLED(COREXY) || ENABLED(COREXZ)
-      #define CORE_X_CMP ==
-    #elif ENABLED(COREYX) || ENABLED(COREZX)
-      #define CORE_X_CMP !=
-    #endif
+  /**
+   * Define conditions for checking endstops
+   */
+
+  #if IS_CORE
+    #define S_(N) stepper.current_block->steps[CORE_AXIS_##N]
+    #define D_(N) stepper.motor_direction(CORE_AXIS_##N)
+  #endif
 
+  #if CORE_IS_XY || CORE_IS_XZ
     /**
      * Head direction in -X axis for CoreXY and CoreXZ bots.
      *
@@ -291,52 +295,16 @@ void Endstops::update() {
      * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z, handled below)
      * If DeltaA ==  DeltaB, the movement is only in the 1st axis (X)
      */
-    #if CORE_IS_XY || CORE_IS_XZ
-      if (stepper.current_block->steps[CORE_AXIS_1] != stepper.current_block->steps[CORE_AXIS_2]
-        || (    stepper.current_block->steps[CORE_AXIS_1] > 0
-             && stepper.motor_direction(CORE_AXIS_1) CORE_X_CMP stepper.motor_direction(CORE_AXIS_2)
-           )
-      ) {
-        if (stepper.motor_direction(X_HEAD))
+    #if ENABLED(COREXY) || ENABLED(COREXZ)
+      #define X_CMP ==
     #else
-      if (stepper.current_block->steps[X_AXIS] > 0)
-        if (stepper.motor_direction(X_AXIS))   // stepping along -X axis (regular Cartesian bot)
+      #define X_CMP !=
     #endif
-      { // -direction
-        #if ENABLED(DUAL_X_CARRIAGE)
-          // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
-          if ( (stepper.current_block->active_extruder == 0 && X_HOME_DIR < 0)
-            || (stepper.current_block->active_extruder != 0 && X2_HOME_DIR < 0)
-          )
-        #endif
-          {
-            #if HAS_X_MIN
-              UPDATE_ENDSTOP(X, MIN);
-            #endif
-          }
-      }
-      else { // +direction
-        #if ENABLED(DUAL_X_CARRIAGE)
-          // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
-          if ( (stepper.current_block->active_extruder == 0 && X_HOME_DIR > 0)
-            || (stepper.current_block->active_extruder != 0 && X2_HOME_DIR > 0)
-          )
-        #endif
-          {
-            #if HAS_X_MAX
-              UPDATE_ENDSTOP(X, MAX);
-            #endif
-          }
-      }
-  #if CORE_IS_XY || CORE_IS_XZ
-    }
-  #endif
-
-  // Handle swapped vs. typical Core axis order
-  #if ENABLED(COREYX) || ENABLED(COREYZ)
-    #define CORE_YZ_CMP ==
-  #elif ENABLED(COREXY) || ENABLED(COREZY)
-    #define CORE_YZ_CMP !=
+    #define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) X_CMP D_(2)) )
+    #define X_AXIS_HEAD X_HEAD
+  #else
+    #define X_MOVE_TEST stepper.current_block->steps[X_AXIS] > 0
+    #define X_AXIS_HEAD X_AXIS
   #endif
 
   #if CORE_IS_XY || CORE_IS_YZ
@@ -347,35 +315,16 @@ void Endstops::update() {
      * If DeltaA ==  DeltaB, the movement is only in the 1st axis (X or Y)
      * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z)
      */
-    if (stepper.current_block->steps[CORE_AXIS_1] != stepper.current_block->steps[CORE_AXIS_2]
-      || (    stepper.current_block->steps[CORE_AXIS_1] > 0
-           && stepper.motor_direction(CORE_AXIS_1) CORE_YZ_CMP stepper.motor_direction(CORE_AXIS_2)
-         )
-    ) {
-      if (stepper.motor_direction(Y_HEAD))
+    #if ENABLED(COREYX) || ENABLED(COREYZ)
+      #define Y_CMP ==
+    #else
+      #define Y_CMP !=
+    #endif
+    #define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Y_CMP D_(2)) )
+    #define Y_AXIS_HEAD Y_HEAD
   #else
-    if (stepper.current_block->steps[Y_AXIS] > 0)
-      if (stepper.motor_direction(Y_AXIS))   // -direction
-  #endif
-      { // -direction
-        #if HAS_Y_MIN
-          UPDATE_ENDSTOP(Y, MIN);
-        #endif
-      }
-      else { // +direction
-        #if HAS_Y_MAX
-          UPDATE_ENDSTOP(Y, MAX);
-        #endif
-      }
-  #if CORE_IS_XY || CORE_IS_YZ
-    }
-  #endif
-
-
-  #if ENABLED(COREZX) || ENABLED(COREZY)
-    #define CORE_YZ_CMP ==
-  #elif ENABLED(COREXZ) || ENABLED(COREYZ)
-    #define CORE_YZ_CMP !=
+    #define Y_MOVE_TEST stepper.current_block->steps[Y_AXIS] > 0
+    #define Y_AXIS_HEAD Y_AXIS
   #endif
 
   #if CORE_IS_XZ || CORE_IS_YZ
@@ -386,76 +335,119 @@ void Endstops::update() {
      * If DeltaA ==  DeltaB, the movement is only in the 1st axis (X or Y, already handled above)
      * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Z)
      */
-    if (stepper.current_block->steps[CORE_AXIS_1] != stepper.current_block->steps[CORE_AXIS_2]
-      || (    stepper.current_block->steps[CORE_AXIS_1] > 0
-           && stepper.motor_direction(CORE_AXIS_1) CORE_YZ_CMP stepper.motor_direction(CORE_AXIS_2)
-         )
-    ) {
-      if (stepper.motor_direction(Z_HEAD))
+    #if ENABLED(COREZX) || ENABLED(COREZY)
+      #define Z_CMP ==
+    #else
+      #define Z_CMP !=
+    #endif
+    #define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Z_CMP D_(2)) )
+    #define Z_AXIS_HEAD Z_HEAD
   #else
-    if (stepper.current_block->steps[Z_AXIS] > 0)
-      if (stepper.motor_direction(Z_AXIS))
+    #define Z_MOVE_TEST stepper.current_block->steps[Z_AXIS] > 0
+    #define Z_AXIS_HEAD Z_AXIS
   #endif
-      { // Z -direction. Gantry down, bed up.
-        #if HAS_Z_MIN
-          #if ENABLED(Z_DUAL_ENDSTOPS)
 
-            UPDATE_ENDSTOP_BIT(Z, MIN);
-            #if HAS_Z2_MIN
-              UPDATE_ENDSTOP_BIT(Z2, MIN);
-            #else
-              COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
-            #endif
+  // With Dual X, endstops are only checked in the homing direction for the active extruder
+  #if ENABLED(DUAL_X_CARRIAGE)
+    #define E0_ACTIVE stepper.current_block->active_extruder == 0
+    #define X_MIN_TEST ((X_HOME_DIR < 0 && E0_ACTIVE) || (X2_HOME_DIR < 0 && !E0_ACTIVE))
+    #define X_MAX_TEST ((X_HOME_DIR > 0 && E0_ACTIVE) || (X2_HOME_DIR > 0 && !E0_ACTIVE))
+  #else
+    #define X_MIN_TEST true
+    #define X_MAX_TEST true
+  #endif
 
-            test_dual_z_endstops(Z_MIN, Z2_MIN);
+  /**
+   * Check and update endstops according to conditions
+   */
 
-          #else // !Z_DUAL_ENDSTOPS
+  if (X_MOVE_TEST) {
+    if (stepper.motor_direction(X_AXIS_HEAD)) {
+      if (X_MIN_TEST) { // -direction
+        #if HAS_X_MIN
+          UPDATE_ENDSTOP(X, MIN);
+        #endif
+      }
+    }
+    else if (X_MAX_TEST) { // +direction
+      #if HAS_X_MAX
+        UPDATE_ENDSTOP(X, MAX);
+      #endif
+    }
+  }
 
-            #if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
-              if (z_probe_enabled) UPDATE_ENDSTOP(Z, MIN);
-            #else
-              UPDATE_ENDSTOP(Z, MIN);
-            #endif
+  if (Y_MOVE_TEST) {
+    if (stepper.motor_direction(Y_AXIS_HEAD)) { // -direction
+      #if HAS_Y_MIN
+        UPDATE_ENDSTOP(Y, MIN);
+      #endif
+    }
+    else { // +direction
+      #if HAS_Y_MAX
+        UPDATE_ENDSTOP(Y, MAX);
+      #endif
+    }
+  }
 
-          #endif // !Z_DUAL_ENDSTOPS
+  if (Z_MOVE_TEST) {
+    if (stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up.
+      #if HAS_Z_MIN
+        #if ENABLED(Z_DUAL_ENDSTOPS)
 
-        #endif // HAS_Z_MIN
+          UPDATE_ENDSTOP_BIT(Z, MIN);
+          #if HAS_Z2_MIN
+            UPDATE_ENDSTOP_BIT(Z2, MIN);
+          #else
+            COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
+          #endif
 
-        // When closing the gap check the enabled probe
-        #if ENABLED(Z_MIN_PROBE_ENDSTOP)
-          if (z_probe_enabled) {
-            UPDATE_ENDSTOP(Z, MIN_PROBE);
-            if (TEST_ENDSTOP(Z_MIN_PROBE)) SBI(endstop_hit_bits, Z_MIN_PROBE);
-          }
-        #endif
-      }
-      else { // Z +direction. Gantry up, bed down.
-        #if HAS_Z_MAX
+          test_dual_z_endstops(Z_MIN, Z2_MIN);
+
+        #else // !Z_DUAL_ENDSTOPS
 
-          // Check both Z dual endstops
-          #if ENABLED(Z_DUAL_ENDSTOPS)
+          #if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
+            if (z_probe_enabled) UPDATE_ENDSTOP(Z, MIN);
+          #else
+            UPDATE_ENDSTOP(Z, MIN);
+          #endif
 
-            UPDATE_ENDSTOP_BIT(Z, MAX);
-            #if HAS_Z2_MAX
-              UPDATE_ENDSTOP_BIT(Z2, MAX);
-            #else
-              COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX);
-            #endif
+        #endif // !Z_DUAL_ENDSTOPS
 
-            test_dual_z_endstops(Z_MAX, Z2_MAX);
+      #endif // HAS_Z_MIN
 
-          // If this pin is not hijacked for the bed probe
-          // then it belongs to the Z endstop
-          #elif DISABLED(Z_MIN_PROBE_ENDSTOP) || Z_MAX_PIN != Z_MIN_PROBE_PIN
+      // When closing the gap check the enabled probe
+      #if ENABLED(Z_MIN_PROBE_ENDSTOP)
+        if (z_probe_enabled) {
+          UPDATE_ENDSTOP(Z, MIN_PROBE);
+          if (TEST_ENDSTOP(Z_MIN_PROBE)) SBI(endstop_hit_bits, Z_MIN_PROBE);
+        }
+      #endif
+    }
+    else { // Z +direction. Gantry up, bed down.
+      #if HAS_Z_MAX
 
-            UPDATE_ENDSTOP(Z, MAX);
+        // Check both Z dual endstops
+        #if ENABLED(Z_DUAL_ENDSTOPS)
 
-          #endif // !Z_MIN_PROBE_PIN...
-        #endif // Z_MAX_PIN
-      }
-  #if CORE_IS_XZ || CORE_IS_YZ
+          UPDATE_ENDSTOP_BIT(Z, MAX);
+          #if HAS_Z2_MAX
+            UPDATE_ENDSTOP_BIT(Z2, MAX);
+          #else
+            COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX);
+          #endif
+
+          test_dual_z_endstops(Z_MAX, Z2_MAX);
+
+        // If this pin is not hijacked for the bed probe
+        // then it belongs to the Z endstop
+        #elif DISABLED(Z_MIN_PROBE_ENDSTOP) || Z_MAX_PIN != Z_MIN_PROBE_PIN
+
+          UPDATE_ENDSTOP(Z, MAX);
+
+        #endif // !Z_MIN_PROBE_PIN...
+      #endif // Z_MAX_PIN
     }
-  #endif
+  }
 
   old_endstop_bits = current_endstop_bits;