00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
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
00057
00058 static servo_t servos[MAX_SERVOS];
00059 static volatile int8_t Channel[_Nbr_16timers ];
00060
00061 uint8_t ServoCount = 0;
00062
00063
00064
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
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;
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);
00082 }
00083
00084 Channel[timer]++;
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)
00088 digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH);
00089 }
00090 else {
00091
00092 if( (unsigned)*TCNTn < (usToTicks(REFRESH_INTERVAL) + 4) )
00093 *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
00094 else
00095 *OCRnA = *TCNTn + 4;
00096 Channel[timer] = -1;
00097 }
00098 }
00099
00100 #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
00101
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
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;
00152 TCCR1B = _BV(CS11);
00153 TCNT1 = 0;
00154 #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
00155 TIFR |= _BV(OCF1A);
00156 TIMSK |= _BV(OCIE1A) ;
00157 #else
00158
00159 TIFR1 |= _BV(OCF1A);
00160 TIMSK1 |= _BV(OCIE1A) ;
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;
00171 TCCR3B = _BV(CS31);
00172 TCNT3 = 0;
00173 #if defined(__AVR_ATmega128__)
00174 TIFR |= _BV(OCF3A);
00175 ETIMSK |= _BV(OCIE3A);
00176 #else
00177 TIFR3 = _BV(OCF3A);
00178 TIMSK3 = _BV(OCIE3A) ;
00179 #endif
00180 #if defined(WIRING)
00181 timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service);
00182 #endif
00183 }
00184 #endif
00185
00186 #if defined (_useTimer4)
00187 if(timer == _timer4) {
00188 TCCR4A = 0;
00189 TCCR4B = _BV(CS41);
00190 TCNT4 = 0;
00191 TIFR4 = _BV(OCF4A);
00192 TIMSK4 = _BV(OCIE4A) ;
00193 }
00194 #endif
00195
00196 #if defined (_useTimer5)
00197 if(timer == _timer5) {
00198 TCCR5A = 0;
00199 TCCR5B = _BV(CS51);
00200 TCNT5 = 0;
00201 TIFR5 = _BV(OCF5A);
00202 TIMSK5 = _BV(OCIE5A) ;
00203 }
00204 #endif
00205 }
00206
00207 static void finISR(timer16_Sequence_t timer)
00208 {
00209
00210 #if defined WIRING // Wiring
00211 if(timer == _timer1) {
00212 #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
00213 TIMSK1 &= ~_BV(OCIE1A) ;
00214 #else
00215 TIMSK &= ~_BV(OCIE1A) ;
00216 #endif
00217 timerDetach(TIMER1OUTCOMPAREA_INT);
00218 }
00219 else if(timer == _timer3) {
00220 #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
00221 TIMSK3 &= ~_BV(OCIE3A);
00222 #else
00223 ETIMSK &= ~_BV(OCIE3A);
00224 #endif
00225 timerDetach(TIMER3OUTCOMPAREA_INT);
00226 }
00227 #else
00228
00229 #endif
00230 }
00231
00232 static boolean isTimerActive(timer16_Sequence_t timer)
00233 {
00234
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
00244
00245 Servo::Servo()
00246 {
00247 if( ServoCount < MAX_SERVOS) {
00248 this->servoIndex = ServoCount++;
00249 servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);
00250 }
00251 else
00252 this->servoIndex = INVALID_SERVO ;
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) ;
00264 servos[this->servoIndex].Pin.nbr = pin;
00265
00266 this->min = (MIN_PULSE_WIDTH - min)/4;
00267 this->max = (MAX_PULSE_WIDTH - max)/4;
00268
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;
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 {
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
00300 byte channel = this->servoIndex;
00301 if( (channel >= 0) && (channel < MAX_SERVOS) )
00302 {
00303 if( value < SERVO_MIN() )
00304 value = SERVO_MIN();
00305 else if( value > SERVO_MAX() )
00306 value = SERVO_MAX();
00307
00308 value = value - TRIM_DURATION;
00309 value = usToTicks(value);
00310
00311 uint8_t oldSREG = SREG;
00312 cli();
00313 servos[channel].ticks = value;
00314 SREG = oldSREG;
00315 }
00316 }
00317
00318 int Servo::read()
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 ;
00328 else
00329 pulsewidth = 0;
00330
00331 return pulsewidth;
00332 }
00333
00334 bool Servo::attached()
00335 {
00336 return servos[this->servoIndex].Pin.isActive ;
00337 }