From 96cf5561393e50b7ba0a05b27fecbbdd2d2d3950 Mon Sep 17 00:00:00 2001
From: MangaValk <patrickvalkmanga@hotmail.com>
Date: Wed, 11 Dec 2019 08:44:05 +0100
Subject: [PATCH] Fix multiple servos with STM32 (#16151)

---
 Marlin/src/HAL/HAL_STM32/Servo.cpp            | 23 +++++++++++--------
 Marlin/src/HAL/HAL_STM32/Servo.h              |  6 +++--
 .../variants/BIGTREE_SKR_PRO_1v1/variant.h    |  4 ++--
 3 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/Marlin/src/HAL/HAL_STM32/Servo.cpp b/Marlin/src/HAL/HAL_STM32/Servo.cpp
index 2882f071ac..9af8ba6184 100644
--- a/Marlin/src/HAL/HAL_STM32/Servo.cpp
+++ b/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
diff --git a/Marlin/src/HAL/HAL_STM32/Servo.h b/Marlin/src/HAL/HAL_STM32/Servo.h
index 592f3a0662..ec4b6be131 100644
--- a/Marlin/src/HAL/HAL_STM32/Servo.h
+++ b/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;
 };
diff --git a/buildroot/share/PlatformIO/variants/BIGTREE_SKR_PRO_1v1/variant.h b/buildroot/share/PlatformIO/variants/BIGTREE_SKR_PRO_1v1/variant.h
index 396f3138c0..55b8a60e10 100644
--- a/buildroot/share/PlatformIO/variants/BIGTREE_SKR_PRO_1v1/variant.h
+++ b/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