1 | /*
|
---|
2 | Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
|
---|
3 | Copyright (c) 2009 Michael Margolis. All right reserved.
|
---|
4 |
|
---|
5 | This library is free software; you can redistribute it and/or
|
---|
6 | modify it under the terms of the GNU Lesser General Public
|
---|
7 | License as published by the Free Software Foundation; either
|
---|
8 | version 2.1 of the License, or (at your option) any later version.
|
---|
9 |
|
---|
10 | This library is distributed in the hope that it will be useful,
|
---|
11 | but WITHOUT ANY WARRANTY; without even the implied warranty of
|
---|
12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
---|
13 | Lesser General Public License for more details.
|
---|
14 |
|
---|
15 | You should have received a copy of the GNU Lesser General Public
|
---|
16 | License along with this library; if not, write to the Free Software
|
---|
17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
---|
18 | */
|
---|
19 |
|
---|
20 | #if defined(ARDUINO_ARCH_AVR)
|
---|
21 |
|
---|
22 | #include <avr/interrupt.h>
|
---|
23 | #include <Arduino.h>
|
---|
24 |
|
---|
25 | #include "Servo.h"
|
---|
26 |
|
---|
27 | #define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009
|
---|
28 | #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
|
---|
29 |
|
---|
30 |
|
---|
31 | #define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
|
---|
32 |
|
---|
33 | //#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER)
|
---|
34 |
|
---|
35 | static servo_t servos[MAX_SERVOS]; // static array of servo structures
|
---|
36 | static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
|
---|
37 |
|
---|
38 | uint8_t ServoCount = 0; // the total number of attached servos
|
---|
39 |
|
---|
40 |
|
---|
41 | // convenience macros
|
---|
42 | #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
|
---|
43 | #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
|
---|
44 | #define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
|
---|
45 | #define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
|
---|
46 |
|
---|
47 | #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
|
---|
48 | #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
|
---|
49 |
|
---|
50 | /************ static functions common to all instances ***********************/
|
---|
51 |
|
---|
52 | static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
|
---|
53 | {
|
---|
54 | if( Channel[timer] < 0 )
|
---|
55 | *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
|
---|
56 | else{
|
---|
57 | if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
|
---|
58 | digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
|
---|
59 | }
|
---|
60 |
|
---|
61 | Channel[timer]++; // increment to the next channel
|
---|
62 | if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
|
---|
63 | *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
|
---|
64 | if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated
|
---|
65 | digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
|
---|
66 | }
|
---|
67 | else {
|
---|
68 | // finished all channels so wait for the refresh period to expire before starting over
|
---|
69 | if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
|
---|
70 | *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
|
---|
71 | else
|
---|
72 | *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
|
---|
73 | Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
|
---|
74 | }
|
---|
75 | }
|
---|
76 |
|
---|
77 | #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
|
---|
78 | // Interrupt handlers for Arduino
|
---|
79 | #if defined(_useTimer1)
|
---|
80 | SIGNAL (TIMER1_COMPA_vect)
|
---|
81 | {
|
---|
82 | handle_interrupts(_timer1, &TCNT1, &OCR1A);
|
---|
83 | }
|
---|
84 | #endif
|
---|
85 |
|
---|
86 | #if defined(_useTimer3)
|
---|
87 | SIGNAL (TIMER3_COMPA_vect)
|
---|
88 | {
|
---|
89 | handle_interrupts(_timer3, &TCNT3, &OCR3A);
|
---|
90 | }
|
---|
91 | #endif
|
---|
92 |
|
---|
93 | #if defined(_useTimer4)
|
---|
94 | SIGNAL (TIMER4_COMPA_vect)
|
---|
95 | {
|
---|
96 | handle_interrupts(_timer4, &TCNT4, &OCR4A);
|
---|
97 | }
|
---|
98 | #endif
|
---|
99 |
|
---|
100 | #if defined(_useTimer5)
|
---|
101 | SIGNAL (TIMER5_COMPA_vect)
|
---|
102 | {
|
---|
103 | handle_interrupts(_timer5, &TCNT5, &OCR5A);
|
---|
104 | }
|
---|
105 | #endif
|
---|
106 |
|
---|
107 | #elif defined WIRING
|
---|
108 | // Interrupt handlers for Wiring
|
---|
109 | #if defined(_useTimer1)
|
---|
110 | void Timer1Service()
|
---|
111 | {
|
---|
112 | handle_interrupts(_timer1, &TCNT1, &OCR1A);
|
---|
113 | }
|
---|
114 | #endif
|
---|
115 | #if defined(_useTimer3)
|
---|
116 | void Timer3Service()
|
---|
117 | {
|
---|
118 | handle_interrupts(_timer3, &TCNT3, &OCR3A);
|
---|
119 | }
|
---|
120 | #endif
|
---|
121 | #endif
|
---|
122 |
|
---|
123 |
|
---|
124 | static void initISR(timer16_Sequence_t timer)
|
---|
125 | {
|
---|
126 | #if defined (_useTimer1)
|
---|
127 | if(timer == _timer1) {
|
---|
128 | TCCR1A = 0; // normal counting mode
|
---|
129 | TCCR1B = _BV(CS11); // set prescaler of 8
|
---|
130 | TCNT1 = 0; // clear the timer count
|
---|
131 | #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
|
---|
132 | TIFR |= _BV(OCF1A); // clear any pending interrupts;
|
---|
133 | TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt
|
---|
134 | #else
|
---|
135 | // here if not ATmega8 or ATmega128
|
---|
136 | TIFR1 |= _BV(OCF1A); // clear any pending interrupts;
|
---|
137 | TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt
|
---|
138 | #endif
|
---|
139 | #if defined(WIRING)
|
---|
140 | timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
|
---|
141 | #endif
|
---|
142 | }
|
---|
143 | #endif
|
---|
144 |
|
---|
145 | #if defined (_useTimer3)
|
---|
146 | if(timer == _timer3) {
|
---|
147 | TCCR3A = 0; // normal counting mode
|
---|
148 | TCCR3B = _BV(CS31); // set prescaler of 8
|
---|
149 | TCNT3 = 0; // clear the timer count
|
---|
150 | #if defined(__AVR_ATmega128__)
|
---|
151 | TIFR |= _BV(OCF3A); // clear any pending interrupts;
|
---|
152 | ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt
|
---|
153 | #else
|
---|
154 | TIFR3 = _BV(OCF3A); // clear any pending interrupts;
|
---|
155 | TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
|
---|
156 | #endif
|
---|
157 | #if defined(WIRING)
|
---|
158 | timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
|
---|
159 | #endif
|
---|
160 | }
|
---|
161 | #endif
|
---|
162 |
|
---|
163 | #if defined (_useTimer4)
|
---|
164 | if(timer == _timer4) {
|
---|
165 | TCCR4A = 0; // normal counting mode
|
---|
166 | TCCR4B = _BV(CS41); // set prescaler of 8
|
---|
167 | TCNT4 = 0; // clear the timer count
|
---|
168 | TIFR4 = _BV(OCF4A); // clear any pending interrupts;
|
---|
169 | TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
|
---|
170 | }
|
---|
171 | #endif
|
---|
172 |
|
---|
173 | #if defined (_useTimer5)
|
---|
174 | if(timer == _timer5) {
|
---|
175 | TCCR5A = 0; // normal counting mode
|
---|
176 | TCCR5B = _BV(CS51); // set prescaler of 8
|
---|
177 | TCNT5 = 0; // clear the timer count
|
---|
178 | TIFR5 = _BV(OCF5A); // clear any pending interrupts;
|
---|
179 | TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
|
---|
180 | }
|
---|
181 | #endif
|
---|
182 | }
|
---|
183 |
|
---|
184 | static void finISR(timer16_Sequence_t timer)
|
---|
185 | {
|
---|
186 | //disable use of the given timer
|
---|
187 | #if defined WIRING // Wiring
|
---|
188 | if(timer == _timer1) {
|
---|
189 | #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
|
---|
190 | TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
|
---|
191 | #else
|
---|
192 | TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
|
---|
193 | #endif
|
---|
194 | timerDetach(TIMER1OUTCOMPAREA_INT);
|
---|
195 | }
|
---|
196 | else if(timer == _timer3) {
|
---|
197 | #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
|
---|
198 | TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
|
---|
199 | #else
|
---|
200 | ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
|
---|
201 | #endif
|
---|
202 | timerDetach(TIMER3OUTCOMPAREA_INT);
|
---|
203 | }
|
---|
204 | #else
|
---|
205 | //For arduino - in future: call here to a currently undefined function to reset the timer
|
---|
206 | #endif
|
---|
207 | }
|
---|
208 |
|
---|
209 | static boolean isTimerActive(timer16_Sequence_t timer)
|
---|
210 | {
|
---|
211 | // returns true if any servo is active on this timer
|
---|
212 | for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
|
---|
213 | if(SERVO(timer,channel).Pin.isActive == true)
|
---|
214 | return true;
|
---|
215 | }
|
---|
216 | return false;
|
---|
217 | }
|
---|
218 |
|
---|
219 |
|
---|
220 | /****************** end of static functions ******************************/
|
---|
221 |
|
---|
222 | Servo::Servo()
|
---|
223 | {
|
---|
224 | if( ServoCount < MAX_SERVOS) {
|
---|
225 | this->servoIndex = ServoCount++; // assign a servo index to this instance
|
---|
226 | servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
|
---|
227 | }
|
---|
228 | else
|
---|
229 | this->servoIndex = INVALID_SERVO ; // too many servos
|
---|
230 | }
|
---|
231 |
|
---|
232 | uint8_t Servo::attach(int pin)
|
---|
233 | {
|
---|
234 | return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
|
---|
235 | }
|
---|
236 |
|
---|
237 | uint8_t Servo::attach(int pin, int min, int max)
|
---|
238 | {
|
---|
239 | if(this->servoIndex < MAX_SERVOS ) {
|
---|
240 | pinMode( pin, OUTPUT) ; // set servo pin to output
|
---|
241 | servos[this->servoIndex].Pin.nbr = pin;
|
---|
242 | // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
|
---|
243 | this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
|
---|
244 | this->max = (MAX_PULSE_WIDTH - max)/4;
|
---|
245 | // initialize the timer if it has not already been initialized
|
---|
246 | timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
|
---|
247 | if(isTimerActive(timer) == false)
|
---|
248 | initISR(timer);
|
---|
249 | servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
|
---|
250 | }
|
---|
251 | return this->servoIndex ;
|
---|
252 | }
|
---|
253 |
|
---|
254 | void Servo::detach()
|
---|
255 | {
|
---|
256 | servos[this->servoIndex].Pin.isActive = false;
|
---|
257 | timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
|
---|
258 | if(isTimerActive(timer) == false) {
|
---|
259 | finISR(timer);
|
---|
260 | }
|
---|
261 | }
|
---|
262 |
|
---|
263 | void Servo::write(int value)
|
---|
264 | {
|
---|
265 | if(value < MIN_PULSE_WIDTH)
|
---|
266 | { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
|
---|
267 | if(value < 0) value = 0;
|
---|
268 | if(value > 180) value = 180;
|
---|
269 | value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
|
---|
270 | }
|
---|
271 | this->writeMicroseconds(value);
|
---|
272 | }
|
---|
273 |
|
---|
274 | void Servo::writeMicroseconds(int value)
|
---|
275 | {
|
---|
276 | // calculate and store the values for the given channel
|
---|
277 | byte channel = this->servoIndex;
|
---|
278 | if( (channel < MAX_SERVOS) ) // ensure channel is valid
|
---|
279 | {
|
---|
280 | if( value < SERVO_MIN() ) // ensure pulse width is valid
|
---|
281 | value = SERVO_MIN();
|
---|
282 | else if( value > SERVO_MAX() )
|
---|
283 | value = SERVO_MAX();
|
---|
284 |
|
---|
285 | value = value - TRIM_DURATION;
|
---|
286 | value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
|
---|
287 |
|
---|
288 | uint8_t oldSREG = SREG;
|
---|
289 | cli();
|
---|
290 | servos[channel].ticks = value;
|
---|
291 | SREG = oldSREG;
|
---|
292 | }
|
---|
293 | }
|
---|
294 |
|
---|
295 | int Servo::read() // return the value as degrees
|
---|
296 | {
|
---|
297 | return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
|
---|
298 | }
|
---|
299 |
|
---|
300 | int Servo::readMicroseconds()
|
---|
301 | {
|
---|
302 | unsigned int pulsewidth;
|
---|
303 | if( this->servoIndex != INVALID_SERVO )
|
---|
304 | pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009
|
---|
305 | else
|
---|
306 | pulsewidth = 0;
|
---|
307 |
|
---|
308 | return pulsewidth;
|
---|
309 | }
|
---|
310 |
|
---|
311 | bool Servo::attached()
|
---|
312 | {
|
---|
313 | return servos[this->servoIndex].Pin.isActive ;
|
---|
314 | }
|
---|
315 |
|
---|
316 | #endif // ARDUINO_ARCH_AVR
|
---|
317 |
|
---|