source: rtos_arduino/trunk/arduino_lib/libraries/Servo/src/samd/Servo.cpp

Last change on this file was 224, checked in by ertl-honda, 8 years ago

1.7.10のファイルに更新
File size: 10.2 KB
Line 
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
26static servo_t servos[MAX_SERVOS]; // static array of servo structures
27
28uint8_t ServoCount = 0; // the total number of attached servos
29uint8_t isTC = 0 ;
30Tc* TCx ;
31Tcc* TCCx;
32uint8_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
50Servo::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
59uint8_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
69uint8_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
249void 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
260void 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
295void 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
377int 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
393int 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
404bool Servo::attached()
405{
406 return servos[this->servoIndex].Pin.isActive;
407}
408
409#endif // ARDUINO_ARCH_SAM
410
Note: See TracBrowser for help on using the repository browser.