Skip to content

Commit db81f52

Browse files
cmagliecmaglie
committed
Added Servo library for Arduino Due
1 parent db32419 commit db81f52

File tree

6 files changed

+545
-0
lines changed

6 files changed

+545
-0
lines changed

‎build/shared/revisions.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,9 @@
1010
* sam: fixed BSoD on some Windows machine (louismdavis)
1111
* sam: added CANRX1/CANTX1 pins 88/89 (same physical pin for 66/53)
1212

13+
[libraries]
14+
* sam: Added Servo library
15+
1316
ARDUINO BETA 1.5.1r2 - 2012.11.06
1417

1518
* Fixed wrong release file for windows.

‎hardware/arduino/sam/libraries/Servo/Servo.cpp

Lines changed: 310 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,310 @@
1+
/*
2+
Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
3+
Copyright (c) 2009 Michael Margolis. All right reserved.
4+
5+
This library is free software; you can redistribute it and/or
6+
modify it under the terms of the GNU Lesser General Public
7+
License as published by the Free Software Foundation; either
8+
version 2.1 of the License, or (at your option) any later version.
9+
10+
This library is distributed in the hope that it will be useful,
11+
but WITHOUT ANY WARRANTY; without even the implied warranty of
12+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13+
Lesser General Public License for more details.
14+
15+
You should have received a copy of the GNU Lesser General Public
16+
License along with this library; if not, write to the Free Software
17+
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
18+
*/
19+
20+
/*
21+
A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
22+
The servos are pulsed in the background using the value most recently written using the write() method
23+
24+
Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
25+
Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
26+
27+
The methods are:
28+
29+
Servo - Class for manipulating servo motors connected to Arduino pins.
30+
31+
attach(pin ) - Attaches a servo motor to an i/o pin.
32+
attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds
33+
default min is 544, max is 2400
34+
35+
write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds)
36+
writeMicroseconds() - Sets the servo pulse width in microseconds
37+
read() - Gets the last written servo pulse width as an angle between 0 and 180.
38+
readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
39+
attached() - Returns true if there is a servo attached.
40+
detach() - Stops an attached servos from pulsing its i/o pin.
41+
42+
*/
43+
44+
#include <Arduino.h>
45+
#include "Servo.h"
46+
47+
#define usToTicks(_us) (( clockCyclesPerMicrosecond() * _us) / 32) // converts microseconds to tick
48+
#define ticksToUs(_ticks) (( (unsigned)_ticks * 32)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
49+
50+
51+
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
52+
53+
static servo_t servos[MAX_SERVOS]; // static array of servo structures
54+
55+
uint8_t ServoCount = 0; // the total number of attached servos
56+
57+
static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
58+
59+
// convenience macros
60+
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
61+
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
62+
#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
63+
#define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
64+
65+
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
66+
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
67+
68+
/************ static functions common to all instances ***********************/
69+
70+
71+
//timer16_Sequence_t timer;
72+
73+
//------------------------------------------------------------------------------
74+
/// Interrupt handler for the TC0 channel 1.
75+
//------------------------------------------------------------------------------
76+
void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel);
77+
#if defined (_useTimer1)
78+
void HANDLER_FOR_TIMER1(void) {
79+
Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1);
80+
}
81+
#endif
82+
#if defined (_useTimer2)
83+
void HANDLER_FOR_TIMER2(void) {
84+
Servo_Handler(_timer2, TC_FOR_TIMER2, CHANNEL_FOR_TIMER2);
85+
}
86+
#endif
87+
#if defined (_useTimer3)
88+
void HANDLER_FOR_TIMER3(void) {
89+
Servo_Handler(_timer3, TC_FOR_TIMER3, CHANNEL_FOR_TIMER3);
90+
}
91+
#endif
92+
#if defined (_useTimer4)
93+
void HANDLER_FOR_TIMER4(void) {
94+
Servo_Handler(_timer4, TC_FOR_TIMER4, CHANNEL_FOR_TIMER4);
95+
}
96+
#endif
97+
#if defined (_useTimer5)
98+
void HANDLER_FOR_TIMER5(void) {
99+
Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5);
100+
}
101+
#endif
102+
103+
void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel)
104+
{
105+
// clear interrupt
106+
tc->TC_CHANNEL[channel].TC_SR;
107+
if (Channel[timer] < 0) {
108+
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
109+
} else {
110+
if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true) {
111+
digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
112+
}
113+
}
114+
115+
Channel[timer]++; // increment to the next channel
116+
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
117+
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks;
118+
if(SERVO(timer,Channel[timer]).Pin.isActive == true) { // check if activated
119+
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
120+
}
121+
}
122+
else {
123+
// finished all channels so wait for the refresh period to expire before starting over
124+
if( (tc->TC_CHANNEL[channel].TC_CV) + 4 < usToTicks(REFRESH_INTERVAL) ) { // allow a few ticks to ensure the next OCR1A not missed
125+
tc->TC_CHANNEL[channel].TC_RA = (unsigned int)usToTicks(REFRESH_INTERVAL);
126+
}
127+
else {
128+
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed
129+
}
130+
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
131+
}
132+
}
133+
134+
static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn)
135+
{
136+
pmc_enable_periph_clk(id);
137+
TC_Configure(tc, channel,
138+
TC_CMR_TCCLKS_TIMER_CLOCK3 | // MCK/32
139+
TC_CMR_WAVE | // Waveform mode
140+
TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC
141+
142+
/* 84MHz, MCK/32, for 1.5ms: 3937 */
143+
TC_SetRA(tc, channel, 2625); // 1ms
144+
145+
/* Configure and enable interrupt */
146+
NVIC_EnableIRQ(irqn);
147+
// TC_IER_CPAS: RA Compare
148+
tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS;
149+
150+
// Enables the timer clock and performs a software reset to start the counting
151+
TC_Start(tc, channel);
152+
}
153+
154+
static void initISR(timer16_Sequence_t timer)
155+
{
156+
#if defined (_useTimer1)
157+
if (timer == _timer1)
158+
_initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
159+
#endif
160+
#if defined (_useTimer2)
161+
if (timer == _timer2)
162+
_initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
163+
#endif
164+
#if defined (_useTimer3)
165+
if (timer == _timer3)
166+
_initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3);
167+
#endif
168+
#if defined (_useTimer4)
169+
if (timer == _timer4)
170+
_initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4);
171+
#endif
172+
#if defined (_useTimer5)
173+
if (timer == _timer5)
174+
_initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5);
175+
#endif
176+
}
177+
178+
static void finISR(timer16_Sequence_t timer)
179+
{
180+
#if defined (_useTimer1)
181+
TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1);
182+
#endif
183+
#if defined (_useTimer2)
184+
TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2);
185+
#endif
186+
#if defined (_useTimer3)
187+
TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3);
188+
#endif
189+
#if defined (_useTimer4)
190+
TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4);
191+
#endif
192+
#if defined (_useTimer5)
193+
TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5);
194+
#endif
195+
}
196+
197+
198+
static boolean isTimerActive(timer16_Sequence_t timer)
199+
{
200+
// returns true if any servo is active on this timer
201+
for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
202+
if(SERVO(timer,channel).Pin.isActive == true)
203+
return true;
204+
}
205+
return false;
206+
}
207+
208+
/****************** end of static functions ******************************/
209+
210+
Servo::Servo()
211+
{
212+
if (ServoCount < MAX_SERVOS) {
213+
this->servoIndex = ServoCount++; // assign a servo index to this instance
214+
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
215+
} else {
216+
this->servoIndex = INVALID_SERVO; // too many servos
217+
}
218+
}
219+
220+
uint8_t Servo::attach(int pin)
221+
{
222+
return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
223+
}
224+
225+
uint8_t Servo::attach(int pin, int min, int max)
226+
{
227+
timer16_Sequence_t timer;
228+
229+
if (this->servoIndex < MAX_SERVOS) {
230+
pinMode(pin, OUTPUT); // set servo pin to output
231+
servos[this->servoIndex].Pin.nbr = pin;
232+
// todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
233+
this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
234+
this->max = (MAX_PULSE_WIDTH - max)/4;
235+
// initialize the timer if it has not already been initialized
236+
timer = SERVO_INDEX_TO_TIMER(servoIndex);
237+
if (isTimerActive(timer) == false) {
238+
initISR(timer);
239+
}
240+
servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
241+
}
242+
return this->servoIndex;
243+
}
244+
245+
void Servo::detach()
246+
{
247+
timer16_Sequence_t timer;
248+
249+
servos[this->servoIndex].Pin.isActive = false;
250+
timer = SERVO_INDEX_TO_TIMER(servoIndex);
251+
if(isTimerActive(timer) == false) {
252+
finISR(timer);
253+
}
254+
}
255+
256+
void Servo::write(int value)
257+
{
258+
// treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
259+
if (value < MIN_PULSE_WIDTH)
260+
{
261+
if (value < 0)
262+
value = 0;
263+
else if (value > 180)
264+
value = 180;
265+
266+
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
267+
}
268+
writeMicroseconds(value);
269+
}
270+
271+
void Servo::writeMicroseconds(int value)
272+
{
273+
// calculate and store the values for the given channel
274+
byte channel = this->servoIndex;
275+
if( (channel < MAX_SERVOS) ) // ensure channel is valid
276+
{
277+
if (value < SERVO_MIN()) // ensure pulse width is valid
278+
value = SERVO_MIN();
279+
else if (value > SERVO_MAX())
280+
value = SERVO_MAX();
281+
282+
value = value - TRIM_DURATION;
283+
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead
284+
servos[channel].ticks = value;
285+
}
286+
}
287+
288+
int Servo::read() // return the value as degrees
289+
{
290+
return map(readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
291+
}
292+
293+
int Servo::readMicroseconds()
294+
{
295+
unsigned int pulsewidth;
296+
if (this->servoIndex != INVALID_SERVO)
297+
pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
298+
else
299+
pulsewidth = 0;
300+
301+
return pulsewidth;
302+
}
303+
304+
bool Servo::attached()
305+
{
306+
return servos[this->servoIndex].Pin.isActive;
307+
}
308+
309+
310+

0 commit comments

Comments
 (0)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy