rooty/Arduino
1
0
Fork 0
mirror of https://github.com/arduino/Arduino.git synced 2025-01-17 06:52:18 +01:00
Code Issues Releases Activity

Added Servo library for Arduino Due

Browse Source
This commit is contained in:
Cristian Maglie 2012-11-23 18:15:23 +01:00
parent db32419a34
commit db81f52c3a
6 changed files with 545 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
build/shared/revisions.txt
View File

@ -10,6 +10,9 @@
* sam: fixed BSoD on some Windows machine (louismdavis) * sam: fixed BSoD on some Windows machine (louismdavis)
* sam: added CANRX1/CANTX1 pins 88/89 (same physical pin for 66/53) * sam: added CANRX1/CANTX1 pins 88/89 (same physical pin for 66/53)
[libraries]
* sam: Added Servo library
ARDUINO BETA 1.5.1r2 - 2012.11.06 ARDUINO BETA 1.5.1r2 - 2012.11.06
* Fixed wrong release file for windows. * Fixed wrong release file for windows.

310
hardware/arduino/sam/libraries/Servo/Servo.cpp Normal file
View File

@ -0,0 +1,310 @@
/*
Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
Copyright (c) 2009 Michael Margolis. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/*
A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
The servos are pulsed in the background using the value most recently written using the write() method
Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
The methods are:
Servo - Class for manipulating servo motors connected to Arduino pins.
attach(pin ) - Attaches a servo motor to an i/o pin.
attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds
default min is 544, max is 2400
write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds)
writeMicroseconds() - Sets the servo pulse width in microseconds
read() - Gets the last written servo pulse width as an angle between 0 and 180.
readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
attached() - Returns true if there is a servo attached.
detach() - Stops an attached servos from pulsing its i/o pin.
*/
#include <Arduino.h>
#include "Servo.h"
#define usToTicks(_us) (( clockCyclesPerMicrosecond() * _us) / 32) // converts microseconds to tick
#define ticksToUs(_ticks) (( (unsigned)_ticks * 32)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
static servo_t servos[MAX_SERVOS]; // static array of servo structures
uint8_t ServoCount = 0; // the total number of attached servos
static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
// convenience macros
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
#define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
/************ static functions common to all instances ***********************/
//timer16_Sequence_t timer;
//------------------------------------------------------------------------------
/// Interrupt handler for the TC0 channel 1.
//------------------------------------------------------------------------------
void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel);
#if defined (_useTimer1)
void HANDLER_FOR_TIMER1(void) {
Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1);
}
#endif
#if defined (_useTimer2)
void HANDLER_FOR_TIMER2(void) {
Servo_Handler(_timer2, TC_FOR_TIMER2, CHANNEL_FOR_TIMER2);
}
#endif
#if defined (_useTimer3)
void HANDLER_FOR_TIMER3(void) {
Servo_Handler(_timer3, TC_FOR_TIMER3, CHANNEL_FOR_TIMER3);
}
#endif
#if defined (_useTimer4)
void HANDLER_FOR_TIMER4(void) {
Servo_Handler(_timer4, TC_FOR_TIMER4, CHANNEL_FOR_TIMER4);
}
#endif
#if defined (_useTimer5)
void HANDLER_FOR_TIMER5(void) {
Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5);
}
#endif
void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel)
{
// clear interrupt
tc->TC_CHANNEL[channel].TC_SR;
if (Channel[timer] < 0) {
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
} else {
if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true) {
digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
}
Channel[timer]++; // increment to the next channel
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks;
if(SERVO(timer,Channel[timer]).Pin.isActive == true) { // check if activated
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
}
}
else {
// finished all channels so wait for the refresh period to expire before starting over
if( (tc->TC_CHANNEL[channel].TC_CV) + 4 < usToTicks(REFRESH_INTERVAL) ) { // allow a few ticks to ensure the next OCR1A not missed
tc->TC_CHANNEL[channel].TC_RA = (unsigned int)usToTicks(REFRESH_INTERVAL);
}
else {
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed
}
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
}
static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn)
{
pmc_enable_periph_clk(id);
TC_Configure(tc, channel,
TC_CMR_TCCLKS_TIMER_CLOCK3 | // MCK/32
TC_CMR_WAVE | // Waveform mode
TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC
/* 84MHz, MCK/32, for 1.5ms: 3937 */
TC_SetRA(tc, channel, 2625); // 1ms
/* Configure and enable interrupt */
NVIC_EnableIRQ(irqn);
// TC_IER_CPAS: RA Compare
tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS;
// Enables the timer clock and performs a software reset to start the counting
TC_Start(tc, channel);
}
static void initISR(timer16_Sequence_t timer)
{
#if defined (_useTimer1)
if (timer == _timer1)
_initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
#endif
#if defined (_useTimer2)
if (timer == _timer2)
_initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
#endif
#if defined (_useTimer3)
if (timer == _timer3)
_initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3);
#endif
#if defined (_useTimer4)
if (timer == _timer4)
_initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4);
#endif
#if defined (_useTimer5)
if (timer == _timer5)
_initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5);
#endif
}
static void finISR(timer16_Sequence_t timer)
{
#if defined (_useTimer1)
TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1);
#endif
#if defined (_useTimer2)
TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2);
#endif
#if defined (_useTimer3)
TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3);
#endif
#if defined (_useTimer4)
TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4);
#endif
#if defined (_useTimer5)
TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5);
#endif
}
static boolean isTimerActive(timer16_Sequence_t timer)
{
// returns true if any servo is active on this timer
for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
if(SERVO(timer,channel).Pin.isActive == true)
return true;
}
return false;
}
/****************** end of static functions ******************************/
Servo::Servo()
{
if (ServoCount < MAX_SERVOS) {
this->servoIndex = ServoCount++; // assign a servo index to this instance
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
} else {
this->servoIndex = INVALID_SERVO; // too many servos
}
}
uint8_t Servo::attach(int pin)
{
return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
}
uint8_t Servo::attach(int pin, int min, int max)
{
timer16_Sequence_t timer;
if (this->servoIndex < MAX_SERVOS) {
pinMode(pin, OUTPUT); // set servo pin to output
servos[this->servoIndex].Pin.nbr = pin;
// todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
this->max = (MAX_PULSE_WIDTH - max)/4;
// initialize the timer if it has not already been initialized
timer = SERVO_INDEX_TO_TIMER(servoIndex);
if (isTimerActive(timer) == false) {
initISR(timer);
}
servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
}
return this->servoIndex;
}
void Servo::detach()
{
timer16_Sequence_t timer;
servos[this->servoIndex].Pin.isActive = false;
timer = SERVO_INDEX_TO_TIMER(servoIndex);
if(isTimerActive(timer) == false) {
finISR(timer);
}
}
void Servo::write(int value)
{
// treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
if (value < MIN_PULSE_WIDTH)
{
if (value < 0)
value = 0;
else if (value > 180)
value = 180;
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
}
writeMicroseconds(value);
}
void Servo::writeMicroseconds(int value)
{
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if( (channel < MAX_SERVOS) ) // ensure channel is valid
{
if (value < SERVO_MIN()) // ensure pulse width is valid
value = SERVO_MIN();
else if (value > SERVO_MAX())
value = SERVO_MAX();
value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead
servos[channel].ticks = value;
}
}
int Servo::read() // return the value as degrees
{
return map(readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
}
int Servo::readMicroseconds()
{
unsigned int pulsewidth;
if (this->servoIndex != INVALID_SERVO)
pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
else
pulsewidth = 0;
return pulsewidth;
}
bool Servo::attached()
{
return servos[this->servoIndex].Pin.isActive;
}

155
hardware/arduino/sam/libraries/Servo/Servo.h Normal file
View File

@ -0,0 +1,155 @@
/*
Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
Copyright (c) 2009 Michael Margolis. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/*
A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
The servos are pulsed in the background using the value most recently written using the write() method
Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
The sequence used to sieze timers is defined in timers.h
The methods are:
Servo - Class for manipulating servo motors connected to Arduino pins.
attach(pin ) - Attaches a servo motor to an i/o pin.
attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds
default min is 544, max is 2400
write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds)
writeMicroseconds() - Sets the servo pulse width in microseconds
read() - Gets the last written servo pulse width as an angle between 0 and 180.
readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
attached() - Returns true if there is a servo attached.
detach() - Stops an attached servos from pulsing its i/o pin.
*/
#ifndef Servo_h
#define Servo_h
#include <inttypes.h>
/*
* Defines for 16 bit timers used with Servo library
*
* If _useTimerX is defined then TimerX is a 16 bit timer on the current board
* timer16_Sequence_t enumerates the sequence that the timers should be allocated
* _Nbr_16timers indicates how many 16 bit timers are available.
*/
// For SAM3X:
#define _useTimer1
#define _useTimer2
#define _useTimer3
#define _useTimer4
#define _useTimer5
/*
TC0, chan 0 => TC0_Handler
TC0, chan 1 => TC1_Handler
TC0, chan 2 => TC2_Handler
TC1, chan 0 => TC3_Handler
TC1, chan 1 => TC4_Handler
TC1, chan 2 => TC5_Handler
TC2, chan 0 => TC6_Handler
TC2, chan 1 => TC7_Handler
TC2, chan 2 => TC8_Handler
*/
#if defined (_useTimer1)
#define TC_FOR_TIMER1 TC1
#define CHANNEL_FOR_TIMER1 0
#define ID_TC_FOR_TIMER1 ID_TC3
#define IRQn_FOR_TIMER1 TC3_IRQn
#define HANDLER_FOR_TIMER1 TC3_Handler
#endif
#if defined (_useTimer2)
#define TC_FOR_TIMER2 TC1
#define CHANNEL_FOR_TIMER2 1
#define ID_TC_FOR_TIMER2 ID_TC4
#define IRQn_FOR_TIMER2 TC4_IRQn
#define HANDLER_FOR_TIMER2 TC4_Handler
#endif
#if defined (_useTimer3)
#define TC_FOR_TIMER3 TC1
#define CHANNEL_FOR_TIMER3 2
#define ID_TC_FOR_TIMER3 ID_TC5
#define IRQn_FOR_TIMER3 TC5_IRQn
#define HANDLER_FOR_TIMER3 TC5_Handler
#endif
#if defined (_useTimer4)
#define TC_FOR_TIMER4 TC0
#define CHANNEL_FOR_TIMER4 2
#define ID_TC_FOR_TIMER4 ID_TC2
#define IRQn_FOR_TIMER4 TC2_IRQn
#define HANDLER_FOR_TIMER4 TC2_Handler
#endif
#if defined (_useTimer5)
#define TC_FOR_TIMER5 TC0
#define CHANNEL_FOR_TIMER5 0
#define ID_TC_FOR_TIMER5 ID_TC0
#define IRQn_FOR_TIMER5 TC0_IRQn
#define HANDLER_FOR_TIMER5 TC0_Handler
#endif
typedef enum { _timer1, _timer2, _timer3, _timer4, _timer5, _Nbr_16timers } timer16_Sequence_t ;
#define Servo_VERSION 2 // software version of this library
#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo (0.544ms)
#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo (2,4ms)
#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached (1.5ms)
#define REFRESH_INTERVAL 20000 // minimum time to refresh servos in microseconds (20ms)
#define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer
#define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER)
#define INVALID_SERVO 255 // flag indicating an invalid servo index
typedef struct {
uint8_t nbr :6 ; // a pin number from 0 to 63
uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false
} ServoPin_t ;
typedef struct {
ServoPin_t Pin;
volatile unsigned int ticks;
} servo_t;
class Servo
{
public:
Servo();
uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
void detach();
void write(int value); // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds
void writeMicroseconds(int value); // Write pulse width in microseconds
int read(); // returns current pulse width as an angle between 0 and 180 degrees
int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release)
bool attached(); // return true if this servo is attached, otherwise false
private:
uint8_t servoIndex; // index into the channel data for this servo
int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH
int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH
};
#endif

22
hardware/arduino/sam/libraries/Servo/examples/Knob/Knob.ino Normal file
View File

@ -0,0 +1,22 @@
// Controlling a servo position using a potentiometer (variable resistor)
// by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>
#include <Servo.h>
Servo myservo; // create servo object to control a servo
int potpin = 0; // analog pin used to connect the potentiometer
int val; // variable to read the value from the analog pin
void setup()
{
myservo.attach(9); // attaches the servo on pin 9 to the servo object
}
void loop()
{
val = analogRead(potpin); // reads the value of the potentiometer (value between 0 and 1023)
val = map(val, 0, 1023, 0, 179); // scale it to use it with the servo (value between 0 and 180)
myservo.write(val); // sets the servo position according to the scaled value
delay(15); // waits for the servo to get there
}

31
hardware/arduino/sam/libraries/Servo/examples/Sweep/Sweep.ino Normal file
View File

@ -0,0 +1,31 @@
// Sweep
// by BARRAGAN <http://barraganstudio.com>
// This example code is in the public domain.
#include <Servo.h>
Servo myservo; // create servo object to control a servo
// a maximum of eight servo objects can be created
int pos = 0; // variable to store the servo position
void setup()
{
myservo.attach(9); // attaches the servo on pin 9 to the servo object
}
void loop()
{
for(pos = 0; pos < 180; pos += 1) // goes from 0 degrees to 180 degrees
{ // in steps of 1 degree
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
for(pos = 180; pos>=1; pos-=1) // goes from 180 degrees to 0 degrees
{
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}

24
hardware/arduino/sam/libraries/Servo/keywords.txt Normal file
View File

@ -0,0 +1,24 @@
#######################################
# Syntax Coloring Map Servo
#######################################
#######################################
# Datatypes (KEYWORD1)
#######################################
Servo KEYWORD1
#######################################
# Methods and Functions (KEYWORD2)
#######################################
attach KEYWORD2
detach KEYWORD2
write KEYWORD2
read KEYWORD2
attached KEYWORD2
writeMicroseconds KEYWORD2
readMicroseconds KEYWORD2
#######################################
# Constants (LITERAL1)
#######################################