|
@@ -30,6 +30,7 @@
|
30
|
30
|
uint8_t ServoCount = 0;
|
31
|
31
|
|
32
|
32
|
#include "HAL_Servo_STM32F1.h"
|
|
33
|
+#include "HAL_timers_STM32F1.h"
|
33
|
34
|
|
34
|
35
|
//#include "Servo.h"
|
35
|
36
|
|
|
@@ -46,7 +47,7 @@ uint8_t ServoCount = 0;
|
46
|
47
|
*
|
47
|
48
|
* This uses the smallest prescaler that allows an overflow < 2^16.
|
48
|
49
|
*/
|
49
|
|
-#define MAX_OVERFLOW ((1 << 16) - 1)
|
|
50
|
+#define MAX_OVERFLOW UINT16_MAX //((1 << 16) - 1)
|
50
|
51
|
#define CYC_MSEC (1000 * CYCLES_PER_MICROSECOND)
|
51
|
52
|
#define TAU_MSEC 20
|
52
|
53
|
#define TAU_USEC (TAU_MSEC * 1000)
|
|
@@ -62,22 +63,45 @@ uint8_t ServoCount = 0;
|
62
|
63
|
#define US_TO_ANGLE(us) ((int16_t)(map((us), SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW, \
|
63
|
64
|
this->minAngle, this->maxAngle)))
|
64
|
65
|
|
|
66
|
+void libServo::servoWrite(uint8_t pin, uint16_t duty_cycle) {
|
|
67
|
+ #ifdef SERVO0_TIMER_NUM
|
|
68
|
+ if (this->servoIndex == 0) {
|
|
69
|
+ this->pwmSetDuty(duty_cycle);
|
|
70
|
+ return;
|
|
71
|
+ }
|
|
72
|
+ #endif
|
|
73
|
+
|
|
74
|
+ timer_dev *tdev = PIN_MAP[pin].timer_device;
|
|
75
|
+ uint8_t tchan = PIN_MAP[pin].timer_channel;
|
|
76
|
+ if (tdev) timer_set_compare(tdev, tchan, duty_cycle);
|
|
77
|
+}
|
|
78
|
+
|
65
|
79
|
libServo::libServo() {
|
66
|
80
|
this->servoIndex = ServoCount < MAX_SERVOS ? ServoCount++ : INVALID_SERVO;
|
67
|
81
|
}
|
68
|
82
|
|
69
|
83
|
bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t maxAngle) {
|
70
|
84
|
if (this->servoIndex >= MAX_SERVOS) return false;
|
71
|
|
- if (!PWM_PIN(pin)) return false;
|
|
85
|
+ if (pin >= BOARD_NR_GPIO_PINS) return false;
|
72
|
86
|
|
73
|
87
|
this->minAngle = minAngle;
|
74
|
88
|
this->maxAngle = maxAngle;
|
|
89
|
+ this->angle = -1;
|
|
90
|
+
|
|
91
|
+ #ifdef SERVO0_TIMER_NUM
|
|
92
|
+ if (this->servoIndex == 0 && this->setupSoftPWM(pin)) {
|
|
93
|
+ this->pin = pin; // set attached()
|
|
94
|
+ return true;
|
|
95
|
+ }
|
|
96
|
+ #endif
|
|
97
|
+
|
|
98
|
+ if (!PWM_PIN(pin)) return false;
|
75
|
99
|
|
76
|
100
|
timer_dev *tdev = PIN_MAP[pin].timer_device;
|
77
|
101
|
uint8_t tchan = PIN_MAP[pin].timer_channel;
|
78
|
102
|
|
79
|
|
- pinMode(pin, PWM);
|
80
|
|
- pwmWrite(pin, 0);
|
|
103
|
+ SET_PWM(pin);
|
|
104
|
+ servoWrite(pin, 0);
|
81
|
105
|
|
82
|
106
|
timer_pause(tdev);
|
83
|
107
|
timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based
|
|
@@ -92,12 +116,16 @@ bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t m
|
92
|
116
|
|
93
|
117
|
bool libServo::detach() {
|
94
|
118
|
if (!this->attached()) return false;
|
95
|
|
- pwmWrite(this->pin, 0);
|
|
119
|
+ this->angle = -1;
|
|
120
|
+ servoWrite(this->pin, 0);
|
96
|
121
|
return true;
|
97
|
122
|
}
|
98
|
123
|
|
99
|
124
|
int32_t libServo::read() const {
|
100
|
125
|
if (this->attached()) {
|
|
126
|
+ #ifdef SERVO0_TIMER_NUM
|
|
127
|
+ if (this->servoIndex == 0) return this->angle;
|
|
128
|
+ #endif
|
101
|
129
|
timer_dev *tdev = PIN_MAP[this->pin].timer_device;
|
102
|
130
|
uint8_t tchan = PIN_MAP[this->pin].timer_channel;
|
103
|
131
|
return US_TO_ANGLE(COMPARE_TO_US(timer_get_compare(tdev, tchan)));
|
|
@@ -110,13 +138,95 @@ void libServo::move(const int32_t value) {
|
110
|
138
|
static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
|
111
|
139
|
|
112
|
140
|
if (this->attached()) {
|
113
|
|
- pwmWrite(this->pin, US_TO_COMPARE(ANGLE_TO_US(constrain(value, this->minAngle, this->maxAngle))));
|
|
141
|
+ this->angle = constrain(value, this->minAngle, this->maxAngle);
|
|
142
|
+ servoWrite(this->pin, US_TO_COMPARE(ANGLE_TO_US(this->angle)));
|
114
|
143
|
safe_delay(servo_delay[this->servoIndex]);
|
115
|
144
|
#if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
|
116
|
145
|
this->detach();
|
117
|
146
|
#endif
|
118
|
147
|
}
|
119
|
148
|
}
|
|
149
|
+
|
|
150
|
+#ifdef SERVO0_TIMER_NUM
|
|
151
|
+ extern "C" void Servo_IRQHandler(void) {
|
|
152
|
+ static timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
|
|
153
|
+ uint16_t SR = timer_get_status(tdev);
|
|
154
|
+ if (SR & TIMER_SR_CC1IF) { // channel 1 off
|
|
155
|
+ #ifdef SERVO0_PWM_OD
|
|
156
|
+ OUT_WRITE_OD(SERVO0_PIN, 1); // off
|
|
157
|
+ #else
|
|
158
|
+ OUT_WRITE(SERVO0_PIN, 0);
|
|
159
|
+ #endif
|
|
160
|
+ timer_reset_status_bit(tdev, TIMER_SR_CC1IF_BIT);
|
|
161
|
+ }
|
|
162
|
+ if (SR & TIMER_SR_CC2IF) { // channel 2 resume
|
|
163
|
+ #ifdef SERVO0_PWM_OD
|
|
164
|
+ OUT_WRITE_OD(SERVO0_PIN, 0); // on
|
|
165
|
+ #else
|
|
166
|
+ OUT_WRITE(SERVO0_PIN, 1);
|
|
167
|
+ #endif
|
|
168
|
+ timer_reset_status_bit(tdev, TIMER_SR_CC2IF_BIT);
|
|
169
|
+ }
|
|
170
|
+ }
|
|
171
|
+
|
|
172
|
+ bool libServo::setupSoftPWM(const int32_t pin) {
|
|
173
|
+ timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
|
|
174
|
+ if (!tdev) return false;
|
|
175
|
+ #ifdef SERVO0_PWM_OD
|
|
176
|
+ OUT_WRITE_OD(pin, 1);
|
|
177
|
+ #else
|
|
178
|
+ OUT_WRITE(pin, 0);
|
|
179
|
+ #endif
|
|
180
|
+
|
|
181
|
+ timer_pause(tdev);
|
|
182
|
+ timer_set_mode(tdev, 1, TIMER_OUTPUT_COMPARE); // counter with isr
|
|
183
|
+ timer_oc_set_mode(tdev, 1, TIMER_OC_MODE_FROZEN, 0); // no pin output change
|
|
184
|
+ timer_oc_set_mode(tdev, 2, TIMER_OC_MODE_FROZEN, 0); // no pin output change
|
|
185
|
+ timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based
|
|
186
|
+ timer_set_reload(tdev, SERVO_OVERFLOW);
|
|
187
|
+ timer_set_compare(tdev, 1, SERVO_OVERFLOW);
|
|
188
|
+ timer_set_compare(tdev, 2, SERVO_OVERFLOW);
|
|
189
|
+ timer_attach_interrupt(tdev, 1, Servo_IRQHandler);
|
|
190
|
+ timer_attach_interrupt(tdev, 2, Servo_IRQHandler);
|
|
191
|
+ timer_generate_update(tdev);
|
|
192
|
+ timer_resume(tdev);
|
|
193
|
+
|
|
194
|
+ return true;
|
|
195
|
+ }
|
|
196
|
+
|
|
197
|
+ void libServo::pwmSetDuty(const uint16_t duty_cycle) {
|
|
198
|
+ timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
|
|
199
|
+ timer_set_compare(tdev, 1, duty_cycle);
|
|
200
|
+ timer_generate_update(tdev);
|
|
201
|
+ if (duty_cycle) {
|
|
202
|
+ timer_enable_irq(tdev, 1);
|
|
203
|
+ timer_enable_irq(tdev, 2);
|
|
204
|
+ }
|
|
205
|
+ else {
|
|
206
|
+ timer_disable_irq(tdev, 1);
|
|
207
|
+ timer_disable_irq(tdev, 2);
|
|
208
|
+ #ifdef SERVO0_PWM_OD
|
|
209
|
+ OUT_WRITE_OD(this->pin, 1); // off
|
|
210
|
+ #else
|
|
211
|
+ OUT_WRITE(this->pin, 0);
|
|
212
|
+ #endif
|
|
213
|
+ }
|
|
214
|
+ }
|
|
215
|
+
|
|
216
|
+ void libServo::pauseSoftPWM() { // detach
|
|
217
|
+ timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
|
|
218
|
+ timer_pause(tdev);
|
|
219
|
+ pwmSetDuty(0);
|
|
220
|
+ }
|
|
221
|
+
|
|
222
|
+#else
|
|
223
|
+
|
|
224
|
+ bool libServo::setupSoftPWM(const int32_t pin) {}
|
|
225
|
+ void libServo::pwmSetDuty(const uint16_t duty_cycle) {}
|
|
226
|
+ void libServo::pauseSoftPWM() {}
|
|
227
|
+
|
|
228
|
+#endif
|
|
229
|
+
|
120
|
230
|
#endif // HAS_SERVOS
|
121
|
231
|
|
122
|
232
|
#endif // __STM32F1__
|