[136] | 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 | #if defined(ARDUINO_ARCH_AVR)
|
---|
| 21 |
|
---|
| 22 | #include <avr/interrupt.h>
|
---|
| 23 | #include <Arduino.h>
|
---|
| 24 |
|
---|
| 25 | #include "Servo.h"
|
---|
| 26 |
|
---|
| 27 | #define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009
|
---|
| 28 | #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
|
---|
| 29 |
|
---|
| 30 |
|
---|
| 31 | #define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
|
---|
| 32 |
|
---|
| 33 | //#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER)
|
---|
| 34 |
|
---|
| 35 | static servo_t servos[MAX_SERVOS]; // static array of servo structures
|
---|
| 36 | static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
|
---|
| 37 |
|
---|
| 38 | uint8_t ServoCount = 0; // the total number of attached servos
|
---|
| 39 |
|
---|
| 40 |
|
---|
| 41 | // convenience macros
|
---|
| 42 | #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
|
---|
| 43 | #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
|
---|
| 44 | #define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
|
---|
| 45 | #define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
|
---|
| 46 |
|
---|
| 47 | #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
|
---|
| 48 | #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
|
---|
| 49 |
|
---|
| 50 | /************ static functions common to all instances ***********************/
|
---|
| 51 |
|
---|
| 52 | static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
|
---|
| 53 | {
|
---|
| 54 | if( Channel[timer] < 0 )
|
---|
| 55 | *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
|
---|
| 56 | else{
|
---|
| 57 | if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
|
---|
| 58 | digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
|
---|
| 59 | }
|
---|
| 60 |
|
---|
| 61 | Channel[timer]++; // increment to the next channel
|
---|
| 62 | if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
|
---|
| 63 | *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
|
---|
| 64 | if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated
|
---|
| 65 | digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
|
---|
| 66 | }
|
---|
| 67 | else {
|
---|
| 68 | // finished all channels so wait for the refresh period to expire before starting over
|
---|
| 69 | if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
|
---|
| 70 | *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
|
---|
| 71 | else
|
---|
| 72 | *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
|
---|
| 73 | Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
|
---|
| 74 | }
|
---|
| 75 | }
|
---|
| 76 |
|
---|
| 77 | #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
|
---|
| 78 | // Interrupt handlers for Arduino
|
---|
| 79 | #if defined(_useTimer1)
|
---|
| 80 | SIGNAL (TIMER1_COMPA_vect)
|
---|
| 81 | {
|
---|
| 82 | handle_interrupts(_timer1, &TCNT1, &OCR1A);
|
---|
| 83 | }
|
---|
| 84 | #endif
|
---|
| 85 |
|
---|
| 86 | #if defined(_useTimer3)
|
---|
| 87 | SIGNAL (TIMER3_COMPA_vect)
|
---|
| 88 | {
|
---|
| 89 | handle_interrupts(_timer3, &TCNT3, &OCR3A);
|
---|
| 90 | }
|
---|
| 91 | #endif
|
---|
| 92 |
|
---|
| 93 | #if defined(_useTimer4)
|
---|
| 94 | SIGNAL (TIMER4_COMPA_vect)
|
---|
| 95 | {
|
---|
| 96 | handle_interrupts(_timer4, &TCNT4, &OCR4A);
|
---|
| 97 | }
|
---|
| 98 | #endif
|
---|
| 99 |
|
---|
| 100 | #if defined(_useTimer5)
|
---|
| 101 | SIGNAL (TIMER5_COMPA_vect)
|
---|
| 102 | {
|
---|
| 103 | handle_interrupts(_timer5, &TCNT5, &OCR5A);
|
---|
| 104 | }
|
---|
| 105 | #endif
|
---|
| 106 |
|
---|
| 107 | #elif defined WIRING
|
---|
| 108 | // Interrupt handlers for Wiring
|
---|
| 109 | #if defined(_useTimer1)
|
---|
| 110 | void Timer1Service()
|
---|
| 111 | {
|
---|
| 112 | handle_interrupts(_timer1, &TCNT1, &OCR1A);
|
---|
| 113 | }
|
---|
| 114 | #endif
|
---|
| 115 | #if defined(_useTimer3)
|
---|
| 116 | void Timer3Service()
|
---|
| 117 | {
|
---|
| 118 | handle_interrupts(_timer3, &TCNT3, &OCR3A);
|
---|
| 119 | }
|
---|
| 120 | #endif
|
---|
| 121 | #endif
|
---|
| 122 |
|
---|
| 123 |
|
---|
| 124 | static void initISR(timer16_Sequence_t timer)
|
---|
| 125 | {
|
---|
| 126 | #if defined (_useTimer1)
|
---|
| 127 | if(timer == _timer1) {
|
---|
| 128 | TCCR1A = 0; // normal counting mode
|
---|
| 129 | TCCR1B = _BV(CS11); // set prescaler of 8
|
---|
| 130 | TCNT1 = 0; // clear the timer count
|
---|
| 131 | #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
|
---|
| 132 | TIFR |= _BV(OCF1A); // clear any pending interrupts;
|
---|
| 133 | TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt
|
---|
| 134 | #else
|
---|
| 135 | // here if not ATmega8 or ATmega128
|
---|
| 136 | TIFR1 |= _BV(OCF1A); // clear any pending interrupts;
|
---|
| 137 | TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt
|
---|
| 138 | #endif
|
---|
| 139 | #if defined(WIRING)
|
---|
| 140 | timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
|
---|
| 141 | #endif
|
---|
| 142 | }
|
---|
| 143 | #endif
|
---|
| 144 |
|
---|
| 145 | #if defined (_useTimer3)
|
---|
| 146 | if(timer == _timer3) {
|
---|
| 147 | TCCR3A = 0; // normal counting mode
|
---|
| 148 | TCCR3B = _BV(CS31); // set prescaler of 8
|
---|
| 149 | TCNT3 = 0; // clear the timer count
|
---|
| 150 | #if defined(__AVR_ATmega128__)
|
---|
| 151 | TIFR |= _BV(OCF3A); // clear any pending interrupts;
|
---|
| 152 | ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt
|
---|
| 153 | #else
|
---|
| 154 | TIFR3 = _BV(OCF3A); // clear any pending interrupts;
|
---|
| 155 | TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
|
---|
| 156 | #endif
|
---|
| 157 | #if defined(WIRING)
|
---|
| 158 | timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
|
---|
| 159 | #endif
|
---|
| 160 | }
|
---|
| 161 | #endif
|
---|
| 162 |
|
---|
| 163 | #if defined (_useTimer4)
|
---|
| 164 | if(timer == _timer4) {
|
---|
| 165 | TCCR4A = 0; // normal counting mode
|
---|
| 166 | TCCR4B = _BV(CS41); // set prescaler of 8
|
---|
| 167 | TCNT4 = 0; // clear the timer count
|
---|
| 168 | TIFR4 = _BV(OCF4A); // clear any pending interrupts;
|
---|
| 169 | TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
|
---|
| 170 | }
|
---|
| 171 | #endif
|
---|
| 172 |
|
---|
| 173 | #if defined (_useTimer5)
|
---|
| 174 | if(timer == _timer5) {
|
---|
| 175 | TCCR5A = 0; // normal counting mode
|
---|
| 176 | TCCR5B = _BV(CS51); // set prescaler of 8
|
---|
| 177 | TCNT5 = 0; // clear the timer count
|
---|
| 178 | TIFR5 = _BV(OCF5A); // clear any pending interrupts;
|
---|
| 179 | TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
|
---|
| 180 | }
|
---|
| 181 | #endif
|
---|
| 182 | }
|
---|
| 183 |
|
---|
| 184 | static void finISR(timer16_Sequence_t timer)
|
---|
| 185 | {
|
---|
| 186 | //disable use of the given timer
|
---|
| 187 | #if defined WIRING // Wiring
|
---|
| 188 | if(timer == _timer1) {
|
---|
| 189 | #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
|
---|
| 190 | TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
|
---|
| 191 | #else
|
---|
| 192 | TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
|
---|
| 193 | #endif
|
---|
| 194 | timerDetach(TIMER1OUTCOMPAREA_INT);
|
---|
| 195 | }
|
---|
| 196 | else if(timer == _timer3) {
|
---|
| 197 | #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
|
---|
| 198 | TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
|
---|
| 199 | #else
|
---|
| 200 | ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
|
---|
| 201 | #endif
|
---|
| 202 | timerDetach(TIMER3OUTCOMPAREA_INT);
|
---|
| 203 | }
|
---|
| 204 | #else
|
---|
| 205 | //For arduino - in future: call here to a currently undefined function to reset the timer
|
---|
| 206 | #endif
|
---|
| 207 | }
|
---|
| 208 |
|
---|
| 209 | static boolean isTimerActive(timer16_Sequence_t timer)
|
---|
| 210 | {
|
---|
| 211 | // returns true if any servo is active on this timer
|
---|
| 212 | for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
|
---|
| 213 | if(SERVO(timer,channel).Pin.isActive == true)
|
---|
| 214 | return true;
|
---|
| 215 | }
|
---|
| 216 | return false;
|
---|
| 217 | }
|
---|
| 218 |
|
---|
| 219 |
|
---|
| 220 | /****************** end of static functions ******************************/
|
---|
| 221 |
|
---|
| 222 | Servo::Servo()
|
---|
| 223 | {
|
---|
| 224 | if( ServoCount < MAX_SERVOS) {
|
---|
| 225 | this->servoIndex = ServoCount++; // assign a servo index to this instance
|
---|
| 226 | servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
|
---|
| 227 | }
|
---|
| 228 | else
|
---|
| 229 | this->servoIndex = INVALID_SERVO ; // too many servos
|
---|
| 230 | }
|
---|
| 231 |
|
---|
| 232 | uint8_t Servo::attach(int pin)
|
---|
| 233 | {
|
---|
| 234 | return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
|
---|
| 235 | }
|
---|
| 236 |
|
---|
| 237 | uint8_t Servo::attach(int pin, int min, int max)
|
---|
| 238 | {
|
---|
| 239 | if(this->servoIndex < MAX_SERVOS ) {
|
---|
| 240 | pinMode( pin, OUTPUT) ; // set servo pin to output
|
---|
| 241 | servos[this->servoIndex].Pin.nbr = pin;
|
---|
| 242 | // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
|
---|
| 243 | this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
|
---|
| 244 | this->max = (MAX_PULSE_WIDTH - max)/4;
|
---|
| 245 | // initialize the timer if it has not already been initialized
|
---|
| 246 | timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
|
---|
| 247 | if(isTimerActive(timer) == false)
|
---|
| 248 | initISR(timer);
|
---|
| 249 | servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
|
---|
| 250 | }
|
---|
| 251 | return this->servoIndex ;
|
---|
| 252 | }
|
---|
| 253 |
|
---|
| 254 | void Servo::detach()
|
---|
| 255 | {
|
---|
| 256 | servos[this->servoIndex].Pin.isActive = false;
|
---|
| 257 | timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
|
---|
| 258 | if(isTimerActive(timer) == false) {
|
---|
| 259 | finISR(timer);
|
---|
| 260 | }
|
---|
| 261 | }
|
---|
| 262 |
|
---|
| 263 | void Servo::write(int value)
|
---|
| 264 | {
|
---|
| 265 | if(value < MIN_PULSE_WIDTH)
|
---|
| 266 | { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
|
---|
| 267 | if(value < 0) value = 0;
|
---|
| 268 | if(value > 180) value = 180;
|
---|
| 269 | value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
|
---|
| 270 | }
|
---|
| 271 | this->writeMicroseconds(value);
|
---|
| 272 | }
|
---|
| 273 |
|
---|
| 274 | void Servo::writeMicroseconds(int value)
|
---|
| 275 | {
|
---|
| 276 | // calculate and store the values for the given channel
|
---|
| 277 | byte channel = this->servoIndex;
|
---|
| 278 | if( (channel < MAX_SERVOS) ) // ensure channel is valid
|
---|
| 279 | {
|
---|
| 280 | if( value < SERVO_MIN() ) // ensure pulse width is valid
|
---|
| 281 | value = SERVO_MIN();
|
---|
| 282 | else if( value > SERVO_MAX() )
|
---|
| 283 | value = SERVO_MAX();
|
---|
| 284 |
|
---|
| 285 | value = value - TRIM_DURATION;
|
---|
| 286 | value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
|
---|
| 287 |
|
---|
| 288 | uint8_t oldSREG = SREG;
|
---|
| 289 | cli();
|
---|
| 290 | servos[channel].ticks = value;
|
---|
| 291 | SREG = oldSREG;
|
---|
| 292 | }
|
---|
| 293 | }
|
---|
| 294 |
|
---|
| 295 | int Servo::read() // return the value as degrees
|
---|
| 296 | {
|
---|
| 297 | return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
|
---|
| 298 | }
|
---|
| 299 |
|
---|
| 300 | int Servo::readMicroseconds()
|
---|
| 301 | {
|
---|
| 302 | unsigned int pulsewidth;
|
---|
| 303 | if( this->servoIndex != INVALID_SERVO )
|
---|
| 304 | pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009
|
---|
| 305 | else
|
---|
| 306 | pulsewidth = 0;
|
---|
| 307 |
|
---|
| 308 | return pulsewidth;
|
---|
| 309 | }
|
---|
| 310 |
|
---|
| 311 | bool Servo::attached()
|
---|
| 312 | {
|
---|
| 313 | return servos[this->servoIndex].Pin.isActive ;
|
---|
| 314 | }
|
---|
| 315 |
|
---|
| 316 | #endif // ARDUINO_ARCH_AVR
|
---|
| 317 |
|
---|