-
Notifications
You must be signed in to change notification settings - Fork 76
/
Servo.cpp
295 lines (255 loc) · 11 KB
/
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
//************************************************************************
/*
Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers
Copyright (c) 2009 Michael Margolis. All right reserved.
Revision date: 08/18/2011(Michelle Yu)
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
//************************************************************************
/*
A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
The servos are pulsed in the background using the value most recently written using the write() method
Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
Timers are seized as needed in groups of 8 servos - 16 servos use two timers, 24 servos will use three.
For the PIC32, the three timers that are used are (in order):
Timer4 (for 1 through 8 servos)
Timer5 (for 9 through 16 servos)
Timer3 (for 17 through 24 servos)
Be careful that other libraries do not use any of the timers that you need for your servos.
The methods are:
Servo - Class for manipulating servo motors connected to Arduino pins.
attach(pin ) - Attaches a servo motor to an i/o pin.
attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds
default min is 544, max is 2400
write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds)
writeMicroseconds() - Sets the servo pulse width in microseconds
read() - Gets the last written servo pulse width as an angle between 0 and 179.
readMicroseconds() - Gets the last written servo pulse width in microseconds.
attached() - Returns true if there is a servo attached.
detach() - Stops an attached servos from pulsing its i/o pin.
*/
//************************************************************************
//* Edit History
//************************************************************************
//* Aug 21, 2011 <GeneApperson> fixed floating point issue by changing to integer math
//* Sep 1, 2011 <MLS> Cleaning up formatting
//* Sep 1, 2011 <MLS> Cleaning up formatting
//* Sep 1, 2011 <MLS> issue #112, changed assigment to compare in finISR
//* Sep 5, 2011 <GeneApperson> added include of plib.h to fix compile errors
//* introduced when plib.h was removed from HardwareSerial.h
//* Sep 18, 2012 <BPS> Fixed math problem with refresh interval, and
//* expanded nbr to 7 bits for pins above 63 (MAX32 has 83)
//* And adjusted trim to 1 tick (measured and adjusted)
//* Feb 7, 2013 <GeneApperson> Removed dependency on Microchip plib library
//************************************************************************
#include "Servo.h"
extern "C"{
#include "int.h"
}
#define usToTicks(_us) (((_us)*5)/4) // converts microseconds to tick
#define ticksToUs(_ticks) ((((unsigned)(_ticks))*4)/5) // converts from ticks back to microseconds
#define TRIM_DURATION 1 // compensation ticks to trim adjust for digitalWrite delays // 12 August 200
static servo_t servos[MAX_SERVOS]; // static array of servo structures
int channel[3]; // channel for the current servo
uint8_t ServoCount = 0; // the total number of attached servos
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((int)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
#define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
/************ static functions common to all instances ***********************/
//************************************************************************
// Note: PIC32 timers (TMRn) reset to zero when they match PRn.
void handle_interrupts(int timer, volatile unsigned int *TMRn, volatile unsigned int *PRn)
{
static uint32_t AccumulatedTicks[3] = {0,0,0}; // Store the number of ticks since the first rising edge for this timer
// Test for invalid timer number
if (timer >= 3)
{
return;
}
// If this value is -1, then we have just finished with the time from the end of the last pulse
// and need to start with the first servo index on this timer again.
if ( channel[timer] < 0 )
{
AccumulatedTicks[timer] = 0; // Clear the accumulated time for this timer on first rising edge
}
else
{
// If this is not the first pulse, then set the old pin low
if ( SERVO_INDEX(timer,channel[timer]) < ServoCount && SERVO(timer,channel[timer]).Pin.isActive == true )
{
digitalWrite( SERVO(timer,channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
}
channel[timer]++; // increment to the next channel
// If we have not run out of channels (on this timer),
if ( SERVO_INDEX(timer,channel[timer]) < ServoCount && channel[timer] < SERVOS_PER_TIMER)
{
// Then set the time we want to fire next (the width for this channel)
*PRn = SERVO(timer,channel[timer]).ticks;
// Set this channel's pin high if its active
if (SERVO(timer,channel[timer]).Pin.isActive == true) // check if activated
{
digitalWrite( SERVO(timer,channel[timer]).Pin.nbr, HIGH); // its an active channel so pulse it high
}
AccumulatedTicks[timer] += *PRn; // Add the time we are about to spend on this channel
}
else
{
// Otherwise, finished all channels so next fire needs to be at REFRESH_INTERVAL - AccumulatedTicks
if (AccumulatedTicks[timer] < (usToTicks(REFRESH_INTERVAL) - 4))
{
*PRn = (unsigned int)(usToTicks(REFRESH_INTERVAL)) - AccumulatedTicks[timer];
}
else
{
*PRn = *TMRn + 4; // at least REFRESH_INTERVAL has elapsed
}
channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
}
//************************************************************************
static void finISR(int timer)
{
//disable use of the given timer
if (timer == TIMER3)
{
IEC0bits.T3IE = 0; // disable T3 interrupt
}
if (timer == TIMER4)
{
IEC0bits.T4IE = 0; // disable T4 interrupt
}
if (timer == TIMER5)
{
IEC0bits.T5IE = 0; // disable T5 interrupt
}
}
//************************************************************************
static boolean isTimerActive(int timer)
{
// returns true if any servo is active on this timer
for (uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++)
{
if (SERVO(timer,channel).Pin.isActive == true)
return true;
}
return false;
}
/****************** end of static functions ******************************/
Servo::Servo()
{
if (ServoCount < MAX_SERVOS)
{
/// TODO: Really, we need to search through servos[] for the first one
/// that is not being used, rather than always use ServoCount as the
/// index. What happens if you create 24 servos, then destroy one
/// and create another. Right now that would fail, even though you
/// weren't actively using 24 servos.
this->servoIndex = ServoCount++; // assign a servo index to this instance
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default to neutral position
}
else
this->servoIndex = INVALID_SERVO ; // too many servos
}
//************************************************************************
uint8_t Servo::attach(int pin)
{
return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
}
//************************************************************************
uint8_t Servo::attach(int pin, int min, int max)
{
if (this->servoIndex < ServoCount)
{
pinMode(pin, OUTPUT); // set servo pin to output
servos[this->servoIndex].Pin.nbr = pin;
this->min = (MIN_PULSE_WIDTH - min)/4; // resolution of min/max is 4 uS
this->max = (MAX_PULSE_WIDTH - max)/4;
// initialize the timer if it has not already been initialized
int timer = SERVO_INDEX_TO_TIMER(this->servoIndex);
if (isTimerActive(timer) == false)
{
initISR(timer);
}
servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
return this->servoIndex;
}
else
{
// return bogus value if this->ServoIndex is invalid
return(INVALID_SERVO);
}
}
//************************************************************************
void Servo::detach()
{
servos[this->servoIndex].Pin.isActive = false;
int timer = SERVO_INDEX_TO_TIMER(servoIndex);
if (isTimerActive(timer) == false)
{
finISR(timer);
}
}
//************************************************************************
void Servo::write(int value)
{
if (value < MIN_PULSE_WIDTH)
{
// treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
if (value < 0) value = 0;
if (value > 179) value = 179;
value = map(value, 0, 179, SERVO_MIN(), SERVO_MAX());
}
this->writeMicroseconds(value);
}
//************************************************************************
void Servo::writeMicroseconds(int value)
{
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if ( (channel >= 0) && (channel < MAX_SERVOS) ) // ensure channel is valid
{
if ( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if ( value > SERVO_MAX() )
value = SERVO_MAX();
value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead
unsigned int status;
status = disableInterrupts();
servos[channel].ticks = value;
restoreInterrupts(status);
}
}
//************************************************************************
int Servo::read() // return the value as degrees
{
return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
}
//************************************************************************
int Servo::readMicroseconds()
{
unsigned int pulsewidth;
if ( this->servoIndex != INVALID_SERVO )
pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009
else
pulsewidth = 0;
return pulsewidth;
}
//************************************************************************
bool Servo::attached()
{
return servos[this->servoIndex].Pin.isActive ;
}