D:/DRISSI/arduino-0022/arduino-0022/libraries/MegaServo/MegaServo.cpp
00001 /*
00002   MegaServo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 0.1
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   This library uses 16 bit timers to drive up to 12 servos on each timer.
00022   It uses interrupts so no explicit refresh activity is required. 
00023   The usage and method naming is similar to the Arduino servo library http://www.arduino.cc/en/Reference/Servo
00024   except that it support up to 48 servos on a Mega (12 on a standard Arduino).
00025   Also pulse widths can be in microseconds or degrees -
00026   write() treats parameters less than 200 as degrees, larger values are treated as milliseconds.
00027  
00028   
00029   A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
00030   The servos are pulsed in the background using the value most recently written using the write() method
00031 
00032   Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
00033   Timers are siezed as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
00034 
00035   The methods are:
00036 
00037    MegaServo - Class for manipulating servo motors connected to Arduino pins.
00038 
00039    attach(pin )  - Attaches a servo motor to an i/o pin.
00040    attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds
00041     default min is 544, max is 2400  
00042  
00043    write()     - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)
00044    writeMicroseconds() - Sets the servo pulse width in microseconds 
00045    read()      - Gets the last written servo pulse width as an angle between 0 and 180. 
00046    readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
00047    attached()  - Returns true if there is a servo attached. 
00048    detach()    - Stops an attached servos from pulsing its i/o pin. 
00049   
00050    March 31 2009: initial release 
00051    Jun 10 2009   : release 2 - added writeMicroseconds method, renamed read_us to readMicroseconds. Added 8MHz clock support
00052                    fixes to read method and ISR handler when less than maximum number of servo instances
00053    
00054  */
00055 #include <avr/interrupt.h>
00056 #include <WProgram.h> 
00057 
00058 
00059 #include "MegaServo.h"
00060 
00061 #define TICKS_PER_uS     (clockCyclesPerMicrosecond() / 8)  // number of timer ticks per microsecond with prescale of 8
00062 
00063 #define SERVOS_PER_TIMER   12                               // the maximum number of servos controlled by one timer 
00064 #define TRIM_DURATION     (SERVOS_PER_TIMER/2)             // compensation ticks to trim adjust for digitalWrite delays 
00065 
00066 #define NBR_TIMERS        (MAX_SERVOS / SERVOS_PER_TIMER)
00067 
00068 static servo_t servos[MAX_SERVOS];                         // static array of servo structures
00069 static volatile int8_t Channel[NBR_TIMERS];                // counter for the servo being pulsed for each timer (or -1 if refresh interval)
00070 #if defined(__AVR_ATmega1280__)
00071 typedef enum {  _timer5, _timer1, _timer3, _timer4 } servoTimer_t; // this is the sequence for timer utilization 
00072 #else
00073 typedef enum { _timer1 } servoTimer_t;                     // this is the sequence for timer utilization 
00074 #endif
00075 
00076 //uint8_t siezedTimers = 0;                                // bitmap of initialized timers (replaced by isTimerActive method)
00077 uint8_t ServoCount = 0;                                    // the total number of attached servos
00078 
00079 // convenience macros
00080 #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((servoTimer_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
00081 #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this timer
00082 #define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)     // macro to access servo index by timer and channel
00083 #define SERVO(_timer,_channel)  (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class by timer and channel
00084 
00085 #define SERVO_MIN(_servo_nbr) (MIN_PULSE_WIDTH - servos[_servo_nbr].min * 4)  // minimum value in uS for this servo
00086 #define SERVO_MAX(_servo_nbr) (MAX_PULSE_WIDTH - servos[_servo_nbr].max * 4)  // maximum value in uS for this servo 
00087 
00088 /************ static functions common to all instances ***********************/
00089 
00090 static inline void handle_interrupts(servoTimer_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
00091 {
00092   if( Channel[timer] < 0 )
00093     *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer 
00094   else{
00095     if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )  
00096       digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated   
00097   }
00098 
00099   Channel[timer]++;    // increment to the next channel
00100   if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
00101     *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
00102     if(SERVO(timer,Channel[timer]).Pin.isActive == true)           // check if activated
00103       digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high   
00104   }     
00105   else { 
00106     // finished all channels so wait for the refresh period to expire before starting over 
00107     if( (unsigned)*TCNTn < (((unsigned int)REFRESH_INTERVAL * TICKS_PER_uS) + 4) )      // allow a few ticks to ensure the next OCR1A not missed
00108       *OCRnA = (unsigned int)REFRESH_INTERVAL * TICKS_PER_uS;   
00109     else 
00110       *OCRnA = *TCNTn + 4;  // at least REFRESH_INTERVAL has elapsed
00111         Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
00112   }
00113 }
00114 
00115 SIGNAL (TIMER1_COMPA_vect) 
00116 { 
00117   handle_interrupts(_timer1, &TCNT1, &OCR1A); 
00118 }
00119 
00120 #if defined(__AVR_ATmega1280__)
00121 SIGNAL (TIMER3_COMPA_vect) 
00122 { 
00123   handle_interrupts(_timer3, &TCNT3, &OCR3A); 
00124 }
00125 SIGNAL (TIMER4_COMPA_vect) 
00126 {
00127   handle_interrupts(_timer4, &TCNT4, &OCR4A); 
00128 }
00129 SIGNAL (TIMER5_COMPA_vect) 
00130 {
00131   handle_interrupts(_timer5, &TCNT5, &OCR5A); 
00132 }
00133 #endif
00134 
00135 static void initISR(servoTimer_t timer)
00136 {  
00137   if(timer == _timer1) {
00138     TCCR1A = 0;             // normal counting mode 
00139     TCCR1B = _BV(CS11);     // set prescaler of 8 
00140     TCNT1 = 0;              // clear the timer count 
00141     TIFR1 = _BV(OCF1A);     // clear any pending interrupts; 
00142     TIMSK1 =  _BV(OCIE1A) ; // enable the output compare interrupt        
00143   }
00144 #if defined(__AVR_ATmega1280__)
00145   else if(timer == _timer3) {
00146     TCCR3A = 0;             // normal counting mode 
00147     TCCR3B = _BV(CS31);     // set prescaler of 8  
00148     TCNT3 = 0;              // clear the timer count 
00149     TIFR3 = _BV(OCF3A);     // clear any pending interrupts; 
00150     TIMSK3 =  _BV(OCIE3A) ; // enable the output compare interrupt                
00151   }
00152   else if(timer == _timer4) {
00153     TCCR4A = 0;             // normal counting mode 
00154     TCCR4B = _BV(CS41);     // set prescaler of 8  
00155     TCNT4 = 0;              // clear the timer count 
00156     TIFR4 = _BV(OCF4A);     // clear any pending interrupts; 
00157     TIMSK4 =  _BV(OCIE4A) ; // enable the output compare interrupt                
00158   }
00159   else if(timer == _timer5) {
00160     TCCR5A = 0;             // normal counting mode 
00161     TCCR5B = _BV(CS51);     // set prescaler of 8  
00162     TCNT5 = 0;              // clear the timer count 
00163     TIFR5 = _BV(OCF5A);     // clear any pending interrupts; 
00164     TIMSK5 =  _BV(OCIE5A) ; // enable the output compare interrupt                
00165   }
00166 #endif
00167 } 
00168 
00169 static boolean isTimerActive(servoTimer_t timer)
00170 {
00171   // returns true if any servo is active on this timer
00172   for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
00173           if(SERVO(timer,channel).Pin.isActive == true)
00174                  return true;
00175   }
00176   return false;
00177 }
00178 
00179 //#define DEBUG_SHOW
00180 #ifdef DEBUG_SHOW
00181 #include <HardwareSerial.h> 
00182 void debugShow(int index, char *label)
00183 {
00184   Serial.print(label);
00185   Serial.print(" for servo: ");
00186   Serial.print(index,DEC);
00187   Serial.print(", pin= ");
00188   Serial.print(servos[index].Pin.nbr,DEC);
00189   Serial.print(", ticks=");
00190   Serial.print(servos[index].ticks,DEC);
00191   Serial.print(", min=");
00192   Serial.print(SERVO_MIN(index),DEC);
00193   Serial.print(", max=");
00194   Serial.println(SERVO_MAX(index),DEC);
00195 }
00196 #endif
00197 
00198 
00199 /****************** end of static functions ******************************/
00200 
00201 MegaServo::MegaServo()
00202 {
00203   if( ServoCount < MAX_SERVOS) {
00204     this->servoIndex = ServoCount++;                    // assign a servo index to this instance
00205         servos[this->servoIndex].ticks = DEFAULT_PULSE_WIDTH * TICKS_PER_uS;   // store default values  
00206   }
00207   else
00208     this->servoIndex = INVALID_SERVO ;  // too many servos 
00209 }
00210 
00211 uint8_t MegaServo::attach(int pin)
00212 {
00213   return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
00214 }
00215 
00216 uint8_t MegaServo::attach(int pin, int min, int max)
00217 {
00218   if(this->servoIndex < MAX_SERVOS ) {
00219     pinMode( pin, OUTPUT) ;                                   // set servo pin to output
00220     servos[this->servoIndex].Pin.nbr = pin;  
00221         // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 
00222         servos[this->servoIndex].min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
00223     servos[this->servoIndex].max  = (MAX_PULSE_WIDTH - max)/4; 
00224         // initialize the timer if it has not already been initialized 
00225         servoTimer_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
00226     if(isTimerActive(timer) == false)
00227       initISR(timer);    
00228         servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
00229   } 
00230   return this->servoIndex ;
00231 }
00232 
00233 void MegaServo::detach()  
00234 {
00235   servos[this->servoIndex].Pin.isActive = false;  
00236 
00237 #ifdef FREE_TIMERS
00238   if(isTimerActive(SERVO_INDEX_TO_TIMER(servoIndex)) == false) {
00239      ;// call to unimplimented function in wiring.c to re-init timer (set timer back to PWM mode) TODO? 
00240   }
00241 #endif
00242 }
00243 
00244 void MegaServo::write(int value)
00245 {       
00246   // calculate and store the values for the given channel
00247   byte channel = this->servoIndex;
00248   if( (channel >= 0) && (channel < MAX_SERVOS) )   // ensure channel is valid
00249   {  
00250     if(value < 200) // convert degrees if value < 200
00251        value = map(value, 0, 180, SERVO_MIN(channel),  SERVO_MAX(channel));      
00252     else {
00253       if( value < SERVO_MIN(channel) )                 // ensure pulse width is valid
00254         value = SERVO_MIN(channel);
00255       else if( value > SERVO_MAX(channel) )
00256         value = SERVO_MAX(channel);      
00257     }
00258     value = (value-TRIM_DURATION) * TICKS_PER_uS;  // convert to ticks after compensating for interrupt overhead
00259         uint8_t oldSREG = SREG;
00260         cli();
00261         servos[channel].ticks = value;  
00262         SREG = oldSREG; 
00263   } 
00264 }
00265 
00266 void MegaServo::writeMicroseconds(int value)
00267 {
00268    this->write(value);
00269 }
00270 
00271 int MegaServo::read() // return the value as degrees
00272 {
00273   return  map( this->readMicroseconds(), SERVO_MIN(this->servoIndex), SERVO_MAX(this->servoIndex), 0, 180);     
00274 }
00275 
00276 int MegaServo::readMicroseconds()
00277 {
00278   unsigned int pulsewidth;
00279   if( this->servoIndex != INVALID_SERVO )
00280     pulsewidth = (servos[this->servoIndex].ticks /  TICKS_PER_uS) + TRIM_DURATION +1 ;
00281   else 
00282     pulsewidth  = 0;
00283 
00284   return pulsewidth;   
00285 }
00286 
00287 bool MegaServo::attached()
00288 {
00289   return servos[this->servoIndex].Pin.isActive ;
00290 }