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

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

ライブラリとOS及びベーシックなサンプルの追加．
File size: 6.6 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
19#if defined(ARDUINO_ARCH_SAMD)
20
21#include <Arduino.h>
22#include <Servo.h>
23
24static servo_t servos[MAX_SERVOS]; // static array of servo structures
25
26uint8_t ServoCount = 0; // the total number of attached servos
27Tcc* TCCx;
28uint8_t Channelx = 0;
29
30// convenience macros
31
32#define SERVO_MIN() (MIN_PULSE_WIDTH_SAMD) // minimum value in uS for this servo
33#define SERVO_MAX() (MAX_PULSE_WIDTH_SAMD) // maximum value in uS for this servo
34
35/************ static functions common to all instances ***********************/
36
37//------------------------------------------------------------------------------
38/// Interrupt handler for the TC0 channel 1.
39//------------------------------------------------------------------------------
40
41
42/****************** end of static functions ******************************/
43
44Servo::Servo()
45{
46 if (ServoCount < MAX_SERVOS) {
47 this->servoIndex = ServoCount++; // assign a servo index to this instance
48 } else { //su questo costruttore forse si deve tornare
49 this->servoIndex = INVALID_SERVO; // too many servos
50 }
51}
52
53uint8_t Servo::attach(int pin)
54{
55 return this->attach(pin, MIN_PULSE_WIDTH_SAMD, MAX_PULSE_WIDTH_SAMD);
56}
57
58uint8_t Servo::attach(int pin, int min, int max)
59{
60
61
62 if (this->servoIndex < MAX_SERVOS) {
63 pinMode(pin, OUTPUT); // set servo pin to output
64 servos[this->servoIndex].Pin.nbr = pin;
65 if(min > MIN_PULSE_WIDTH_SAMD) min = MIN_PULSE_WIDTH_SAMD;
66 if (max > MAX_PULSE_WIDTH_SAMD) max = MAX_PULSE_WIDTH_SAMD;
67 this->min = min;
68 this->max = max;
69
70 switch(pin)
71 {
72 case 2:
73 {
74 pinPeripheral(pin, g_APinDescription[pin].ulPinType);
75 TCCx=TCC0;
76 Channelx=0;
77 }
78 break;
79
80 case 3:
81 {
82 pinPeripheral(pin, g_APinDescription[pin].ulPinType);
83 TCCx=TCC0;
84 Channelx=1;
85 }
86 break;
87
88 case 6:
89 {
90 pinPeripheral(pin, g_APinDescription[pin].ulPinType);
91 TCCx=TCC0;
92 Channelx=2;
93 }
94 break;
95
96 case 7:
97 {
98 pinPeripheral(pin, g_APinDescription[pin].ulPinType);
99 TCCx=TCC0;
100 Channelx=3;
101 }
102 break;
103
104 case 8:
105 {
106 pinPeripheral(pin, g_APinDescription[pin].ulPinType);
107 TCCx=TCC1;
108 Channelx=0;
109 }
110 break;
111
112 case 9:
113 {
114 pinPeripheral(pin, g_APinDescription[pin].ulPinType);
115 TCCx=TCC1;
116 Channelx=1;
117 }
118 break;
119
120 case 11:
121 {
122 pinPeripheral(pin, g_APinDescription[pin].ulPinType);
123 TCCx=TCC2;
124 Channelx=0;
125 }
126 break;
127
128 case 13:
129 {
130 pinPeripheral(pin, g_APinDescription[pin].ulPinType);
131 TCCx=TCC2;
132 Channelx=1;
133 }
134 break;
135
136 default:
137 break;
138
139 }
140
141 if ((TCCx==TCC0) | (TCCx==TCC1)) GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC0_TCC1 )) ;
142 else if(TCCx==TCC2) GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC2_TC3 )) ;
143 else;
144
145 if(servos[this->servoIndex].Pin.isActive == false)
146 {
147 TCCx->CTRLA.reg &=~(TCC_CTRLA_ENABLE); //disable TCC module
148 TCCx->CTRLA.reg |=TCC_CTRLA_PRESCALER_DIV8; //setting prescaler to divide by 8
149 TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM; //Set TCCx as normal PWM
150 TCCx->CC[Channelx].reg=1500; //default value for servo position
151 TCCx->PER.reg=20000; // setting servo frequency (50 hz)
152 TCCx->CTRLA.reg |= TCC_CTRLA_ENABLE ; //ENABLE TCCx
153 servos[this->servoIndex].Pin.isActive = true;
154 }
155
156 //servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
157 }
158 return this->servoIndex;
159}
160
161void Servo::detach()
162{
163
164
165 servos[this->servoIndex].Pin.isActive = false;
166 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);
167 else if((servos[this->servoIndex].Pin.nbr == 8) | (servos[this->servoIndex].Pin.nbr == 9)) TCC1->CTRLA.reg &=~(TCC_CTRLA_ENABLE);
168 else if ((servos[this->servoIndex].Pin.nbr == 11) | (servos[this->servoIndex].Pin.nbr == 13)) TCC2->CTRLA.reg &=~(TCC_CTRLA_ENABLE);
169}
170
171void Servo::write(int value)
172{
173 // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
174 if (value < MIN_PULSE_WIDTH)
175 {
176 if (value < 0)
177 value = 0;
178 else if (value > 180)
179 value = 180;
180
181 value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
182 }
183 writeMicroseconds(value);
184}
185
186void Servo::writeMicroseconds(int value)
187{
188 // calculate and store the values for the given channel
189 byte channel = this->servoIndex;
190 if( (channel < MAX_SERVOS) ) // ensure channel is valid
191 {
192 if (value < SERVO_MIN()) // ensure pulse width is valid
193 value = SERVO_MIN();
194 else if (value > SERVO_MAX())
195 value = SERVO_MAX();
196 servos[this->servoIndex].ticks = value; //da sistemare
197 switch(servos[this->servoIndex].Pin.nbr)
198 {
199 case 2:
200 TCC0->CC[0].reg=value;
201 break;
202
203 case 3:
204 TCC0->CC[1].reg=value;
205 break;
206
207 case 6:
208 TCC0->CC[2].reg=value;
209 break;
210
211 case 7:
212 TCC0->CC[3].reg=value;
213 break;
214
215 case 8:
216 TCC1->CC[0].reg=value;
217 break;
218
219 case 9:
220 TCC1->CC[1].reg=value;
221 break;
222
223 case 11:
224 TCC2->CC[0].reg=value;
225 break;
226
227 case 13:
228 TCC2->CC[1].reg=value;
229 break;
230
231 default:
232 break;
233
234 }
235
236 //servos[this->servoIndex].ticks = value; //da sistemare
237 //servos[channel].ticks = value;
238 }
239}
240
241int Servo::read() // return the value as degrees
242{
243 return map(readMicroseconds(), SERVO_MIN(), SERVO_MAX(), 0, 180);
244}
245
246int Servo::readMicroseconds()
247{
248 unsigned int pulsewidth;
249 if (this->servoIndex != INVALID_SERVO)
250 pulsewidth = servos[this->servoIndex].ticks;
251 else
252 pulsewidth = 0;
253
254 return pulsewidth;
255}
256
257bool Servo::attached()
258{
259 return servos[this->servoIndex].Pin.isActive;
260}
261
262#endif // ARDUINO_ARCH_SAM
263
Note: See TracBrowser for help on using the repository browser.