D:/DRISSI/arduino-0022/arduino-0022/libraries/Servo/Servo.cpp
00001 /*
00002  Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
00003  Copyright (c) 2009 Michael Margolis.  All right reserved.
00004  
00005  This library is free software; you can redistribute it and/or
00006  modify it under the terms of the GNU Lesser General Public
00007  License as published by the Free Software Foundation; either
00008  version 2.1 of the License, or (at your option) any later version.
00009  
00010  This library is distributed in the hope that it will be useful,
00011  but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013  Lesser General Public License for more details.
00014  
00015  You should have received a copy of the GNU Lesser General Public
00016  License along with this library; if not, write to the Free Software
00017  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00018  */
00019 
00020 /* 
00021  
00022  A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
00023  The servos are pulsed in the background using the value most recently written using the write() method
00024  
00025  Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
00026  Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
00027  
00028  The methods are:
00029  
00030  Servo - Class for manipulating servo motors connected to Arduino pins.
00031  
00032  attach(pin )  - Attaches a servo motor to an i/o pin.
00033  attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds
00034  default min is 544, max is 2400  
00035  
00036  write()     - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)
00037  writeMicroseconds() - Sets the servo pulse width in microseconds 
00038  read()      - Gets the last written servo pulse width as an angle between 0 and 180. 
00039  readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
00040  attached()  - Returns true if there is a servo attached. 
00041  detach()    - Stops an attached servos from pulsing its i/o pin. 
00042  
00043 */
00044 
00045 #include <avr/interrupt.h>
00046 #include <WProgram.h> 
00047 
00048 #include "Servo.h"
00049 
00050 #define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds to tick (assumes prescale of 8)  // 12 Aug 2009
00051 #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
00052 
00053 
00054 #define TRIM_DURATION       2                               // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
00055 
00056 //#define NBR_TIMERS        (MAX_SERVOS / SERVOS_PER_TIMER)
00057 
00058 static servo_t servos[MAX_SERVOS];                          // static array of servo structures
00059 static volatile int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed for each timer (or -1 if refresh interval)
00060 
00061 uint8_t ServoCount = 0;                                     // the total number of attached servos
00062 
00063 
00064 // convenience macros
00065 #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
00066 #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this timer
00067 #define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)     // macro to access servo index by timer and channel
00068 #define SERVO(_timer,_channel)  (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class by timer and channel
00069 
00070 #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
00071 #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo 
00072 
00073 /************ static functions common to all instances ***********************/
00074 
00075 static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
00076 {
00077   if( Channel[timer] < 0 )
00078     *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer 
00079   else{
00080     if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )  
00081       digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated   
00082   }
00083 
00084   Channel[timer]++;    // increment to the next channel
00085   if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
00086     *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
00087     if(SERVO(timer,Channel[timer]).Pin.isActive == true)     // check if activated
00088       digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high   
00089   }  
00090   else { 
00091     // finished all channels so wait for the refresh period to expire before starting over 
00092     if( (unsigned)*TCNTn <  (usToTicks(REFRESH_INTERVAL) + 4) )  // allow a few ticks to ensure the next OCR1A not missed
00093       *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);  
00094     else 
00095       *OCRnA = *TCNTn + 4;  // at least REFRESH_INTERVAL has elapsed
00096     Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
00097   }
00098 }
00099 
00100 #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
00101 // Interrupt handlers for Arduino 
00102 #if defined(_useTimer1)
00103 SIGNAL (TIMER1_COMPA_vect) 
00104 { 
00105   handle_interrupts(_timer1, &TCNT1, &OCR1A); 
00106 }
00107 #endif
00108 
00109 #if defined(_useTimer3)
00110 SIGNAL (TIMER3_COMPA_vect) 
00111 { 
00112   handle_interrupts(_timer3, &TCNT3, &OCR3A); 
00113 }
00114 #endif
00115 
00116 #if defined(_useTimer4)
00117 SIGNAL (TIMER4_COMPA_vect) 
00118 {
00119   handle_interrupts(_timer4, &TCNT4, &OCR4A); 
00120 }
00121 #endif
00122 
00123 #if defined(_useTimer5)
00124 SIGNAL (TIMER5_COMPA_vect) 
00125 {
00126   handle_interrupts(_timer5, &TCNT5, &OCR5A); 
00127 }
00128 #endif
00129 
00130 #elif defined WIRING
00131 // Interrupt handlers for Wiring 
00132 #if defined(_useTimer1)
00133 void Timer1Service() 
00134 { 
00135   handle_interrupts(_timer1, &TCNT1, &OCR1A); 
00136 }
00137 #endif
00138 #if defined(_useTimer3)
00139 void Timer3Service() 
00140 { 
00141   handle_interrupts(_timer3, &TCNT3, &OCR3A); 
00142 }
00143 #endif
00144 #endif
00145 
00146 
00147 static void initISR(timer16_Sequence_t timer)
00148 {  
00149 #if defined (_useTimer1)
00150   if(timer == _timer1) {
00151     TCCR1A = 0;             // normal counting mode 
00152     TCCR1B = _BV(CS11);     // set prescaler of 8 
00153     TCNT1 = 0;              // clear the timer count 
00154 #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
00155     TIFR |= _BV(OCF1A);      // clear any pending interrupts; 
00156     TIMSK |=  _BV(OCIE1A) ;  // enable the output compare interrupt  
00157 #else
00158     // here if not ATmega8 or ATmega128
00159     TIFR1 |= _BV(OCF1A);     // clear any pending interrupts; 
00160     TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt 
00161 #endif    
00162 #if defined(WIRING)       
00163     timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service); 
00164 #endif  
00165   } 
00166 #endif  
00167 
00168 #if defined (_useTimer3)
00169   if(timer == _timer3) {
00170     TCCR3A = 0;             // normal counting mode 
00171     TCCR3B = _BV(CS31);     // set prescaler of 8  
00172     TCNT3 = 0;              // clear the timer count 
00173 #if defined(__AVR_ATmega128__)
00174     TIFR |= _BV(OCF3A);     // clear any pending interrupts;   
00175         ETIMSK |= _BV(OCIE3A);  // enable the output compare interrupt     
00176 #else  
00177     TIFR3 = _BV(OCF3A);     // clear any pending interrupts; 
00178     TIMSK3 =  _BV(OCIE3A) ; // enable the output compare interrupt      
00179 #endif
00180 #if defined(WIRING)    
00181     timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service);  // for Wiring platform only     
00182 #endif  
00183   }
00184 #endif
00185 
00186 #if defined (_useTimer4)
00187   if(timer == _timer4) {
00188     TCCR4A = 0;             // normal counting mode 
00189     TCCR4B = _BV(CS41);     // set prescaler of 8  
00190     TCNT4 = 0;              // clear the timer count 
00191     TIFR4 = _BV(OCF4A);     // clear any pending interrupts; 
00192     TIMSK4 =  _BV(OCIE4A) ; // enable the output compare interrupt
00193   }    
00194 #endif
00195 
00196 #if defined (_useTimer5)
00197   if(timer == _timer5) {
00198     TCCR5A = 0;             // normal counting mode 
00199     TCCR5B = _BV(CS51);     // set prescaler of 8  
00200     TCNT5 = 0;              // clear the timer count 
00201     TIFR5 = _BV(OCF5A);     // clear any pending interrupts; 
00202     TIMSK5 =  _BV(OCIE5A) ; // enable the output compare interrupt      
00203   }
00204 #endif
00205 } 
00206 
00207 static void finISR(timer16_Sequence_t timer)
00208 {
00209     //disable use of the given timer
00210 #if defined WIRING   // Wiring
00211   if(timer == _timer1) {
00212     #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
00213     TIMSK1 &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt
00214     #else 
00215     TIMSK &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt   
00216     #endif
00217     timerDetach(TIMER1OUTCOMPAREA_INT); 
00218   }
00219   else if(timer == _timer3) {     
00220     #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
00221     TIMSK3 &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
00222     #else
00223     ETIMSK &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
00224     #endif
00225     timerDetach(TIMER3OUTCOMPAREA_INT);
00226   }
00227 #else
00228     //For arduino - in future: call here to a currently undefined function to reset the timer
00229 #endif
00230 }
00231 
00232 static boolean isTimerActive(timer16_Sequence_t timer)
00233 {
00234   // returns true if any servo is active on this timer
00235   for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
00236     if(SERVO(timer,channel).Pin.isActive == true)
00237       return true;
00238   }
00239   return false;
00240 }
00241 
00242 
00243 /****************** end of static functions ******************************/
00244 
00245 Servo::Servo()
00246 {
00247   if( ServoCount < MAX_SERVOS) {
00248     this->servoIndex = ServoCount++;                    // assign a servo index to this instance
00249         servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values  - 12 Aug 2009
00250   }
00251   else
00252     this->servoIndex = INVALID_SERVO ;  // too many servos 
00253 }
00254 
00255 uint8_t Servo::attach(int pin)
00256 {
00257   return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
00258 }
00259 
00260 uint8_t Servo::attach(int pin, int min, int max)
00261 {
00262   if(this->servoIndex < MAX_SERVOS ) {
00263     pinMode( pin, OUTPUT) ;                                   // set servo pin to output
00264     servos[this->servoIndex].Pin.nbr = pin;  
00265     // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 
00266     this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
00267     this->max  = (MAX_PULSE_WIDTH - max)/4; 
00268     // initialize the timer if it has not already been initialized 
00269     timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
00270     if(isTimerActive(timer) == false)
00271       initISR(timer);    
00272     servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
00273   } 
00274   return this->servoIndex ;
00275 }
00276 
00277 void Servo::detach()  
00278 {
00279   servos[this->servoIndex].Pin.isActive = false;  
00280   timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
00281   if(isTimerActive(timer) == false) {
00282     finISR(timer);
00283   }
00284 }
00285 
00286 void Servo::write(int value)
00287 {  
00288   if(value < MIN_PULSE_WIDTH)
00289   {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
00290     if(value < 0) value = 0;
00291     if(value > 180) value = 180;
00292     value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());      
00293   }
00294   this->writeMicroseconds(value);
00295 }
00296 
00297 void Servo::writeMicroseconds(int value)
00298 {
00299   // calculate and store the values for the given channel
00300   byte channel = this->servoIndex;
00301   if( (channel >= 0) && (channel < MAX_SERVOS) )   // ensure channel is valid
00302   {  
00303     if( value < SERVO_MIN() )          // ensure pulse width is valid
00304       value = SERVO_MIN();
00305     else if( value > SERVO_MAX() )
00306       value = SERVO_MAX();   
00307     
00308         value = value - TRIM_DURATION;
00309     value = usToTicks(value);  // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
00310 
00311     uint8_t oldSREG = SREG;
00312     cli();
00313     servos[channel].ticks = value;  
00314     SREG = oldSREG;   
00315   } 
00316 }
00317 
00318 int Servo::read() // return the value as degrees
00319 {
00320   return  map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);     
00321 }
00322 
00323 int Servo::readMicroseconds()
00324 {
00325   unsigned int pulsewidth;
00326   if( this->servoIndex != INVALID_SERVO )
00327     pulsewidth = ticksToUs(servos[this->servoIndex].ticks)  + TRIM_DURATION ;   // 12 aug 2009
00328   else 
00329     pulsewidth  = 0;
00330 
00331   return pulsewidth;   
00332 }
00333 
00334 bool Servo::attached()
00335 {
00336   return servos[this->servoIndex].Pin.isActive ;
00337 }