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 |
|
---|