From c271a89a4fead7bdca111a002980ce4732f8d95b Mon Sep 17 00:00:00 2001
From: Scott Lahteine <thinkyhead@users.noreply.github.com>
Date: Thu, 5 Dec 2024 22:06:13 -0600
Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=A8=20Servo=20angles=20tweaks?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Marlin/src/inc/Conditionals-4-adv.h | 16 ++++++-------
 Marlin/src/module/servo.h           | 37 ++++++++++-------------------
 2 files changed, 20 insertions(+), 33 deletions(-)

diff --git a/Marlin/src/inc/Conditionals-4-adv.h b/Marlin/src/inc/Conditionals-4-adv.h
index ccded2eec6..be65120642 100644
--- a/Marlin/src/inc/Conditionals-4-adv.h
+++ b/Marlin/src/inc/Conditionals-4-adv.h
@@ -51,39 +51,39 @@
   #define NUM_SERVOS 0
   #if HAS_Z_SERVO_PROBE && NUM_SERVOS <= Z_PROBE_SERVO_NR
     #undef NUM_SERVOS
-    #define NUM_SERVOS (Z_PROBE_SERVO_NR + 1)
+    #define NUM_SERVOS INCREMENT(Z_PROBE_SERVO_NR)
   #endif
   #if ENABLED(CHAMBER_VENT) && NUM_SERVOS <= CHAMBER_VENT_SERVO_NR
     #undef NUM_SERVOS
-    #define NUM_SERVOS (CHAMBER_VENT_SERVO_NR + 1)
+    #define NUM_SERVOS INCREMENT(CHAMBER_VENT_SERVO_NR)
   #endif
   #if ENABLED(SWITCHING_TOOLHEAD) && NUM_SERVOS <= SWITCHING_TOOLHEAD_SERVO_NR
     #undef NUM_SERVOS
-    #define NUM_SERVOS (SWITCHING_TOOLHEAD_SERVO_NR + 1)
+    #define NUM_SERVOS INCREMENT(SWITCHING_TOOLHEAD_SERVO_NR)
   #endif
   #if ENABLED(SWITCHING_NOZZLE)
     #if NUM_SERVOS <= SWITCHING_NOZZLE_SERVO_NR
       #undef NUM_SERVOS
-      #define NUM_SERVOS (SWITCHING_NOZZLE_SERVO_NR + 1)
+      #define NUM_SERVOS INCREMENT(SWITCHING_NOZZLE_SERVO_NR)
     #endif
     #if NUM_SERVOS <= SWITCHING_NOZZLE_E1_SERVO_NR
       #undef NUM_SERVOS
-      #define NUM_SERVOS (SWITCHING_NOZZLE_E1_SERVO_NR + 1)
+      #define NUM_SERVOS INCREMENT(SWITCHING_NOZZLE_E1_SERVO_NR)
     #endif
   #endif
   #if ENABLED(SWITCHING_EXTRUDER)
     #if NUM_SERVOS <= SWITCHING_EXTRUDER_SERVO_NR
       #undef NUM_SERVOS
-      #define NUM_SERVOS (SWITCHING_EXTRUDER_SERVO_NR + 1)
+      #define NUM_SERVOS INCREMENT(SWITCHING_EXTRUDER_SERVO_NR)
     #endif
     #if NUM_SERVOS <= SWITCHING_EXTRUDER_E23_SERVO_NR
       #undef NUM_SERVOS
-      #define NUM_SERVOS (SWITCHING_EXTRUDER_E23_SERVO_NR + 1)
+      #define NUM_SERVOS INCREMENT(SWITCHING_EXTRUDER_E23_SERVO_NR)
     #endif
   #endif
   #if ENABLED(SPINDLE_SERVO) && NUM_SERVOS <= SPINDLE_SERVO_NR
     #undef NUM_SERVOS
-    #define NUM_SERVOS (SPINDLE_SERVO_NR + 1)
+    #define NUM_SERVOS INCREMENT(SPINDLE_SERVO_NR)
   #endif
 #endif // !defined(NUM_SERVOS)
 
diff --git a/Marlin/src/module/servo.h b/Marlin/src/module/servo.h
index 0286fe905b..3b6c33a8f7 100644
--- a/Marlin/src/module/servo.h
+++ b/Marlin/src/module/servo.h
@@ -55,7 +55,7 @@
     constexpr uint16_t sasn[2] = { 0 };
   #endif
 
-  #ifdef Z_PROBE_SERVO_NR
+  #if HAS_Z_SERVO_PROBE
     #if ENABLED(BLTOUCH)
       #include "../feature/bltouch.h"
       #undef Z_SERVO_ANGLES
@@ -76,6 +76,9 @@
   #ifndef SWITCHING_NOZZLE_SERVO_NR
     #define SWITCHING_NOZZLE_SERVO_NR -1
   #endif
+  #ifndef SWITCHING_NOZZLE_E1_SERVO_NR
+    #define SWITCHING_NOZZLE_E1_SERVO_NR -1
+  #endif
   #ifndef Z_PROBE_SERVO_NR
     #define Z_PROBE_SERVO_NR -1
   #endif
@@ -83,12 +86,12 @@
   #define SASN(J,I) TERN(SWITCHING_NOZZLE_TWO_SERVOS, sasn[J][I], sasn[I])
 
   #define ASRC(N,I) ( \
-                                         N == SWITCHING_EXTRUDER_SERVO_NR     ? sase[I]     \
-                                       : N == SWITCHING_EXTRUDER_E23_SERVO_NR ? sase[I+2]   \
-                                       : N == SWITCHING_NOZZLE_SERVO_NR       ? SASN(0,I)   \
-    TERN_(SWITCHING_NOZZLE_TWO_SERVOS, : N == SWITCHING_NOZZLE_E1_SERVO_NR    ? SASN(1,I))  \
-                                       : N == Z_PROBE_SERVO_NR                ? sazp[I]     \
-                                       : 0                                                  )
+      N == SWITCHING_EXTRUDER_SERVO_NR     ? sase[I]   \
+    : N == SWITCHING_EXTRUDER_E23_SERVO_NR ? sase[I+2] \
+    : N == SWITCHING_NOZZLE_SERVO_NR       ? SASN(0,I) \
+    : N == SWITCHING_NOZZLE_E1_SERVO_NR    ? SASN(1,I) \
+    : N == Z_PROBE_SERVO_NR                ? sazp[I]   \
+    : 0                                                )
 
   #if ENABLED(EDITABLE_SERVO_ANGLES)
     extern uint16_t servo_angles[NUM_SERVOS][2];
@@ -97,24 +100,8 @@
     #define CONST_SERVO_ANGLES servo_angles
   #endif
 
-  constexpr uint16_t CONST_SERVO_ANGLES [NUM_SERVOS][2] = {
-      { ASRC(0,0), ASRC(0,1) }
-    #if NUM_SERVOS > 1
-      , { ASRC(1,0), ASRC(1,1) }
-      #if NUM_SERVOS > 2
-        , { ASRC(2,0), ASRC(2,1) }
-        #if NUM_SERVOS > 3
-          , { ASRC(3,0), ASRC(3,1) }
-          #if NUM_SERVOS > 4
-            , { ASRC(4,0), ASRC(4,1) }
-            #if NUM_SERVOS > 5
-              , { ASRC(5,0), ASRC(5,1) }
-            #endif
-          #endif
-        #endif
-      #endif
-    #endif
-  };
+  #define _ASRC_PAIR(N) { ASRC(N,0), ASRC(N,1) },
+  constexpr uint16_t CONST_SERVO_ANGLES [NUM_SERVOS][2] = { REPEAT(NUM_SERVOS, _ASRC_PAIR) };
 
   #if HAS_Z_SERVO_PROBE
     #define DEPLOY_Z_SERVO() servo[Z_PROBE_SERVO_NR].move(servo_angles[Z_PROBE_SERVO_NR][0])