source: arduino-1-6-7/trunk/fuentes/arduino-ide-amd64/libraries/Servo/src/avr/Servo.cpp @ 46

Last change on this file since 46 was 46, checked in by jrpelegrina, 4 years ago

First release to Xenial

File size: 10.6 KB
Line 
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
35static servo_t servos[MAX_SERVOS];                          // static array of servo structures
36static volatile int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed for each timer (or -1 if refresh interval)
37
38uint8_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
52static 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)
80SIGNAL (TIMER1_COMPA_vect)
81{
82  handle_interrupts(_timer1, &TCNT1, &OCR1A);
83}
84#endif
85
86#if defined(_useTimer3)
87SIGNAL (TIMER3_COMPA_vect)
88{
89  handle_interrupts(_timer3, &TCNT3, &OCR3A);
90}
91#endif
92
93#if defined(_useTimer4)
94SIGNAL (TIMER4_COMPA_vect)
95{
96  handle_interrupts(_timer4, &TCNT4, &OCR4A);
97}
98#endif
99
100#if defined(_useTimer5)
101SIGNAL (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)
110void Timer1Service()
111{
112  handle_interrupts(_timer1, &TCNT1, &OCR1A);
113}
114#endif
115#if defined(_useTimer3)
116void Timer3Service()
117{
118  handle_interrupts(_timer3, &TCNT3, &OCR3A);
119}
120#endif
121#endif
122
123
124static 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
184static 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
209static 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
222Servo::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
232uint8_t Servo::attach(int pin)
233{
234  return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
235}
236
237uint8_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
254void 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
263void 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
274void 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
295int Servo::read() // return the value as degrees
296{
297  return  map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
298}
299
300int 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
311bool Servo::attached()
312{
313  return servos[this->servoIndex].Pin.isActive ;
314}
315
316#endif // ARDUINO_ARCH_AVR
317
Note: See TracBrowser for help on using the repository browser.