Browse Source

Fix multiple servos with STM32 (#16151)

MangaValk 4 years ago
parent
commit
96cf556139

+ 14
- 9
Marlin/src/HAL/HAL_STM32/Servo.cpp View File

@@ -28,25 +28,30 @@
28 28
 
29 29
 #include "Servo.h"
30 30
 
31
-uint8_t servoPin[MAX_SERVOS] = { 0 };
31
+static uint_fast8_t servoCount = 0;
32
+constexpr millis_t servoDelay[] = SERVO_DELAY;
33
+static_assert(COUNT(servoDelay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
34
+
35
+libServo::libServo()
36
+: delay(servoDelay[servoCount++])
37
+{}
32 38
 
33 39
 int8_t libServo::attach(const int pin) {
34
-  if (servoIndex >= MAX_SERVOS) return -1;
35
-  if (pin > 0) servoPin[servoIndex] = pin;
36
-  return super::attach(servoPin[servoIndex]);
40
+  if (servoCount >= MAX_SERVOS) return -1;
41
+  if (pin > 0) servo_pin = pin;
42
+  return super::attach(servo_pin);
37 43
 }
38 44
 
39 45
 int8_t libServo::attach(const int pin, const int min, const int max) {
40
-  if (pin > 0) servoPin[servoIndex] = pin;
41
-  return super::attach(servoPin[servoIndex], min, max);
46
+  if (servoCount >= MAX_SERVOS) return -1;
47
+  if (pin > 0) servo_pin = pin;
48
+  return super::attach(servo_pin, min, max);
42 49
 }
43 50
 
44 51
 void libServo::move(const int value) {
45
-  constexpr uint16_t servo_delay[] = SERVO_DELAY;
46
-  static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
47 52
   if (attach(0) >= 0) {
48 53
     write(value);
49
-    safe_delay(servo_delay[servoIndex]);
54
+    safe_delay(delay);
50 55
     #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
51 56
       detach();
52 57
     #endif

+ 4
- 2
Marlin/src/HAL/HAL_STM32/Servo.h View File

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

+ 2
- 2
buildroot/share/PlatformIO/variants/BIGTREE_SKR_PRO_1v1/variant.h View File

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

Loading…
Cancel
Save