Fix multiple servos with STM32 (#16151)

Marlin/src/HAL/HAL_STM32/Servo.cpp

@@ -28,25 +28,30 @@
 
 #include "Servo.h"
 
-uint8_t servoPin[MAX_SERVOS] = { 0 };
+static uint_fast8_t servoCount = 0;
+constexpr millis_t servoDelay[] = SERVO_DELAY;
+static_assert(COUNT(servoDelay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
+
+libServo::libServo()
+: delay(servoDelay[servoCount++])
+{}
 
 int8_t libServo::attach(const int pin) {
-  if (servoIndex >= MAX_SERVOS) return -1;
-  if (pin > 0) servoPin[servoIndex] = pin;
-  return super::attach(servoPin[servoIndex]);
+  if (servoCount >= MAX_SERVOS) return -1;
+  if (pin > 0) servo_pin = pin;
+  return super::attach(servo_pin);
 }
 
 int8_t libServo::attach(const int pin, const int min, const int max) {
-  if (pin > 0) servoPin[servoIndex] = pin;
-  return super::attach(servoPin[servoIndex], min, max);
+  if (servoCount >= MAX_SERVOS) return -1;
+  if (pin > 0) servo_pin = pin;
+  return super::attach(servo_pin, min, max);
 }
 
 void libServo::move(const int value) {
-  constexpr uint16_t servo_delay[] = SERVO_DELAY;
-  static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
   if (attach(0) >= 0) {
     write(value);
-    safe_delay(servo_delay[servoIndex]);
+    safe_delay(delay);
     #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
       detach();
     #endif

Marlin/src/HAL/HAL_STM32/Servo.h

@@ -27,11 +27,13 @@
 // Inherit and expand on the official library
 class libServo : public Servo {
   public:
+    libServo();
     int8_t attach(const int pin);
     int8_t attach(const int pin, const int min, const int max);
     void move(const int value);
   private:
     typedef Servo super;
-    uint16_t min_ticks, max_ticks;
-    uint8_t servoIndex;               // index into the channel data for this servo
+
+    int servo_pin = 0;
+    millis_t delay = 0;
 };

buildroot/share/PlatformIO/variants/BIGTREE_SKR_PRO_1v1/variant.h

@@ -246,11 +246,11 @@ extern "C" {
 
 // Timer Definitions
 //Do not use timer used by PWM pins when possible. See PinMap_PWM in PeripheralPins.c
-#define TIMER_TONE              TIM6
+#define TIMER_TONE              TIM2
 #define TIMER_SERIAL            TIM7
 
 // Do not use basic timer: OC is required
-#define TIMER_SERVO             TIM2  //TODO: advanced-control timers don't work
+#define TIMER_SERVO             TIM6  //TODO: advanced-control timers don't work
 
 // UART Definitions
 // Define here Serial instance number to map on Serial generic name