🔧 More angles for 2x Servo Switching Nozzle (MarlinFirmware#26303)

Co-authored-by: Scott Lahteine <[email protected]>
EvilGremlin · Oct 26, 2023 · 977135c · 977135c
1 parent e8e87e1
commit 977135c
Show file tree

Hide file tree

Showing 5 changed files with 26 additions and 12 deletions.
diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
@@ -244,7 +244,8 @@
 #if ENABLED(SWITCHING_NOZZLE)
   #define SWITCHING_NOZZLE_SERVO_NR 0
   //#define SWITCHING_NOZZLE_E1_SERVO_NR 1          // If two servos are used, the index of the second
-  #define SWITCHING_NOZZLE_SERVO_ANGLES { 0, 90 }   // Angles for E0, E1 (single servo) or lowered/raised (dual servo)
+  #define SWITCHING_NOZZLE_SERVO_ANGLES { 0, 90 }   // A pair of angles for { E0, E1 }.
+                                                    // For Dual Servo use two pairs: { { lower, raise }, { lower, raise } }
   #define SWITCHING_NOZZLE_SERVO_DWELL 2500         // Dwell time to wait for servo to make physical move
 #endif
 

diff --git a/Marlin/src/gcode/config/M281.cpp b/Marlin/src/gcode/config/M281.cpp
@@ -66,6 +66,9 @@ void GcodeSuite::M281_report(const bool forReplay/*=true*/) {
         #endif
       #elif ENABLED(SWITCHING_NOZZLE)
         case SWITCHING_NOZZLE_SERVO_NR:
+        #if ENABLED(SWITCHING_NOZZLE_TWO_SERVOS)
+          case SWITCHING_NOZZLE_E1_SERVO_NR:
+        #endif
       #elif ENABLED(BLTOUCH) || (HAS_Z_SERVO_PROBE && defined(Z_SERVO_ANGLES))
         case Z_PROBE_SERVO_NR:
       #endif

diff --git a/Marlin/src/module/servo.h b/Marlin/src/module/servo.h
@@ -44,8 +44,13 @@
   #endif
 
   #if ENABLED(SWITCHING_NOZZLE)
-    constexpr uint16_t sasn[] = SWITCHING_NOZZLE_SERVO_ANGLES;
-    static_assert(COUNT(sasn) == 2, "SWITCHING_NOZZLE_SERVO_ANGLES needs 2 angles.");
+    #if SWITCHING_NOZZLE_TWO_SERVOS
+      constexpr uint16_t sasn[][2] = SWITCHING_NOZZLE_SERVO_ANGLES;
+      static_assert(COUNT(sasn) == 2, "SWITCHING_NOZZLE_SERVO_ANGLES (with SWITCHING_NOZZLE_E1_SERVO_NR) needs 2 sets of angles: { { lower, raise }, { lower, raise } }.");
+    #else
+      constexpr uint16_t sasn[] = SWITCHING_NOZZLE_SERVO_ANGLES;
+      static_assert(COUNT(sasn) == 2, "SWITCHING_NOZZLE_SERVO_ANGLES needs two angles: { E0, E1 }.");
+    #endif
   #else
     constexpr uint16_t sasn[2] = { 0 };
   #endif
@@ -75,12 +80,15 @@
     #define Z_PROBE_SERVO_NR -1
   #endif
 
-  #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[I]   \
-    : N == Z_PROBE_SERVO_NR                ? sazp[I]   \
-    : 0                                                )
+  #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                                                  )
 
   #if ENABLED(EDITABLE_SERVO_ANGLES)
     extern uint16_t servo_angles[NUM_SERVOS][2];
@@ -97,6 +105,9 @@
         , { 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) }
+          #endif
         #endif
       #endif
     #endif

diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
@@ -127,9 +127,8 @@
 
     inline void _move_nozzle_servo(const uint8_t e, const uint8_t angle_index) {
       constexpr int8_t  sns_index[2] = { SWITCHING_NOZZLE_SERVO_NR, SWITCHING_NOZZLE_E1_SERVO_NR };
-      constexpr int16_t sns_angles[2] = SWITCHING_NOZZLE_SERVO_ANGLES;
       planner.synchronize();
-      servo[sns_index[e]].move(sns_angles[angle_index]);
+      servo[sns_index[e]].move(servo_angles[sns_index[e]][angle_index]);
       safe_delay(SWITCHING_NOZZLE_SERVO_DWELL);
     }
 

diff --git a/buildroot/tests/LPC1768 b/buildroot/tests/LPC1768
@@ -27,7 +27,7 @@ exec_test $1 $2 "ReARM EFB VIKI2, SDSUPPORT, 2 Serial ports (USB CDC + UART0), N
 restore_configs
 opt_set MOTHERBOARD BOARD_MKS_SBASE \
         EXTRUDERS 2 TEMP_SENSOR_1 1 \
-        NUM_SERVOS 2 SERVO_DELAY '{ 300, 300 }'
+        NUM_SERVOS 2 SERVO_DELAY '{ 300, 300 }' SWITCHING_NOZZLE_SERVO_ANGLES '{ { 0, 90 }, { 90, 0 } }'
 opt_enable SWITCHING_NOZZLE SWITCHING_NOZZLE_E1_SERVO_NR EDITABLE_SERVO_ANGLES SERVO_DETACH_GCODE \
            ULTIMAKERCONTROLLER REALTIME_REPORTING_COMMANDS FULL_REPORT_TO_HOST_FEATURE
 exec_test $1 $2 "MKS SBASE with SWITCHING_NOZZLE, Grbl Realtime Report" "$3"