1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
|
/**
* Marlin 3D Printer Firmware
* Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
* Copyright (c) 2017 Victor Perez
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
*/
#ifdef __STM32F1__
#include "../../inc/MarlinConfig.h"
#if HAS_SERVOS
uint8_t ServoCount = 0;
#include "Servo.h"
//#include "Servo.h"
#include <boards.h>
#include <io.h>
#include <pwm.h>
#include <wirish_math.h>
/**
* 20 millisecond period config. For a 1-based prescaler,
*
* (prescaler * overflow / CYC_MSEC) msec = 1 timer cycle = 20 msec
* => prescaler * overflow = 20 * CYC_MSEC
*
* This uses the smallest prescaler that allows an overflow < 2^16.
*/
#define MAX_OVERFLOW UINT16_MAX // _BV(16) - 1
#define CYC_MSEC (1000 * CYCLES_PER_MICROSECOND)
#define TAU_MSEC 20
#define TAU_USEC (TAU_MSEC * 1000)
#define TAU_CYC (TAU_MSEC * CYC_MSEC)
#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1)
#define SERVO_OVERFLOW ((uint16_t)round((double)TAU_CYC / SERVO_PRESCALER))
// Unit conversions
#define US_TO_COMPARE(us) uint16_t(map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW))
#define COMPARE_TO_US(c) uint32_t(map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC))
#define ANGLE_TO_US(a) uint16_t(map((a), minAngle, maxAngle, SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW))
#define US_TO_ANGLE(us) int16_t(map((us), SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW, minAngle, maxAngle))
void libServo::servoWrite(uint8_t inPin, uint16_t duty_cycle) {
#ifdef SERVO0_TIMER_NUM
if (servoIndex == 0) {
pwmSetDuty(duty_cycle);
return;
}
#endif
timer_dev *tdev = PIN_MAP[inPin].timer_device;
uint8_t tchan = PIN_MAP[inPin].timer_channel;
if (tdev) timer_set_compare(tdev, tchan, duty_cycle);
}
libServo::libServo() {
servoIndex = ServoCount < MAX_SERVOS ? ServoCount++ : INVALID_SERVO;
timer_set_interrupt_priority(SERVO0_TIMER_NUM, SERVO0_TIMER_IRQ_PRIO);
}
bool libServo::attach(const int32_t inPin, const int32_t inMinAngle, const int32_t inMaxAngle) {
if (servoIndex >= MAX_SERVOS) return false;
if (inPin >= BOARD_NR_GPIO_PINS) return false;
minAngle = inMinAngle;
maxAngle = inMaxAngle;
angle = -1;
#ifdef SERVO0_TIMER_NUM
if (servoIndex == 0 && setupSoftPWM(inPin)) {
pin = inPin; // set attached()
return true;
}
#endif
if (!PWM_PIN(inPin)) return false;
timer_dev *tdev = PIN_MAP[inPin].timer_device;
//uint8_t tchan = PIN_MAP[inPin].timer_channel;
SET_PWM(inPin);
servoWrite(inPin, 0);
timer_pause(tdev);
timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based
timer_set_reload(tdev, SERVO_OVERFLOW);
timer_generate_update(tdev);
timer_resume(tdev);
pin = inPin; // set attached()
return true;
}
bool libServo::detach() {
if (!attached()) return false;
angle = -1;
servoWrite(pin, 0);
return true;
}
int32_t libServo::read() const {
if (attached()) {
#ifdef SERVO0_TIMER_NUM
if (servoIndex == 0) return angle;
#endif
timer_dev *tdev = PIN_MAP[pin].timer_device;
uint8_t tchan = PIN_MAP[pin].timer_channel;
return US_TO_ANGLE(COMPARE_TO_US(timer_get_compare(tdev, tchan)));
}
return 0;
}
void libServo::move(const int32_t 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 (attached()) {
angle = constrain(value, minAngle, maxAngle);
servoWrite(pin, US_TO_COMPARE(ANGLE_TO_US(angle)));
safe_delay(servo_delay[servoIndex]);
TERN_(DEACTIVATE_SERVOS_AFTER_MOVE, detach());
}
}
#ifdef SERVO0_TIMER_NUM
extern "C" void Servo_IRQHandler() {
static timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
uint16_t SR = timer_get_status(tdev);
if (SR & TIMER_SR_CC1IF) { // channel 1 off
#ifdef SERVO0_PWM_OD
OUT_WRITE_OD(SERVO0_PIN, 1); // off
#else
OUT_WRITE(SERVO0_PIN, 0);
#endif
timer_reset_status_bit(tdev, TIMER_SR_CC1IF_BIT);
}
if (SR & TIMER_SR_CC2IF) { // channel 2 resume
#ifdef SERVO0_PWM_OD
OUT_WRITE_OD(SERVO0_PIN, 0); // on
#else
OUT_WRITE(SERVO0_PIN, 1);
#endif
timer_reset_status_bit(tdev, TIMER_SR_CC2IF_BIT);
}
}
bool libServo::setupSoftPWM(const int32_t inPin) {
timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
if (!tdev) return false;
#ifdef SERVO0_PWM_OD
OUT_WRITE_OD(inPin, 1);
#else
OUT_WRITE(inPin, 0);
#endif
timer_pause(tdev);
timer_set_mode(tdev, 1, TIMER_OUTPUT_COMPARE); // counter with isr
timer_oc_set_mode(tdev, 1, TIMER_OC_MODE_FROZEN, 0); // no pin output change
timer_oc_set_mode(tdev, 2, TIMER_OC_MODE_FROZEN, 0); // no pin output change
timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based
timer_set_reload(tdev, SERVO_OVERFLOW);
timer_set_compare(tdev, 1, SERVO_OVERFLOW);
timer_set_compare(tdev, 2, SERVO_OVERFLOW);
timer_attach_interrupt(tdev, 1, Servo_IRQHandler);
timer_attach_interrupt(tdev, 2, Servo_IRQHandler);
timer_generate_update(tdev);
timer_resume(tdev);
return true;
}
void libServo::pwmSetDuty(const uint16_t duty_cycle) {
timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
timer_set_compare(tdev, 1, duty_cycle);
timer_generate_update(tdev);
if (duty_cycle) {
timer_enable_irq(tdev, 1);
timer_enable_irq(tdev, 2);
}
else {
timer_disable_irq(tdev, 1);
timer_disable_irq(tdev, 2);
#ifdef SERVO0_PWM_OD
OUT_WRITE_OD(pin, 1); // off
#else
OUT_WRITE(pin, 0);
#endif
}
}
void libServo::pauseSoftPWM() { // detach
timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM);
timer_pause(tdev);
pwmSetDuty(0);
}
#else
bool libServo::setupSoftPWM(const int32_t inPin) { return false; }
void libServo::pwmSetDuty(const uint16_t duty_cycle) {}
void libServo::pauseSoftPWM() {}
#endif
#endif // HAS_SERVOS
#endif // __STM32F1__
|
