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 | //Edited by Arduino Srl development team.
|
---|
19 |
|
---|
20 | #if defined(ARDUINO_ARCH_SAMD)
|
---|
21 |
|
---|
22 | #include <Arduino.h>
|
---|
23 | #include <Servo.h>
|
---|
24 |
|
---|
25 |
|
---|
26 | static servo_t servos[MAX_SERVOS]; // static array of servo structures
|
---|
27 |
|
---|
28 | uint8_t ServoCount = 0; // the total number of attached servos
|
---|
29 | uint8_t isTC = 0 ;
|
---|
30 | Tc* TCx ;
|
---|
31 | Tcc* TCCx;
|
---|
32 | uint8_t Channelx = 0;
|
---|
33 |
|
---|
34 | // convenience macros
|
---|
35 |
|
---|
36 | #define SERVO_MIN_TCC() (MIN_PULSE_WIDTH_SAMD_TCC) // minimum value in uS for this servo if TCC timer is used
|
---|
37 | #define SERVO_MAX_TCC() (MAX_PULSE_WIDTH_SAMD_TCC) // maximum value in uS for this servo if TCC timer is used
|
---|
38 | #define SERVO_MIN_TC() (MIN_PULSE_WIDTH_SAMD_TC) // minimum value in uS for this servo if TC timer is used
|
---|
39 | #define SERVO_MAX_TC() (MAX_PULSE_WIDTH_SAMD_TC) // maximum value in uS for this servo if TC timer is used
|
---|
40 |
|
---|
41 | /************ static functions common to all instances ***********************/
|
---|
42 |
|
---|
43 | //------------------------------------------------------------------------------
|
---|
44 | /// Interrupt handler for the TC0 channel 1.
|
---|
45 | //------------------------------------------------------------------------------
|
---|
46 |
|
---|
47 |
|
---|
48 | /****************** end of static functions ******************************/
|
---|
49 |
|
---|
50 | Servo::Servo()
|
---|
51 | {
|
---|
52 | if (ServoCount < MAX_SERVOS) {
|
---|
53 | this->servoIndex = ServoCount++; // assign a servo index to this instance
|
---|
54 | } else {
|
---|
55 | this->servoIndex = INVALID_SERVO; // too many servos
|
---|
56 | }
|
---|
57 | }
|
---|
58 |
|
---|
59 | uint8_t Servo::attach(int pin)
|
---|
60 | {
|
---|
61 | if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
|
---|
62 | return this->attach(pin, SERVO_MIN_TC(), SERVO_MAX_TC());
|
---|
63 | }
|
---|
64 | else{
|
---|
65 | return this->attach(pin, SERVO_MIN_TCC(), SERVO_MAX_TCC());
|
---|
66 | }
|
---|
67 | }
|
---|
68 |
|
---|
69 | uint8_t Servo::attach(int pin, int min, int max)
|
---|
70 | {
|
---|
71 |
|
---|
72 |
|
---|
73 | if (this->servoIndex < MAX_SERVOS) {
|
---|
74 | pinMode(pin, OUTPUT); // set servo pin to output
|
---|
75 | servos[this->servoIndex].Pin.nbr = pin;
|
---|
76 | int servo_min, servo_max;
|
---|
77 | if(pin==4 | pin==5 | pin==10 | pin==12){
|
---|
78 | servo_min=SERVO_MIN_TC();
|
---|
79 | servo_max=SERVO_MAX_TC();
|
---|
80 | }
|
---|
81 | else{
|
---|
82 | servo_min=SERVO_MIN_TCC();
|
---|
83 | servo_max=SERVO_MAX_TCC();
|
---|
84 | }
|
---|
85 | if(min > servo_min) min = servo_min;
|
---|
86 | if (max > servo_max) max = servo_max;
|
---|
87 | this->min = min;
|
---|
88 | this->max = max;
|
---|
89 |
|
---|
90 | switch(pin)
|
---|
91 | {
|
---|
92 | case 2:
|
---|
93 | {
|
---|
94 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
95 | TCCx=TCC0;
|
---|
96 | Channelx=0;
|
---|
97 | isTC=0;
|
---|
98 | }
|
---|
99 | break;
|
---|
100 |
|
---|
101 | case 3:
|
---|
102 | {
|
---|
103 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
104 | TCCx=TCC0;
|
---|
105 | Channelx=1;
|
---|
106 | isTC=0;
|
---|
107 | }
|
---|
108 | break;
|
---|
109 |
|
---|
110 | case 4:
|
---|
111 | {
|
---|
112 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
113 | TCx=TC3;
|
---|
114 | Channelx=0;
|
---|
115 | isTC=1;
|
---|
116 |
|
---|
117 | }
|
---|
118 | break;
|
---|
119 |
|
---|
120 | case 5:
|
---|
121 | {
|
---|
122 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
123 | TCx=TC3;
|
---|
124 | Channelx=1;
|
---|
125 | isTC=1;
|
---|
126 |
|
---|
127 | }
|
---|
128 | break;
|
---|
129 | case 6:
|
---|
130 | {
|
---|
131 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
132 | TCCx=TCC0;
|
---|
133 | Channelx=2;
|
---|
134 | isTC=0;
|
---|
135 | }
|
---|
136 | break;
|
---|
137 |
|
---|
138 | case 7:
|
---|
139 | {
|
---|
140 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
141 | TCCx=TCC0;
|
---|
142 | Channelx=3;
|
---|
143 | isTC=0;
|
---|
144 | }
|
---|
145 | break;
|
---|
146 |
|
---|
147 | case 8:
|
---|
148 | {
|
---|
149 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
150 | TCCx=TCC1;
|
---|
151 | Channelx=0;
|
---|
152 | isTC=0;
|
---|
153 | }
|
---|
154 | break;
|
---|
155 |
|
---|
156 | case 9:
|
---|
157 | {
|
---|
158 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
159 | TCCx=TCC1;
|
---|
160 | Channelx=1;
|
---|
161 | isTC=0;
|
---|
162 | }
|
---|
163 | break;
|
---|
164 |
|
---|
165 | case 10:
|
---|
166 | {
|
---|
167 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
168 | TCx=TC3;
|
---|
169 | Channelx=0;
|
---|
170 | isTC=1;
|
---|
171 |
|
---|
172 | }
|
---|
173 | break;
|
---|
174 |
|
---|
175 | case 11:
|
---|
176 | {
|
---|
177 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
178 | TCCx=TCC2;
|
---|
179 | Channelx=0;
|
---|
180 | isTC=0;
|
---|
181 | }
|
---|
182 | break;
|
---|
183 |
|
---|
184 | case 12:
|
---|
185 | {
|
---|
186 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
187 | TCx=TC3;
|
---|
188 | Channelx=1;
|
---|
189 | isTC=1;
|
---|
190 |
|
---|
191 | }
|
---|
192 | break;
|
---|
193 |
|
---|
194 | case 13:
|
---|
195 | {
|
---|
196 | pinPeripheral(pin, g_APinDescription[pin].ulPinType);
|
---|
197 | TCCx=TCC2;
|
---|
198 | Channelx=1;
|
---|
199 | isTC=0;
|
---|
200 | }
|
---|
201 | break;
|
---|
202 |
|
---|
203 | default:
|
---|
204 | break;
|
---|
205 |
|
---|
206 | }
|
---|
207 |
|
---|
208 | if ((TCCx==TCC0) | (TCCx==TCC1)) GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC0_TCC1 )) ;
|
---|
209 | else if((TCCx==TCC2) | (TCx==TC3 ))GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC2_TC3 )) ;
|
---|
210 | else;
|
---|
211 |
|
---|
212 | if(servos[this->servoIndex].Pin.isActive == false){
|
---|
213 | // Set PORT
|
---|
214 | if ( isTC )
|
---|
215 | {
|
---|
216 | // -- Configure TC
|
---|
217 | //DISABLE TCx
|
---|
218 | TCx->COUNT16.CTRLA.reg &=~(TC_CTRLA_ENABLE);
|
---|
219 | //Set Timer counter Mode to 16 bits
|
---|
220 | TCx->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16;
|
---|
221 | //Set Prescaler to divide by 2
|
---|
222 | TCx->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV2;
|
---|
223 | //Set TCx as normal PWM
|
---|
224 | TCx->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_NPWM;
|
---|
225 | //default value for servo position
|
---|
226 | TCx->COUNT16.CC[Channelx].reg = 1500;
|
---|
227 | //ENABLE TCx
|
---|
228 | TCx->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE;
|
---|
229 | servos[this->servoIndex].Pin.isActive = true;
|
---|
230 | }
|
---|
231 | else
|
---|
232 | {
|
---|
233 | // -- Configure TCC
|
---|
234 |
|
---|
235 | TCCx->CTRLA.reg &=~(TCC_CTRLA_ENABLE); //disable TCC module
|
---|
236 | TCCx->CTRLA.reg |=TCC_CTRLA_PRESCALER_DIV8; //setting prescaler to divide by 8
|
---|
237 | TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM; //Set TCCx as normal PWM
|
---|
238 | TCCx->CC[Channelx].reg=1500; //default value for servo position
|
---|
239 | TCCx->PER.reg=20000; // setting servo frequency (50 hz)
|
---|
240 | TCCx->CTRLA.reg |= TCC_CTRLA_ENABLE ; //ENABLE TCCx
|
---|
241 | servos[this->servoIndex].Pin.isActive = true;
|
---|
242 | }
|
---|
243 |
|
---|
244 | }
|
---|
245 | }
|
---|
246 | return this->servoIndex;
|
---|
247 | }
|
---|
248 |
|
---|
249 | void Servo::detach()
|
---|
250 | {
|
---|
251 |
|
---|
252 |
|
---|
253 | servos[this->servoIndex].Pin.isActive = false;
|
---|
254 | if((servos[this->servoIndex].Pin.nbr == 2) | (servos[this->servoIndex].Pin.nbr == 3) | (servos[this->servoIndex].Pin.nbr == 6) | (servos[this->servoIndex].Pin.nbr == 7)) TCC0->CTRLA.reg &=~(TCC_CTRLA_ENABLE);
|
---|
255 | else if((servos[this->servoIndex].Pin.nbr == 8) | (servos[this->servoIndex].Pin.nbr == 9)) TCC1->CTRLA.reg &=~(TCC_CTRLA_ENABLE);
|
---|
256 | else if ((servos[this->servoIndex].Pin.nbr == 11) | (servos[this->servoIndex].Pin.nbr == 13)) TCC2->CTRLA.reg &=~(TCC_CTRLA_ENABLE);
|
---|
257 | else if ((servos[this->servoIndex].Pin.nbr == 4 ) | (servos[this->servoIndex].Pin.nbr == 5 ) | (servos[this->servoIndex].Pin.nbr == 10 ) | (servos[this->servoIndex].Pin.nbr == 12 ))TC3->COUNT16.CTRLA.reg &=~(TC_CTRLA_ENABLE);
|
---|
258 | }
|
---|
259 |
|
---|
260 | void Servo::write(int value)
|
---|
261 | {
|
---|
262 | //select the right values for servo motor
|
---|
263 | int servo_min;
|
---|
264 | int servo_max;
|
---|
265 | if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
|
---|
266 | servo_min=SERVO_MIN_TC();
|
---|
267 | servo_max=SERVO_MAX_TC();
|
---|
268 | // treat values less than 1700 as angles in degrees (valid values in microseconds are handled as microseconds)
|
---|
269 | if (value < servo_min)
|
---|
270 | {
|
---|
271 | if (value < 0)
|
---|
272 | value = 0;
|
---|
273 | else if (value > 180)
|
---|
274 | value = 180;
|
---|
275 | value = map(value, 0, 180, servo_min, servo_max);
|
---|
276 | }
|
---|
277 | }
|
---|
278 | else{
|
---|
279 | servo_min=SERVO_MIN_TCC();
|
---|
280 | servo_max=SERVO_MAX_TCC();
|
---|
281 | // treat values less than 400 as angles in degrees (valid values in microseconds are handled as microseconds)
|
---|
282 | if (value < servo_min)
|
---|
283 | {
|
---|
284 | if (value < 0)
|
---|
285 | value = 0;
|
---|
286 | else if (value > 180)
|
---|
287 | value = 180;
|
---|
288 | value = map(value, 0, 180, servo_min, servo_max);
|
---|
289 | }
|
---|
290 | }
|
---|
291 |
|
---|
292 | writeMicroseconds(value);
|
---|
293 | }
|
---|
294 |
|
---|
295 | void Servo::writeMicroseconds(int value)
|
---|
296 | {
|
---|
297 | // calculate and store the values for the given channel
|
---|
298 | byte channel = this->servoIndex;
|
---|
299 | if( (channel < MAX_SERVOS) ) // ensure channel is valid
|
---|
300 | {
|
---|
301 | //select the right values for servo motor
|
---|
302 | int servo_min;
|
---|
303 | int servo_max;
|
---|
304 | if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
|
---|
305 | servo_min=SERVO_MIN_TC();
|
---|
306 | servo_max=SERVO_MAX_TC();
|
---|
307 | }
|
---|
308 | else{
|
---|
309 | servo_min=SERVO_MIN_TCC();
|
---|
310 | servo_max=SERVO_MAX_TCC();
|
---|
311 | }
|
---|
312 | if (value < servo_min) // ensure pulse width is valid
|
---|
313 | value = servo_min;
|
---|
314 | else if (value > servo_max)
|
---|
315 | value = servo_max;
|
---|
316 | servos[this->servoIndex].ticks = value;
|
---|
317 | switch(servos[this->servoIndex].Pin.nbr)
|
---|
318 | {
|
---|
319 | case 2:
|
---|
320 | TCC0->CC[0].reg=value;
|
---|
321 | break;
|
---|
322 |
|
---|
323 | case 3:
|
---|
324 | TCC0->CC[1].reg=value;
|
---|
325 | break;
|
---|
326 |
|
---|
327 | case 4:
|
---|
328 | TC3->COUNT16.CC[0].reg = value;
|
---|
329 | break;
|
---|
330 |
|
---|
331 | case 5:
|
---|
332 | TC3->COUNT16.CC[1].reg = value;
|
---|
333 | break;
|
---|
334 |
|
---|
335 | case 6:
|
---|
336 | TCC0->CC[2].reg=value;
|
---|
337 | break;
|
---|
338 |
|
---|
339 | case 7:
|
---|
340 | TCC0->CC[3].reg=value;
|
---|
341 | break;
|
---|
342 |
|
---|
343 | case 8:
|
---|
344 | TCC1->CC[0].reg=value;
|
---|
345 | break;
|
---|
346 |
|
---|
347 | case 9:
|
---|
348 | TCC1->CC[1].reg=value;
|
---|
349 | break;
|
---|
350 |
|
---|
351 | case 10:
|
---|
352 | TC3->COUNT16.CC[0].reg = value;
|
---|
353 | break;
|
---|
354 |
|
---|
355 | case 11:
|
---|
356 | TCC2->CC[0].reg=value;
|
---|
357 | break;
|
---|
358 |
|
---|
359 | case 12:
|
---|
360 | TC3->COUNT16.CC[1].reg = value;
|
---|
361 | break;
|
---|
362 |
|
---|
363 | case 13:
|
---|
364 | TCC2->CC[1].reg=value;
|
---|
365 | break;
|
---|
366 |
|
---|
367 | default:
|
---|
368 | break;
|
---|
369 |
|
---|
370 | }
|
---|
371 |
|
---|
372 | //servos[this->servoIndex].ticks = value; //to be fixed
|
---|
373 | //servos[channel].ticks = value;
|
---|
374 | }
|
---|
375 | }
|
---|
376 |
|
---|
377 | int Servo::read() // return the value as degrees
|
---|
378 | {
|
---|
379 | //select the right values for servo motor
|
---|
380 | int servo_min;
|
---|
381 | int servo_max;
|
---|
382 | if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
|
---|
383 | servo_min=SERVO_MIN_TC();
|
---|
384 | servo_max=SERVO_MAX_TC();
|
---|
385 | }
|
---|
386 | else{
|
---|
387 | servo_min=SERVO_MIN_TCC();
|
---|
388 | servo_max=SERVO_MAX_TCC();
|
---|
389 | }
|
---|
390 | return map(readMicroseconds(), servo_min, servo_max, 0, 180);
|
---|
391 | }
|
---|
392 |
|
---|
393 | int Servo::readMicroseconds()
|
---|
394 | {
|
---|
395 | unsigned int pulsewidth;
|
---|
396 | if (this->servoIndex != INVALID_SERVO)
|
---|
397 | pulsewidth = servos[this->servoIndex].ticks;
|
---|
398 | else
|
---|
399 | pulsewidth = 0;
|
---|
400 |
|
---|
401 | return pulsewidth;
|
---|
402 | }
|
---|
403 |
|
---|
404 | bool Servo::attached()
|
---|
405 | {
|
---|
406 | return servos[this->servoIndex].Pin.isActive;
|
---|
407 | }
|
---|
408 |
|
---|
409 | #endif // ARDUINO_ARCH_SAM
|
---|
410 |
|
---|