Statistics
| Branch: | Revision:

root / arduino-1.0 / libraries / Servo / Servo.cpp @ 58d82c77

History | View | Annotate | Download (12.2 KB)

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
/* 
21
 
22
 A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
23
 The servos are pulsed in the background using the value most recently written using the write() method
24
 
25
 Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
26
 Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
27
 
28
 The methods are:
29
 
30
 Servo - Class for manipulating servo motors connected to Arduino pins.
31
 
32
 attach(pin )  - Attaches a servo motor to an i/o pin.
33
 attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds
34
 default min is 544, max is 2400  
35
 
36
 write()     - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)
37
 writeMicroseconds() - Sets the servo pulse width in microseconds 
38
 read()      - Gets the last written servo pulse width as an angle between 0 and 180. 
39
 readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
40
 attached()  - Returns true if there is a servo attached. 
41
 detach()    - Stops an attached servos from pulsing its i/o pin. 
42
 
43
*/
44

    
45
#include <avr/interrupt.h>
46
#include <Arduino.h> 
47

    
48
#include "Servo.h"
49

    
50
#define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds to tick (assumes prescale of 8)  // 12 Aug 2009
51
#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
52

    
53

    
54
#define TRIM_DURATION       2                               // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
55

    
56
//#define NBR_TIMERS        (MAX_SERVOS / SERVOS_PER_TIMER)
57

    
58
static servo_t servos[MAX_SERVOS];                          // static array of servo structures
59
static volatile int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed for each timer (or -1 if refresh interval)
60

    
61
uint8_t ServoCount = 0;                                     // the total number of attached servos
62

    
63

    
64
// convenience macros
65
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
66
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this timer
67
#define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)     // macro to access servo index by timer and channel
68
#define SERVO(_timer,_channel)  (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class by timer and channel
69

    
70
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
71
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo 
72

    
73
/************ static functions common to all instances ***********************/
74

    
75
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
76
{
77
  if( Channel[timer] < 0 )
78
    *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer 
79
  else{
80
    if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )  
81
      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated   
82
  }
83

    
84
  Channel[timer]++;    // increment to the next channel
85
  if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
86
    *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
87
    if(SERVO(timer,Channel[timer]).Pin.isActive == true)     // check if activated
88
      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high   
89
  }  
90
  else { 
91
    // finished all channels so wait for the refresh period to expire before starting over 
92
    if( (unsigned)*TCNTn <  (usToTicks(REFRESH_INTERVAL) + 4) )  // allow a few ticks to ensure the next OCR1A not missed
93
      *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);  
94
    else 
95
      *OCRnA = *TCNTn + 4;  // at least REFRESH_INTERVAL has elapsed
96
    Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
97
  }
98
}
99

    
100
#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
101
// Interrupt handlers for Arduino 
102
#if defined(_useTimer1)
103
SIGNAL (TIMER1_COMPA_vect) 
104
{ 
105
  handle_interrupts(_timer1, &TCNT1, &OCR1A); 
106
}
107
#endif
108

    
109
#if defined(_useTimer3)
110
SIGNAL (TIMER3_COMPA_vect) 
111
{ 
112
  handle_interrupts(_timer3, &TCNT3, &OCR3A); 
113
}
114
#endif
115

    
116
#if defined(_useTimer4)
117
SIGNAL (TIMER4_COMPA_vect) 
118
{
119
  handle_interrupts(_timer4, &TCNT4, &OCR4A); 
120
}
121
#endif
122

    
123
#if defined(_useTimer5)
124
SIGNAL (TIMER5_COMPA_vect) 
125
{
126
  handle_interrupts(_timer5, &TCNT5, &OCR5A); 
127
}
128
#endif
129

    
130
#elif defined WIRING
131
// Interrupt handlers for Wiring 
132
#if defined(_useTimer1)
133
void Timer1Service() 
134
{ 
135
  handle_interrupts(_timer1, &TCNT1, &OCR1A); 
136
}
137
#endif
138
#if defined(_useTimer3)
139
void Timer3Service() 
140
{ 
141
  handle_interrupts(_timer3, &TCNT3, &OCR3A); 
142
}
143
#endif
144
#endif
145

    
146

    
147
static void initISR(timer16_Sequence_t timer)
148
{  
149
#if defined (_useTimer1)
150
  if(timer == _timer1) {
151
    TCCR1A = 0;             // normal counting mode 
152
    TCCR1B = _BV(CS11);     // set prescaler of 8 
153
    TCNT1 = 0;              // clear the timer count 
154
#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
155
    TIFR |= _BV(OCF1A);      // clear any pending interrupts; 
156
    TIMSK |=  _BV(OCIE1A) ;  // enable the output compare interrupt  
157
#else
158
    // here if not ATmega8 or ATmega128
159
    TIFR1 |= _BV(OCF1A);     // clear any pending interrupts; 
160
    TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt 
161
#endif    
162
#if defined(WIRING)       
163
    timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service); 
164
#endif        
165
  } 
166
#endif  
167

    
168
#if defined (_useTimer3)
169
  if(timer == _timer3) {
170
    TCCR3A = 0;             // normal counting mode 
171
    TCCR3B = _BV(CS31);     // set prescaler of 8  
172
    TCNT3 = 0;              // clear the timer count 
173
#if defined(__AVR_ATmega128__)
174
    TIFR |= _BV(OCF3A);     // clear any pending interrupts;   
175
        ETIMSK |= _BV(OCIE3A);  // enable the output compare interrupt     
176
#else  
177
    TIFR3 = _BV(OCF3A);     // clear any pending interrupts; 
178
    TIMSK3 =  _BV(OCIE3A) ; // enable the output compare interrupt      
179
#endif
180
#if defined(WIRING)    
181
    timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service);  // for Wiring platform only        
182
#endif  
183
  }
184
#endif
185

    
186
#if defined (_useTimer4)
187
  if(timer == _timer4) {
188
    TCCR4A = 0;             // normal counting mode 
189
    TCCR4B = _BV(CS41);     // set prescaler of 8  
190
    TCNT4 = 0;              // clear the timer count 
191
    TIFR4 = _BV(OCF4A);     // clear any pending interrupts; 
192
    TIMSK4 =  _BV(OCIE4A) ; // enable the output compare interrupt
193
  }    
194
#endif
195

    
196
#if defined (_useTimer5)
197
  if(timer == _timer5) {
198
    TCCR5A = 0;             // normal counting mode 
199
    TCCR5B = _BV(CS51);     // set prescaler of 8  
200
    TCNT5 = 0;              // clear the timer count 
201
    TIFR5 = _BV(OCF5A);     // clear any pending interrupts; 
202
    TIMSK5 =  _BV(OCIE5A) ; // enable the output compare interrupt      
203
  }
204
#endif
205
} 
206

    
207
static void finISR(timer16_Sequence_t timer)
208
{
209
    //disable use of the given timer
210
#if defined WIRING   // Wiring
211
  if(timer == _timer1) {
212
    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
213
    TIMSK1 &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt
214
    #else 
215
    TIMSK &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt   
216
    #endif
217
    timerDetach(TIMER1OUTCOMPAREA_INT); 
218
  }
219
  else if(timer == _timer3) {     
220
    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
221
    TIMSK3 &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
222
    #else
223
    ETIMSK &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
224
    #endif
225
    timerDetach(TIMER3OUTCOMPAREA_INT);
226
  }
227
#else
228
    //For arduino - in future: call here to a currently undefined function to reset the timer
229
#endif
230
}
231

    
232
static boolean isTimerActive(timer16_Sequence_t timer)
233
{
234
  // returns true if any servo is active on this timer
235
  for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
236
    if(SERVO(timer,channel).Pin.isActive == true)
237
      return true;
238
  }
239
  return false;
240
}
241

    
242

    
243
/****************** end of static functions ******************************/
244

    
245
Servo::Servo()
246
{
247
  if( ServoCount < MAX_SERVOS) {
248
    this->servoIndex = ServoCount++;                    // assign a servo index to this instance
249
        servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values  - 12 Aug 2009
250
  }
251
  else
252
    this->servoIndex = INVALID_SERVO ;  // too many servos 
253
}
254

    
255
uint8_t Servo::attach(int pin)
256
{
257
  return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
258
}
259

    
260
uint8_t Servo::attach(int pin, int min, int max)
261
{
262
  if(this->servoIndex < MAX_SERVOS ) {
263
    pinMode( pin, OUTPUT) ;                                   // set servo pin to output
264
    servos[this->servoIndex].Pin.nbr = pin;  
265
    // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 
266
    this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
267
    this->max  = (MAX_PULSE_WIDTH - max)/4; 
268
    // initialize the timer if it has not already been initialized 
269
    timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
270
    if(isTimerActive(timer) == false)
271
      initISR(timer);    
272
    servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
273
  } 
274
  return this->servoIndex ;
275
}
276

    
277
void Servo::detach()  
278
{
279
  servos[this->servoIndex].Pin.isActive = false;  
280
  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
281
  if(isTimerActive(timer) == false) {
282
    finISR(timer);
283
  }
284
}
285

    
286
void Servo::write(int value)
287
{  
288
  if(value < MIN_PULSE_WIDTH)
289
  {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
290
    if(value < 0) value = 0;
291
    if(value > 180) value = 180;
292
    value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());      
293
  }
294
  this->writeMicroseconds(value);
295
}
296

    
297
void Servo::writeMicroseconds(int value)
298
{
299
  // calculate and store the values for the given channel
300
  byte channel = this->servoIndex;
301
  if( (channel >= 0) && (channel < MAX_SERVOS) )   // ensure channel is valid
302
  {  
303
    if( value < SERVO_MIN() )          // ensure pulse width is valid
304
      value = SERVO_MIN();
305
    else if( value > SERVO_MAX() )
306
      value = SERVO_MAX();   
307
    
308
          value = value - TRIM_DURATION;
309
    value = usToTicks(value);  // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
310

    
311
    uint8_t oldSREG = SREG;
312
    cli();
313
    servos[channel].ticks = value;  
314
    SREG = oldSREG;   
315
  } 
316
}
317

    
318
int Servo::read() // return the value as degrees
319
{
320
  return  map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);     
321
}
322

    
323
int Servo::readMicroseconds()
324
{
325
  unsigned int pulsewidth;
326
  if( this->servoIndex != INVALID_SERVO )
327
    pulsewidth = ticksToUs(servos[this->servoIndex].ticks)  + TRIM_DURATION ;   // 12 aug 2009
328
  else 
329
    pulsewidth  = 0;
330

    
331
  return pulsewidth;   
332
}
333

    
334
bool Servo::attached()
335
{
336
  return servos[this->servoIndex].Pin.isActive ;
337
}