-
Notifications
You must be signed in to change notification settings - Fork 0
/
PCA9685_servo.cpp
351 lines (304 loc) · 8.3 KB
/
PCA9685_servo.cpp
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
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
#include "PCA9685_servo.h"
/// @brief PCA9685_servo constructor.
/// @param controller PCA9685_servo_controller pointer
/// @param channel channel number of the servo on the PCA9685
/// @param PWMmin minimal PWM signal for the servo. This is not the minimal pulse width of the servo, but rather the pulse length count. Min and max values usually within (150-600) range.
/// @param PWMmax maximal PWM signal for the servo. Just like the PWMmin, to be determined experimentally, by slowly raising the value and checking the motion range.
PCA9685_servo::PCA9685_servo(PCA9685_servo_driver *controller, uint8_t channel, uint16_t PWMmin, uint16_t PWMmax) :
_controller{controller}, _channel{channel}, _PWMmin{PWMmin}, _PWMmax{PWMmax}
{}
/// @brief Routine to call every loop iteration.
/// @note Enclose this in a for()/while() loop and call every time, passing
/// TEllapsed argument containing the time passed since loop() was last called.
/// @note E.g.
/// @note TNow = time_us_64(); // time now in microseconds
/// @note TEllapsed = TNow - TPrevious; // time, in microseconds, since the last loop
/// @note TPrevious = TNow; // store this ready for the next loop
/// @note servo.loop(TEllapsed);
/// @param TEllapsed time since last function call in microseconds
void PCA9685_servo::loop(uint64_t TEllapsed)
{
switch (_mode)
{
case MODE_FAST:
{
if (_isMoving)
{
// has enough time ellasped to allow us to have moved into position?
_Tcntr += TEllapsed;
if (_Tcntr >= _TFastDuration)
{
// we have finished moving
_isMoving = 0;
if (onStopMove != NULL) onStopMove(0);
}
}
break;
}
case MODE_TCONSTANT:
case MODE_SCONSTANT:
{
if (_isMoving)
{
// has enough time ellasped that we are due to move again?
_Tcntr += TEllapsed;
if (_Tcntr >= _TConstantPeriod)
{
_Tcntr = 0;
// we need to move by one degree
if (_currentAngle < _targetAngle)
{
// increase angle
_currentAngle++;
uint16_t PWM = (uint16_t) _controller->map(_currentAngle, -90, 90, _PWMmin, _PWMmax);
_controller->setPWM(_channel, PWM);
}
else
{
// decrease angle
_currentAngle--;
uint16_t PWM = (uint16_t) _controller->map(_currentAngle, -90, 90, _PWMmin, _PWMmax);
_controller->setPWM(_channel, PWM);
}
}
// have we finished moving?
if (_currentAngle == _targetAngle)
{
_isMoving = 0;
if (onStopMove != NULL) onStopMove(0);
}
}
break;
}
}
return;
}
/// @brief Set target position for the servo.
/// @param Angle target position in degrees
void PCA9685_servo::setPosition(int8_t angle)
{
_startAngle = _currentAngle;
if (angle < _minAngle) angle = _minAngle;
if (angle > _maxAngle) angle = _maxAngle;
switch (_mode)
{
// move to the set point as quickly as possible
case MODE_FAST:
{
_currentAngle = angle;
uint16_t PWM = (uint16_t) _controller->map(angle, -90, 90, _PWMmin, _PWMmax);
_controller->setPWM(_channel, PWM);
_isMoving = 1;
_Tcntr = 0;
if (onStartMove != NULL) onStartMove(0);
break;
}
// move to the set point gradually over a constant time
case MODE_TCONSTANT:
{
_targetAngle = angle; // where we wish to end up
uint64_t AngleDelta = abs(angle - _currentAngle); // how many degrees we need to move
// how long between 1 degree movements?
_TConstantPeriod = _TConstantDuration / AngleDelta;
_isMoving = 1;
_Tcntr = 0;
if (onStartMove != NULL) onStartMove(0);
break;
}
// move to the set point gradually at a set angular velocity
case MODE_SCONSTANT:
{
_targetAngle = angle; // where we wish to end up
_TConstantPeriod = _SConstantPeriod;
_isMoving = 1;
_Tcntr = 0;
if (onStartMove != NULL) onStartMove(0);
break;
}
}
return;
}
/// @brief Set relative target position for the servo (in realtion to current angle).
/// @param angle relative angle in degrees
/// @note Does not work in MODE_FAST mode.
void PCA9685_servo::setRelativePosition(int16_t angle)
{
int16_t newAngle = _currentAngle + angle;
if (newAngle < _minAngle) newAngle = _minAngle;
if (newAngle > _maxAngle) newAngle = _maxAngle;
setPosition(newAngle);
}
/// @brief Get the current angle of the servo.
/// @param
/// @return current angle of the servo.
int8_t PCA9685_servo::getPosition(void)
{
return _currentAngle;
}
void PCA9685_servo::setRange(int8_t minAngle, int8_t maxAngle)
{
_minAngle = minAngle;
_maxAngle = maxAngle;
_midAngle = ((_maxAngle - _minAngle) / 2) + _minAngle;
return;
}
void PCA9685_servo::setRange(int8_t minAngle, int8_t midAngle, int8_t maxAngle)
{
_minAngle = minAngle;
_maxAngle = maxAngle;
_midAngle = midAngle;
return;
}
uint8_t PCA9685_servo::getInvertMode(void)
{
return _invert;
}
void PCA9685_servo::setInvertMode(uint8_t invert)
{
_invert = invert;
}
/// @brief Set servo position to its max or min angle value.
/// @param value 0 if movement to max, -1 if movement to min.
void PCA9685_servo::throwServo(uint8_t value)
{
if (_invert)
{
// inverted operation
if (value == 0)
{
setPosition(_maxAngle);
}
else
{
setPosition(_minAngle);
}
}
else
{
// normal operation
if (value == 0)
{
setPosition(_minAngle);
}
else
{
setPosition(_maxAngle);
}
}
return;
}
int8_t PCA9685_servo::getMinAngle(void)
{
return _minAngle;
}
int8_t PCA9685_servo::getMaxAngle(void)
{
return _maxAngle;
}
int8_t PCA9685_servo::getMidAngle(void)
{
return _midAngle;
}
uint8_t PCA9685_servo::getMode(void)
{
return _mode;
}
uint16_t PCA9685_servo::getAddress(void)
{
return _address;
}
/// @brief Check if servo is in motion.
/// @param
/// @return true if servo in motion, false otherwise.
uint8_t PCA9685_servo::isMoving(void)
{
return _isMoving;
}
uint64_t PCA9685_servo::getTConstantDuration(void)
{
return _TConstantDuration;
}
uint64_t PCA9685_servo::getSConstantPeriod(void)
{
return _SConstantPeriod;
}
void PCA9685_servo::setMinAngle(int8_t angle)
{
if (_currentAngle < angle)
{
_currentAngle = angle;
setPosition(_currentAngle);
}
_minAngle = angle;
return;
}
void PCA9685_servo::setMidAngle(int8_t angle)
{
if ((angle >= _minAngle) && (angle <= _maxAngle))
{
_midAngle = angle;
}
return;
}
void PCA9685_servo::setMaxAngle(int8_t angle)
{
if (_currentAngle > angle)
{
_currentAngle = angle;
setPosition(_currentAngle);
}
_maxAngle = angle;
return;
}
/// @brief Set movement type.
/// @param mode mode type {MODE_FAST, MODE_TCONSTANT, MODE_SCONSTANT}
void PCA9685_servo::setMode(uint8_t mode)
{
_mode = mode;
return;
}
/// @brief Set duration of movement in MODE_TCONSTANT mode, takes effect only after setPosition() is called.
/// @param TConstantDuration duration of movement in microseconds
void PCA9685_servo::setTConstantDuration(uint64_t TConstantDuration)
{
_TConstantDuration = TConstantDuration;
return;
}
/// @brief Set interval time between each step (changing servo angle) in MODE_SCONSTANT mode, takes effect only after setPosition() is called.
/// @param SConstantPeriod interval time in microseconds
void PCA9685_servo::setSConstantPeriod(uint64_t SConstantPeriod)
{
_SConstantPeriod = SConstantPeriod;
return;
}
/// @brief Set the pin number on the PCA9685 the servo is connected to.
/// @param address pin number (0-15)
void PCA9685_servo::setAddress(uint16_t address)
{
_address = address;
return;
}
/// @brief Changes the speed of current movement, only use if you want to change the speed in MODE_TCONSTANT during the movement.
/// Otherwise use setTConstantDuration
/// @param TConstantDuration is the duration of the movement in microseconds
void PCA9685_servo::changeSpeedConstT(uint64_t TConstantDuration)
{
// _targetAngle = Angle; // where we wish to end up
uint64_t AngleDelta = abs(_targetAngle - _currentAngle); // how many degrees we need to move
_TConstantDuration = TConstantDuration;
// how long between 1 degree movements?
_TConstantPeriod = _TConstantDuration / AngleDelta;
}
/// @brief Changes the angular velocity used in MODE_SCONSTANT and MODE_TCONSTANT.
/// @param ang_vel is the angular velocity in degrees/s
void PCA9685_servo::setAngularVelocity(double angVel)
{
double time_interval_s = 1 / angVel;
_SConstantPeriod = time_interval_s * 1e6;
_TConstantPeriod = _SConstantPeriod;
}
/// @brief Stop the servo.
void PCA9685_servo::stop()
{
_targetAngle = _currentAngle;
}