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
00046
00047
00048
00049
00050
00051
00052
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];
00069 static volatile int8_t Channel[NBR_TIMERS];
00070 #if defined(__AVR_ATmega1280__)
00071 typedef enum { _timer5, _timer1, _timer3, _timer4 } servoTimer_t;
00072 #else
00073 typedef enum { _timer1 } servoTimer_t;
00074 #endif
00075
00076
00077 uint8_t ServoCount = 0;
00078
00079
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
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;
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);
00097 }
00098
00099 Channel[timer]++;
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)
00103 digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH);
00104 }
00105 else {
00106
00107 if( (unsigned)*TCNTn < (((unsigned int)REFRESH_INTERVAL * TICKS_PER_uS) + 4) )
00108 *OCRnA = (unsigned int)REFRESH_INTERVAL * TICKS_PER_uS;
00109 else
00110 *OCRnA = *TCNTn + 4;
00111 Channel[timer] = -1;
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;
00139 TCCR1B = _BV(CS11);
00140 TCNT1 = 0;
00141 TIFR1 = _BV(OCF1A);
00142 TIMSK1 = _BV(OCIE1A) ;
00143 }
00144 #if defined(__AVR_ATmega1280__)
00145 else if(timer == _timer3) {
00146 TCCR3A = 0;
00147 TCCR3B = _BV(CS31);
00148 TCNT3 = 0;
00149 TIFR3 = _BV(OCF3A);
00150 TIMSK3 = _BV(OCIE3A) ;
00151 }
00152 else if(timer == _timer4) {
00153 TCCR4A = 0;
00154 TCCR4B = _BV(CS41);
00155 TCNT4 = 0;
00156 TIFR4 = _BV(OCF4A);
00157 TIMSK4 = _BV(OCIE4A) ;
00158 }
00159 else if(timer == _timer5) {
00160 TCCR5A = 0;
00161 TCCR5B = _BV(CS51);
00162 TCNT5 = 0;
00163 TIFR5 = _BV(OCF5A);
00164 TIMSK5 = _BV(OCIE5A) ;
00165 }
00166 #endif
00167 }
00168
00169 static boolean isTimerActive(servoTimer_t timer)
00170 {
00171
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
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
00200
00201 MegaServo::MegaServo()
00202 {
00203 if( ServoCount < MAX_SERVOS) {
00204 this->servoIndex = ServoCount++;
00205 servos[this->servoIndex].ticks = DEFAULT_PULSE_WIDTH * TICKS_PER_uS;
00206 }
00207 else
00208 this->servoIndex = INVALID_SERVO ;
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) ;
00220 servos[this->servoIndex].Pin.nbr = pin;
00221
00222 servos[this->servoIndex].min = (MIN_PULSE_WIDTH - min)/4;
00223 servos[this->servoIndex].max = (MAX_PULSE_WIDTH - max)/4;
00224
00225 servoTimer_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
00226 if(isTimerActive(timer) == false)
00227 initISR(timer);
00228 servos[this->servoIndex].Pin.isActive = true;
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 ;
00240 }
00241 #endif
00242 }
00243
00244 void MegaServo::write(int value)
00245 {
00246
00247 byte channel = this->servoIndex;
00248 if( (channel >= 0) && (channel < MAX_SERVOS) )
00249 {
00250 if(value < 200)
00251 value = map(value, 0, 180, SERVO_MIN(channel), SERVO_MAX(channel));
00252 else {
00253 if( value < SERVO_MIN(channel) )
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;
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()
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 }