From db81f52c3a27784fb44ca1ad6c77f9b1f0a4ef7d Mon Sep 17 00:00:00 2001 From: Cristian Maglie Date: Fri, 23 Nov 2012 18:15:23 +0100 Subject: [PATCH] Added Servo library for Arduino Due --- build/shared/revisions.txt | 3 + .../arduino/sam/libraries/Servo/Servo.cpp | 310 ++++++++++++++++++ hardware/arduino/sam/libraries/Servo/Servo.h | 155 +++++++++ .../libraries/Servo/examples/Knob/Knob.ino | 22 ++ .../libraries/Servo/examples/Sweep/Sweep.ino | 31 ++ .../arduino/sam/libraries/Servo/keywords.txt | 24 ++ 6 files changed, 545 insertions(+) create mode 100644 hardware/arduino/sam/libraries/Servo/Servo.cpp create mode 100644 hardware/arduino/sam/libraries/Servo/Servo.h create mode 100644 hardware/arduino/sam/libraries/Servo/examples/Knob/Knob.ino create mode 100644 hardware/arduino/sam/libraries/Servo/examples/Sweep/Sweep.ino create mode 100644 hardware/arduino/sam/libraries/Servo/keywords.txt diff --git a/build/shared/revisions.txt b/build/shared/revisions.txt index d0a31750c0b..d0e78fffbe4 100644 --- a/build/shared/revisions.txt +++ b/build/shared/revisions.txt @@ -10,6 +10,9 @@ * sam: fixed BSoD on some Windows machine (louismdavis) * 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 * Fixed wrong release file for windows. diff --git a/hardware/arduino/sam/libraries/Servo/Servo.cpp b/hardware/arduino/sam/libraries/Servo/Servo.cpp new file mode 100644 index 00000000000..756e926624f --- /dev/null +++ b/hardware/arduino/sam/libraries/Servo/Servo.cpp @@ -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 +#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; +} + + + diff --git a/hardware/arduino/sam/libraries/Servo/Servo.h b/hardware/arduino/sam/libraries/Servo/Servo.h new file mode 100644 index 00000000000..4fdaf9c18d1 --- /dev/null +++ b/hardware/arduino/sam/libraries/Servo/Servo.h @@ -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 + +/* + * 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 diff --git a/hardware/arduino/sam/libraries/Servo/examples/Knob/Knob.ino b/hardware/arduino/sam/libraries/Servo/examples/Knob/Knob.ino new file mode 100644 index 00000000000..886e107f82a --- /dev/null +++ b/hardware/arduino/sam/libraries/Servo/examples/Knob/Knob.ino @@ -0,0 +1,22 @@ +// Controlling a servo position using a potentiometer (variable resistor) +// by Michal Rinott + +#include + +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 +} diff --git a/hardware/arduino/sam/libraries/Servo/examples/Sweep/Sweep.ino b/hardware/arduino/sam/libraries/Servo/examples/Sweep/Sweep.ino new file mode 100644 index 00000000000..fb326e7e784 --- /dev/null +++ b/hardware/arduino/sam/libraries/Servo/examples/Sweep/Sweep.ino @@ -0,0 +1,31 @@ +// Sweep +// by BARRAGAN +// This example code is in the public domain. + + +#include + +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 + } +} diff --git a/hardware/arduino/sam/libraries/Servo/keywords.txt b/hardware/arduino/sam/libraries/Servo/keywords.txt new file mode 100644 index 00000000000..ca5ba79e39c --- /dev/null +++ b/hardware/arduino/sam/libraries/Servo/keywords.txt @@ -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) +#######################################