Fix multiple servos with STM32 (MarlinFirmware#16151)

griehsler · Jan 30, 2020 · fd5f003 · fd5f003
1 parent 407c92f
commit fd5f003
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 13 deletions.
diff --git 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
@@ -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
@@ -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