Fix issues, make optional

Marlin/Configuration.h

 // If the servo can't reach the requested position, increase it.
 #define SERVO_DELAY { 300 }
 
-// Servo deactivation
-//
-// With this option servos are powered only during movement, then turned off to prevent jitter.
+// Only power servos during movement, otherwise leave off to prevent jitter
 //#define DEACTIVATE_SERVOS_AFTER_MOVE
 
+// Allow servo angle to be edited and saved to EEPROM
+//#define EDITABLE_SERVO_ANGLES
+
 #endif // CONFIGURATION_H

Marlin/src/config/default/Configuration.h

 // If the servo can't reach the requested position, increase it.
 #define SERVO_DELAY { 300 }
 
-// Servo deactivation
-//
-// With this option servos are powered only during movement, then turned off to prevent jitter.
+// Only power servos during movement, otherwise leave off to prevent jitter
 //#define DEACTIVATE_SERVOS_AFTER_MOVE
 
+// Allow servo angle to be edited and saved to EEPROM
+//#define EDITABLE_SERVO_ANGLES
+
 #endif // CONFIGURATION_H

Marlin/src/gcode/config/M281.cpp

  */
 #include "../../inc/MarlinConfig.h"
 
-#if HAS_SERVOS
+#if HAS_SERVOS && ENABLED(EDITABLE_SERVO_ANGLES)
 
 #include "../gcode.h"
 #include "../../module/servo.h"
 
 void GcodeSuite::M281() {
-    if (!parser.seen('P')) return;
+  if (!parser.seenval('P')) return;
   const int servo_index = parser.value_int();
   if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) {
     bool angle_change = false;
   }
 }
 
-#endif
+#endif // HAS_SERVOS && EDITABLE_SERVO_ANGLES

Marlin/src/gcode/config/M43.cpp

     uint8_t i = 0;
     bool deploy_state, stow_state;
     do {
-      MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); //deploy
+      MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy
       safe_delay(500);
       deploy_state = READ(PROBE_TEST_PIN);
-      MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); //stow
+      MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); // Stow
       safe_delay(500);
       stow_state = READ(PROBE_TEST_PIN);
     } while (++i < 4);
       #endif
     }
     else {                                           // measure active signal length
-      MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]);     // deploy
+      MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy
       safe_delay(500);
       SERIAL_PROTOCOLLNPGM("please trigger probe");
       uint16_t probe_counter = 0;
           else
             SERIAL_PROTOCOLLNPGM("noise detected - please re-run test"); // less than 2mS pulse
 
-          MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); //stow
+          MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); // Stow
 
         }  // pulse detected
 

Marlin/src/gcode/gcode.cpp

 
       #if HAS_SERVOS
         case 280: M280(); break;                                  // M280: Set servo position absolute
-        case 281: M281(); break;
+        #if ENABLED(EDITABLE_SERVO_ANGLES)
+          case 281: M281(); break;                                // M281: Set servo angles
+        #endif
       #endif
 
       #if ENABLED(BABYSTEPPING)

Marlin/src/gcode/gcode.h

  * M260 - i2c Send Data (Requires EXPERIMENTAL_I2CBUS)
  * M261 - i2c Request Data (Requires EXPERIMENTAL_I2CBUS)
  * M280 - Set servo position absolute: "M280 P<index> S<angle|µs>". (Requires servos)
- * M281 - Set servo min|max position: "M281 P<index> L<min> U<max>". (Requires servos)
+ * M281 - Set servo min|max position: "M281 P<index> L<min> U<max>". (Requires EDITABLE_SERVO_ANGLES)
  * M290 - Babystepping (Requires BABYSTEPPING)
  * M300 - Play beep sound S<frequency Hz> P<duration ms>
  * M301 - Set PID parameters P I and D. (Requires PIDTEMP)
 
   #if HAS_SERVOS
     static void M280();
-    static void M281();
+    #if ENABLED(EDITABLE_SERVO_ANGLES)
+      static void M281();
+    #endif
   #endif
 
   #if ENABLED(BABYSTEPPING)

Marlin/src/inc/SanityCheck.h

 /**
  * Limited number of servos
  */
-#if NUM_SERVOS > 4
-  #error "The maximum number of SERVOS in Marlin is 4."
+#if NUM_SERVOS > MAX_SERVOS
+  #error "The selected board doesn't support enough servos for your configuration. Reduce NUM_SERVOS."
 #endif
 
 /**

Marlin/src/module/configuration_store.cpp

 
 #if HAS_SERVOS
   #include "servo.h"
-#endif 
+#endif
 
 #if HAS_BED_PROBE
   #include "../module/probe.h"
   //
   // SERVO_ANGLES
   //
-  #if HAS_SERVOS
-    uint8_t servo_angles[NUM_SERVOS][2];
-  #endif
+  uint16_t servo_angles[MAX_SERVOS][2];                 // M281 P L U
 
   //
   // DELTA / [XYZ]_DUAL_ENDSTOPS
       EEPROM_WRITE(storage_slot);
     #endif // AUTO_BED_LEVELING_UBL
 
-    #if HAS_SERVOS
-      EEPROM_WRITE(servo_angles);
+    #if !HAS_SERVOS || DISABLED(EDITABLE_SERVO_ANGLES)
+      #if ENABLED(SWITCHING_EXTRUDER)
+        constexpr uint16_t sesa[][2] = SWITCHING_EXTRUDER_SERVO_ANGLES;
+      #endif
+      constexpr uint16_t servo_angles[MAX_SERVOS][2] = {
+        #if ENABLED(SWITCHING_EXTRUDER)
+          [SWITCHING_EXTRUDER_SERVO_NR] = { sesa[0], sesa[1] }
+          #if EXTRUDERS > 3
+            , [SWITCHING_EXTRUDER_E23_SERVO_NR] = { sesa[2], sesa[3] }
+          #endif
+        #elif ENABLED(SWITCHING_NOZZLE)
+          [SWITCHING_NOZZLE_SERVO_NR] = SWITCHING_NOZZLE_SERVO_ANGLES
+        #elif defined(Z_SERVO_ANGLES) && defined(Z_PROBE_SERVO_NR)
+          [Z_PROBE_SERVO_NR] = Z_SERVO_ANGLES
+        #endif
+      };
     #endif
 
+    EEPROM_WRITE(servo_angles);
+
     // 11 floats for DELTA / [XYZ]_DUAL_ENDSTOPS
     #if ENABLED(DELTA)
 
       //
       // SERVO_ANGLES
       //
-      #if HAS_SERVOS
-        EEPROM_READ(servo_angles);
+      #if !HAS_SERVOS || DISABLED(EDITABLE_SERVO_ANGLES)
+        uint16_t servo_angles[MAX_SERVOS][2];
       #endif
-
+      EEPROM_READ(servo_angles);
 
       //
       // DELTA Geometry or Dual Endstops offsets
   // Servo Angles
   //
 
-  #if HAS_SERVOS
+  #if HAS_SERVOS && ENABLED(EDITABLE_SERVO_ANGLES)
+
     #if ENABLED(SWITCHING_EXTRUDER)
+
       #if EXTRUDERS > 3
         #define REQ_ANGLES 4
       #else
         #define REQ_ANGLES 2
       #endif
-      const uint8_t extruder_angles[2] = SWITCHING_EXTRUDER_SERVO_ANGLES;
+      constexpr uint16_t extruder_angles[] = SWITCHING_EXTRUDER_SERVO_ANGLES;
       static_assert(COUNT(extruder_angles) == REQ_ANGLES, "SWITCHING_EXTRUDER_SERVO_ANGLES needs " STRINGIFY(REQ_ANGLES) " angles.");
       servo_angles[SWITCHING_EXTRUDER_SERVO_NR][0] = extruder_angles[0];
       servo_angles[SWITCHING_EXTRUDER_SERVO_NR][1] = extruder_angles[1];
-    #endif
+      #if EXTRUDERS > 3
+        servo_angles[SWITCHING_EXTRUDER_E23_SERVO_NR][0] = extruder_angles[2];
+        servo_angles[SWITCHING_EXTRUDER_E23_SERVO_NR][1] = extruder_angles[3];
+      #endif
 
-    #if ENABLED(SWITCHING_NOZZLE)
-      const uint8_t nozzel_angles[2] = SWITCHING_NOZZLE_SERVO_ANGLES;
-      servo_angles[SWITCHING_NOZZLE_SERVO_NR][0] = nozzel_angles[0];
-      servo_angles[SWITCHING_NOZZLE_SERVO_NR][1] = nozzel_angles[1];
-    #endif
+    #elif ENABLED(SWITCHING_NOZZLE)
 
-    #if defined(Z_SERVO_ANGLES) && defined(Z_PROBE_SERVO_NR)
-      const uint8_t z_probe_angles[2] = Z_SERVO_ANGLES;
+      constexpr uint16_t nozzle_angles[2] = SWITCHING_NOZZLE_SERVO_ANGLES;
+      servo_angles[SWITCHING_NOZZLE_SERVO_NR][0] = nozzle_angles[0];
+      servo_angles[SWITCHING_NOZZLE_SERVO_NR][1] = nozzle_angles[1];
+
+    #elif defined(Z_SERVO_ANGLES) && defined(Z_PROBE_SERVO_NR)
+
+      constexpr uint16_t z_probe_angles[2] = Z_SERVO_ANGLES;
       servo_angles[Z_PROBE_SERVO_NR][0] = z_probe_angles[0];
       servo_angles[Z_PROBE_SERVO_NR][1] = z_probe_angles[1];
+
     #endif
 
-  #endif
+  #endif // HAS_SERVOS && EDITABLE_SERVO_ANGLES
 
   #if ENABLED(DELTA)
     const float adj[ABC] = DELTA_ENDSTOP_ADJ,
 
     #endif // HAS_LEVELING
 
-    #if HAS_SERVOS
+    #if HAS_SERVOS && ENABLED(EDITABLE_SERVO_ANGLES)
+
       if (!forReplay) {
         CONFIG_ECHO_START;
         SERIAL_ECHOLNPGM_P(port, "Servo Angles:");
       }
       for (uint8_t i = 0; i < NUM_SERVOS; i++) {
-        CONFIG_ECHO_START;
-        SERIAL_ECHOPAIR_P(port, "  M281 P", i);
-        SERIAL_ECHOPAIR_P(port, " L",servo_angles[i][0]);
-        SERIAL_ECHOPAIR_P(port, " U",servo_angles[i][1]);
-        SERIAL_EOL_P(port);
+        switch (i) {
+          #if ENABLED(SWITCHING_EXTRUDER)
+            case SWITCHING_EXTRUDER_SERVO_NR:
+            #if EXTRUDERS > 3
+              case SWITCHING_EXTRUDER_E23_SERVO_NR:
+            #endif
+          #elif ENABLED(SWITCHING_NOZZLE)
+            case SWITCHING_NOZZLE_SERVO_NR:
+          #elif defined(Z_SERVO_ANGLES) && defined(Z_PROBE_SERVO_NR)
+            case Z_PROBE_SERVO_NR:
+          #endif
+            CONFIG_ECHO_START;
+            SERIAL_ECHOPAIR_P(port, "  M281 P", int(i));
+            SERIAL_ECHOPAIR_P(port, " L", servo_angles[i][0]);
+            SERIAL_ECHOPAIR_P(port, " U", servo_angles[i][1]);
+            SERIAL_EOL_P(port);
+          default: break;
+        }
       }
-    #endif
+
+    #endif // HAS_SERVOS && EDITABLE_SERVO_ANGLES
 
     #if ENABLED(DELTA)
 

Marlin/src/module/probe.cpp

 
       #elif HAS_Z_SERVO_PROBE && DISABLED(BLTOUCH)
 
-        MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][(deploy ? 0 : 1)]);
+        MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][deploy ? 0 : 1]);
 
       #elif ENABLED(Z_PROBE_ALLEN_KEY)
 

Marlin/src/module/servo.cpp

 #include "servo.h"
 
 HAL_SERVO_LIB servo[NUM_SERVOS];
-uint8_t servo_angles[NUM_SERVOS][2];
+uint16_t servo_angles[NUM_SERVOS][2];
 
 void servo_init() {
   #if NUM_SERVOS >= 1 && HAS_SERVO_0

Marlin/src/module/servo.h

 #include "../HAL/shared/servo.h"
 
 extern HAL_SERVO_LIB servo[NUM_SERVOS];
-extern uint8_t servo_angles[NUM_SERVOS][2];
+extern uint16_t servo_angles[NUM_SERVOS][2];
 extern void servo_init();
 
 #define MOVE_SERVO(I, P) servo[I].move(P)

Marlin/src/module/tool_change.cpp

 #if DO_SWITCH_EXTRUDER
 
   #if EXTRUDERS > 3
-    #define _SERVO_NR (e < 2 ? SWITCHING_EXTRUDER_SERVO_NR : SWITCHING_EXTRUDER_E23_SERVO_NR)
+    #define _SERVO_NR(E) ((E) < 2 ? SWITCHING_EXTRUDER_SERVO_NR : SWITCHING_EXTRUDER_E23_SERVO_NR)
   #else
-    #define _SERVO_NR SWITCHING_EXTRUDER_SERVO_NR
+    #define _SERVO_NR(E) SWITCHING_EXTRUDER_SERVO_NR
   #endif
 
   void move_extruder_servo(const uint8_t e) {
       if (e < EXTRUDERS - 1)
     #endif
     {
-      MOVE_SERVO(_SERVO_NR, servo_angles[_SERVO_NR][e]);
+      MOVE_SERVO(_SERVO_NR(e), servo_angles[_SERVO_NR(e)][e]);
       safe_delay(500);
     }
   }
 
   void move_nozzle_servo(const uint8_t e) {
     planner.synchronize();
-    MOVE_SERVO(SWITCHING_NOZZLE_SERVO_NR, servo_angles[SWITCHING_EXTRUDER_SERVO_NR][e]);
+    MOVE_SERVO(SWITCHING_NOZZLE_SERVO_NR, servo_angles[SWITCHING_NOZZLE_SERVO_NR][e]);
     safe_delay(500);
   }
 

Marlin/src/pins/pins.h

   #define MAX_EXTRUDERS 5
 #endif
 
+#ifndef MAX_SERVOS
+  #define MAX_SERVOS 4
+#endif
+
 //
 // Assign auto fan pins if needed
 //