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 |
|
---|
24 | static servo_t servos[MAX_SERVOS]; // static array of servo structures
|
---|
25 |
|
---|
26 | uint8_t ServoCount = 0; // the total number of attached servos
|
---|
27 | Tcc* TCCx;
|
---|
28 | uint8_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 |
|
---|
44 | Servo::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 |
|
---|
53 | uint8_t Servo::attach(int pin)
|
---|
54 | {
|
---|
55 | return this->attach(pin, MIN_PULSE_WIDTH_SAMD, MAX_PULSE_WIDTH_SAMD);
|
---|
56 | }
|
---|
57 |
|
---|
58 | uint8_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 |
|
---|
161 | void 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 |
|
---|
171 | void 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 |
|
---|
186 | void 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 |
|
---|
241 | int Servo::read() // return the value as degrees
|
---|
242 | {
|
---|
243 | return map(readMicroseconds(), SERVO_MIN(), SERVO_MAX(), 0, 180);
|
---|
244 | }
|
---|
245 |
|
---|
246 | int 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 |
|
---|
257 | bool Servo::attached()
|
---|
258 | {
|
---|
259 | return servos[this->servoIndex].Pin.isActive;
|
---|
260 | }
|
---|
261 |
|
---|
262 | #endif // ARDUINO_ARCH_SAM
|
---|
263 |
|
---|