[136] | 1 | /*
|
---|
| 2 | Copyright (c) 2013 Arduino LLC. All right reserved.
|
---|
| 3 |
|
---|
| 4 | This library is free software; you can redistribute it and/or
|
---|
| 5 | modify it under the terms of the GNU Lesser General Public
|
---|
| 6 | License as published by the Free Software Foundation; either
|
---|
| 7 | version 2.1 of the License, or (at your option) any later version.
|
---|
| 8 |
|
---|
| 9 | This library is distributed in the hope that it will be useful,
|
---|
| 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of
|
---|
| 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
---|
| 12 | Lesser General Public License for more details.
|
---|
| 13 |
|
---|
| 14 | You should have received a copy of the GNU Lesser General Public
|
---|
| 15 | License along with this library; if not, write to the Free Software
|
---|
| 16 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
---|
| 17 | */
|
---|
| 18 |
|
---|
| 19 | #if defined(ARDUINO_ARCH_SAM)
|
---|
| 20 |
|
---|
| 21 | #include <Arduino.h>
|
---|
| 22 | #include <Servo.h>
|
---|
| 23 |
|
---|
| 24 | #define usToTicks(_us) (( clockCyclesPerMicrosecond() * _us) / 32) // converts microseconds to tick
|
---|
| 25 | #define ticksToUs(_ticks) (( (unsigned)_ticks * 32)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
|
---|
| 26 |
|
---|
| 27 | #define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
|
---|
| 28 |
|
---|
| 29 | static servo_t servos[MAX_SERVOS]; // static array of servo structures
|
---|
| 30 |
|
---|
| 31 | uint8_t ServoCount = 0; // the total number of attached servos
|
---|
| 32 |
|
---|
| 33 | static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
|
---|
| 34 |
|
---|
| 35 | // convenience macros
|
---|
| 36 | #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
|
---|
| 37 | #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
|
---|
| 38 | #define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
|
---|
| 39 | #define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
|
---|
| 40 |
|
---|
| 41 | #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
|
---|
| 42 | #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
|
---|
| 43 |
|
---|
| 44 | /************ static functions common to all instances ***********************/
|
---|
| 45 |
|
---|
| 46 | //------------------------------------------------------------------------------
|
---|
| 47 | /// Interrupt handler for the TC0 channel 1.
|
---|
| 48 | //------------------------------------------------------------------------------
|
---|
| 49 | void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel);
|
---|
| 50 | #if defined (_useTimer1)
|
---|
| 51 | void HANDLER_FOR_TIMER1(void) {
|
---|
| 52 | Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1);
|
---|
| 53 | }
|
---|
| 54 | #endif
|
---|
| 55 | #if defined (_useTimer2)
|
---|
| 56 | void HANDLER_FOR_TIMER2(void) {
|
---|
| 57 | Servo_Handler(_timer2, TC_FOR_TIMER2, CHANNEL_FOR_TIMER2);
|
---|
| 58 | }
|
---|
| 59 | #endif
|
---|
| 60 | #if defined (_useTimer3)
|
---|
| 61 | void HANDLER_FOR_TIMER3(void) {
|
---|
| 62 | Servo_Handler(_timer3, TC_FOR_TIMER3, CHANNEL_FOR_TIMER3);
|
---|
| 63 | }
|
---|
| 64 | #endif
|
---|
| 65 | #if defined (_useTimer4)
|
---|
| 66 | void HANDLER_FOR_TIMER4(void) {
|
---|
| 67 | Servo_Handler(_timer4, TC_FOR_TIMER4, CHANNEL_FOR_TIMER4);
|
---|
| 68 | }
|
---|
| 69 | #endif
|
---|
| 70 | #if defined (_useTimer5)
|
---|
| 71 | void HANDLER_FOR_TIMER5(void) {
|
---|
| 72 | Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5);
|
---|
| 73 | }
|
---|
| 74 | #endif
|
---|
| 75 |
|
---|
| 76 | void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel)
|
---|
| 77 | {
|
---|
| 78 | // clear interrupt
|
---|
| 79 | tc->TC_CHANNEL[channel].TC_SR;
|
---|
| 80 | if (Channel[timer] < 0) {
|
---|
| 81 | tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
|
---|
| 82 | } else {
|
---|
| 83 | if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true) {
|
---|
| 84 | digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
|
---|
| 85 | }
|
---|
| 86 | }
|
---|
| 87 |
|
---|
| 88 | Channel[timer]++; // increment to the next channel
|
---|
| 89 | if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
|
---|
| 90 | tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks;
|
---|
| 91 | if(SERVO(timer,Channel[timer]).Pin.isActive == true) { // check if activated
|
---|
| 92 | digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
|
---|
| 93 | }
|
---|
| 94 | }
|
---|
| 95 | else {
|
---|
| 96 | // finished all channels so wait for the refresh period to expire before starting over
|
---|
| 97 | if( (tc->TC_CHANNEL[channel].TC_CV) + 4 < usToTicks(REFRESH_INTERVAL) ) { // allow a few ticks to ensure the next OCR1A not missed
|
---|
| 98 | tc->TC_CHANNEL[channel].TC_RA = (unsigned int)usToTicks(REFRESH_INTERVAL);
|
---|
| 99 | }
|
---|
| 100 | else {
|
---|
| 101 | tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed
|
---|
| 102 | }
|
---|
| 103 | Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
|
---|
| 104 | }
|
---|
| 105 | }
|
---|
| 106 |
|
---|
| 107 | static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn)
|
---|
| 108 | {
|
---|
| 109 | pmc_enable_periph_clk(id);
|
---|
| 110 | TC_Configure(tc, channel,
|
---|
| 111 | TC_CMR_TCCLKS_TIMER_CLOCK3 | // MCK/32
|
---|
| 112 | TC_CMR_WAVE | // Waveform mode
|
---|
| 113 | TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC
|
---|
| 114 |
|
---|
| 115 | /* 84MHz, MCK/32, for 1.5ms: 3937 */
|
---|
| 116 | TC_SetRA(tc, channel, 2625); // 1ms
|
---|
| 117 |
|
---|
| 118 | /* Configure and enable interrupt */
|
---|
| 119 | NVIC_EnableIRQ(irqn);
|
---|
| 120 | // TC_IER_CPAS: RA Compare
|
---|
| 121 | tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS;
|
---|
| 122 |
|
---|
| 123 | // Enables the timer clock and performs a software reset to start the counting
|
---|
| 124 | TC_Start(tc, channel);
|
---|
| 125 | }
|
---|
| 126 |
|
---|
| 127 | static void initISR(timer16_Sequence_t timer)
|
---|
| 128 | {
|
---|
| 129 | #if defined (_useTimer1)
|
---|
| 130 | if (timer == _timer1)
|
---|
| 131 | _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
|
---|
| 132 | #endif
|
---|
| 133 | #if defined (_useTimer2)
|
---|
| 134 | if (timer == _timer2)
|
---|
| 135 | _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
|
---|
| 136 | #endif
|
---|
| 137 | #if defined (_useTimer3)
|
---|
| 138 | if (timer == _timer3)
|
---|
| 139 | _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3);
|
---|
| 140 | #endif
|
---|
| 141 | #if defined (_useTimer4)
|
---|
| 142 | if (timer == _timer4)
|
---|
| 143 | _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4);
|
---|
| 144 | #endif
|
---|
| 145 | #if defined (_useTimer5)
|
---|
| 146 | if (timer == _timer5)
|
---|
| 147 | _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5);
|
---|
| 148 | #endif
|
---|
| 149 | }
|
---|
| 150 |
|
---|
| 151 | static void finISR(timer16_Sequence_t timer)
|
---|
| 152 | {
|
---|
| 153 | #if defined (_useTimer1)
|
---|
| 154 | TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1);
|
---|
| 155 | #endif
|
---|
| 156 | #if defined (_useTimer2)
|
---|
| 157 | TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2);
|
---|
| 158 | #endif
|
---|
| 159 | #if defined (_useTimer3)
|
---|
| 160 | TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3);
|
---|
| 161 | #endif
|
---|
| 162 | #if defined (_useTimer4)
|
---|
| 163 | TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4);
|
---|
| 164 | #endif
|
---|
| 165 | #if defined (_useTimer5)
|
---|
| 166 | TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5);
|
---|
| 167 | #endif
|
---|
| 168 | }
|
---|
| 169 |
|
---|
| 170 |
|
---|
| 171 | static boolean isTimerActive(timer16_Sequence_t timer)
|
---|
| 172 | {
|
---|
| 173 | // returns true if any servo is active on this timer
|
---|
| 174 | for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
|
---|
| 175 | if(SERVO(timer,channel).Pin.isActive == true)
|
---|
| 176 | return true;
|
---|
| 177 | }
|
---|
| 178 | return false;
|
---|
| 179 | }
|
---|
| 180 |
|
---|
| 181 | /****************** end of static functions ******************************/
|
---|
| 182 |
|
---|
| 183 | Servo::Servo()
|
---|
| 184 | {
|
---|
| 185 | if (ServoCount < MAX_SERVOS) {
|
---|
| 186 | this->servoIndex = ServoCount++; // assign a servo index to this instance
|
---|
| 187 | servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
|
---|
| 188 | } else {
|
---|
| 189 | this->servoIndex = INVALID_SERVO; // too many servos
|
---|
| 190 | }
|
---|
| 191 | }
|
---|
| 192 |
|
---|
| 193 | uint8_t Servo::attach(int pin)
|
---|
| 194 | {
|
---|
| 195 | return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
|
---|
| 196 | }
|
---|
| 197 |
|
---|
| 198 | uint8_t Servo::attach(int pin, int min, int max)
|
---|
| 199 | {
|
---|
| 200 | timer16_Sequence_t timer;
|
---|
| 201 |
|
---|
| 202 | if (this->servoIndex < MAX_SERVOS) {
|
---|
| 203 | pinMode(pin, OUTPUT); // set servo pin to output
|
---|
| 204 | servos[this->servoIndex].Pin.nbr = pin;
|
---|
| 205 | // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
|
---|
| 206 | this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
|
---|
| 207 | this->max = (MAX_PULSE_WIDTH - max)/4;
|
---|
| 208 | // initialize the timer if it has not already been initialized
|
---|
| 209 | timer = SERVO_INDEX_TO_TIMER(servoIndex);
|
---|
| 210 | if (isTimerActive(timer) == false) {
|
---|
| 211 | initISR(timer);
|
---|
| 212 | }
|
---|
| 213 | servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
|
---|
| 214 | }
|
---|
| 215 | return this->servoIndex;
|
---|
| 216 | }
|
---|
| 217 |
|
---|
| 218 | void Servo::detach()
|
---|
| 219 | {
|
---|
| 220 | timer16_Sequence_t timer;
|
---|
| 221 |
|
---|
| 222 | servos[this->servoIndex].Pin.isActive = false;
|
---|
| 223 | timer = SERVO_INDEX_TO_TIMER(servoIndex);
|
---|
| 224 | if(isTimerActive(timer) == false) {
|
---|
| 225 | finISR(timer);
|
---|
| 226 | }
|
---|
| 227 | }
|
---|
| 228 |
|
---|
| 229 | void Servo::write(int value)
|
---|
| 230 | {
|
---|
| 231 | // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
|
---|
| 232 | if (value < MIN_PULSE_WIDTH)
|
---|
| 233 | {
|
---|
| 234 | if (value < 0)
|
---|
| 235 | value = 0;
|
---|
| 236 | else if (value > 180)
|
---|
| 237 | value = 180;
|
---|
| 238 |
|
---|
| 239 | value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
|
---|
| 240 | }
|
---|
| 241 | writeMicroseconds(value);
|
---|
| 242 | }
|
---|
| 243 |
|
---|
| 244 | void Servo::writeMicroseconds(int value)
|
---|
| 245 | {
|
---|
| 246 | // calculate and store the values for the given channel
|
---|
| 247 | byte channel = this->servoIndex;
|
---|
| 248 | if( (channel < MAX_SERVOS) ) // ensure channel is valid
|
---|
| 249 | {
|
---|
| 250 | if (value < SERVO_MIN()) // ensure pulse width is valid
|
---|
| 251 | value = SERVO_MIN();
|
---|
| 252 | else if (value > SERVO_MAX())
|
---|
| 253 | value = SERVO_MAX();
|
---|
| 254 |
|
---|
| 255 | value = value - TRIM_DURATION;
|
---|
| 256 | value = usToTicks(value); // convert to ticks after compensating for interrupt overhead
|
---|
| 257 | servos[channel].ticks = value;
|
---|
| 258 | }
|
---|
| 259 | }
|
---|
| 260 |
|
---|
| 261 | int Servo::read() // return the value as degrees
|
---|
| 262 | {
|
---|
| 263 | return map(readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
|
---|
| 264 | }
|
---|
| 265 |
|
---|
| 266 | int Servo::readMicroseconds()
|
---|
| 267 | {
|
---|
| 268 | unsigned int pulsewidth;
|
---|
| 269 | if (this->servoIndex != INVALID_SERVO)
|
---|
| 270 | pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
|
---|
| 271 | else
|
---|
| 272 | pulsewidth = 0;
|
---|
| 273 |
|
---|
| 274 | return pulsewidth;
|
---|
| 275 | }
|
---|
| 276 |
|
---|
| 277 | bool Servo::attached()
|
---|
| 278 | {
|
---|
| 279 | return servos[this->servoIndex].Pin.isActive;
|
---|
| 280 | }
|
---|
| 281 |
|
---|
| 282 | #endif // ARDUINO_ARCH_SAM
|
---|
| 283 |
|
---|