diff --git a/Servo/README.adoc b/Servo/README.adoc new file mode 100644 index 0000000..dd3f0ba --- /dev/null +++ b/Servo/README.adoc @@ -0,0 +1,25 @@ += Servo Library for Arduino = + +This library allows an Arduino board to control RC (hobby) servo motors. + +For more information about this library please visit us at +http://www.arduino.cc/en/Reference/Servo + +== License == + +Copyright (c) 2013 Arduino LLC. All right reserved. +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 diff --git a/Servo/examples/Knob/Knob.ino b/Servo/examples/Knob/Knob.ino new file mode 100644 index 0000000..0db8770 --- /dev/null +++ b/Servo/examples/Knob/Knob.ino @@ -0,0 +1,27 @@ +/* + Controlling a servo position using a potentiometer (variable resistor) + by Michal Rinott + + modified on 8 Nov 2013 + by Scott Fitzgerald + http://www.arduino.cc/en/Tutorial/Knob +*/ + +#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, 180); // 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/Servo/examples/Sweep/Sweep.ino b/Servo/examples/Sweep/Sweep.ino new file mode 100644 index 0000000..df904af --- /dev/null +++ b/Servo/examples/Sweep/Sweep.ino @@ -0,0 +1,32 @@ +/* Sweep + by BARRAGAN + This example code is in the public domain. + + modified 8 Nov 2013 + by Scott Fitzgerald + http://www.arduino.cc/en/Tutorial/Sweep +*/ + +#include + +Servo myservo; // create servo object to control a servo +// twelve servo objects can be created on most boards + +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 >= 0; 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/Servo/keywords.txt b/Servo/keywords.txt new file mode 100644 index 0000000..0a7ca1e --- /dev/null +++ b/Servo/keywords.txt @@ -0,0 +1,24 @@ +####################################### +# Syntax Coloring Map Servo +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +Servo KEYWORD1 Servo + +####################################### +# Methods and Functions (KEYWORD2) +####################################### +attach KEYWORD2 +detach KEYWORD2 +write KEYWORD2 +read KEYWORD2 +attached KEYWORD2 +writeMicroseconds KEYWORD2 +readMicroseconds KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### diff --git a/Servo/library.properties b/Servo/library.properties new file mode 100644 index 0000000..94dfc92 --- /dev/null +++ b/Servo/library.properties @@ -0,0 +1,9 @@ +name=Servo +version=1.1.2 +author=Michael Margolis, Arduino +maintainer=Arduino +sentence=Allows Arduino/Genuino boards to control a variety of servo motors. +paragraph=This library can control a great number of servos.
It makes careful use of timers: the library can control 12 servos using only 1 timer.
On the Arduino Due you can control up to 60 servos.
+category=Device Control +url=http://www.arduino.cc/en/Reference/Servo +architectures=avr,sam,samd,nrf52,stm32f4 diff --git a/Servo/src/Servo.h b/Servo/src/Servo.h new file mode 100644 index 0000000..4890a14 --- /dev/null +++ b/Servo/src/Servo.h @@ -0,0 +1,119 @@ +/* + 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. + */ + +// Architecture specific include +#if defined(ARDUINO_ARCH_AVR) +#include "avr/ServoTimers.h" +#elif defined(ARDUINO_ARCH_SAM) +#include "sam/ServoTimers.h" +#elif defined(ARDUINO_ARCH_SAMD) +#include "samd/ServoTimers.h" +#elif defined(ARDUINO_ARCH_STM32F4) +#include "stm32f4/ServoTimers.h" +#elif defined(ARDUINO_ARCH_NRF52) +#include "nrf52/ServoTimers.h" +#else +#error "This library only supports boards with an AVR, SAM, SAMD, NRF52 or STM32F4 processor." +#endif + +#define Servo_VERSION 2 // software version of this library + +#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo +#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo +#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached +#define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds + +#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 + +#if !defined(ARDUINO_ARCH_STM32F4) + +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 +#endif diff --git a/Servo/src/avr/Servo.cpp b/Servo/src/avr/Servo.cpp new file mode 100644 index 0000000..ed7376e --- /dev/null +++ b/Servo/src/avr/Servo.cpp @@ -0,0 +1,318 @@ +/* + 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 + */ + +#if defined(ARDUINO_ARCH_AVR) + +#include +#include + +#include "Servo.h" + +#define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009 +#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds + + +#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009 + +//#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER) + +static servo_t servos[MAX_SERVOS]; // static array of servo structures +static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval) + +uint8_t ServoCount = 0; // the total number of attached servos + + +// 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 ***********************/ + +static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) +{ + if( Channel[timer] < 0 ) + *TCNTn = 0; // 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) { + *OCRnA = *TCNTn + 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( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed + *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL); + else + *OCRnA = *TCNTn + 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 + } +} + +#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform +// Interrupt handlers for Arduino +#if defined(_useTimer1) +SIGNAL (TIMER1_COMPA_vect) +{ + handle_interrupts(_timer1, &TCNT1, &OCR1A); +} +#endif + +#if defined(_useTimer3) +SIGNAL (TIMER3_COMPA_vect) +{ + handle_interrupts(_timer3, &TCNT3, &OCR3A); +} +#endif + +#if defined(_useTimer4) +SIGNAL (TIMER4_COMPA_vect) +{ + handle_interrupts(_timer4, &TCNT4, &OCR4A); +} +#endif + +#if defined(_useTimer5) +SIGNAL (TIMER5_COMPA_vect) +{ + handle_interrupts(_timer5, &TCNT5, &OCR5A); +} +#endif + +#elif defined WIRING +// Interrupt handlers for Wiring +#if defined(_useTimer1) +void Timer1Service() +{ + handle_interrupts(_timer1, &TCNT1, &OCR1A); +} +#endif +#if defined(_useTimer3) +void Timer3Service() +{ + handle_interrupts(_timer3, &TCNT3, &OCR3A); +} +#endif +#endif + + +static void initISR(timer16_Sequence_t timer) +{ +#if defined (_useTimer1) + if(timer == _timer1) { + TCCR1A = 0; // normal counting mode + TCCR1B = _BV(CS11); // set prescaler of 8 + TCNT1 = 0; // clear the timer count +#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__) + TIFR |= _BV(OCF1A); // clear any pending interrupts; + TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt +#else + // here if not ATmega8 or ATmega128 + TIFR1 |= _BV(OCF1A); // clear any pending interrupts; + TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt +#endif +#if defined(WIRING) + timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service); +#endif + } +#endif + +#if defined (_useTimer3) + if(timer == _timer3) { + TCCR3A = 0; // normal counting mode + TCCR3B = _BV(CS31); // set prescaler of 8 + TCNT3 = 0; // clear the timer count +#if defined(__AVR_ATmega128__) + TIFR |= _BV(OCF3A); // clear any pending interrupts; + ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt +#else + TIFR3 = _BV(OCF3A); // clear any pending interrupts; + TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt +#endif +#if defined(WIRING) + timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only +#endif + } +#endif + +#if defined (_useTimer4) + if(timer == _timer4) { + TCCR4A = 0; // normal counting mode + TCCR4B = _BV(CS41); // set prescaler of 8 + TCNT4 = 0; // clear the timer count + TIFR4 = _BV(OCF4A); // clear any pending interrupts; + TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt + } +#endif + +#if defined (_useTimer5) + if(timer == _timer5) { + TCCR5A = 0; // normal counting mode + TCCR5B = _BV(CS51); // set prescaler of 8 + TCNT5 = 0; // clear the timer count + TIFR5 = _BV(OCF5A); // clear any pending interrupts; + TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt + } +#endif +} + +static void finISR(timer16_Sequence_t timer) +{ + //disable use of the given timer +#if defined WIRING // Wiring + if(timer == _timer1) { + #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) + TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt + #else + TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt + #endif + timerDetach(TIMER1OUTCOMPAREA_INT); + } + else if(timer == _timer3) { + #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) + TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt + #else + ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt + #endif + timerDetach(TIMER3OUTCOMPAREA_INT); + } +#else + //For arduino - in future: call here to a currently undefined function to reset the timer + (void) timer; // squash "unused parameter 'timer' [-Wunused-parameter]" warning +#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 - 12 Aug 2009 + } + 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) +{ + 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 + timer16_Sequence_t 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() +{ + servos[this->servoIndex].Pin.isActive = false; + timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + if(isTimerActive(timer) == false) { + finISR(timer); + } +} + +void Servo::write(int value) +{ + if(value < MIN_PULSE_WIDTH) + { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds) + if(value < 0) value = 0; + if(value > 180) value = 180; + value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX()); + } + this->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 - 12 Aug 2009 + + uint8_t oldSREG = SREG; + cli(); + servos[channel].ticks = value; + SREG = oldSREG; + } +} + +int Servo::read() // return the value as degrees +{ + return map( this->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 ; // 12 aug 2009 + else + pulsewidth = 0; + + return pulsewidth; +} + +bool Servo::attached() +{ + return servos[this->servoIndex].Pin.isActive ; +} + +#endif // ARDUINO_ARCH_AVR + diff --git a/Servo/src/avr/ServoTimers.h b/Servo/src/avr/ServoTimers.h new file mode 100644 index 0000000..9794c8e --- /dev/null +++ b/Servo/src/avr/ServoTimers.h @@ -0,0 +1,59 @@ +/* + 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 +*/ + +/* + * 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. + */ + +/** + * AVR Only definitions + * -------------------- + */ + +// Say which 16 bit timers can be used and in what order +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) +#define _useTimer5 +#define _useTimer1 +#define _useTimer3 +#define _useTimer4 +typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t; + +#elif defined(__AVR_ATmega32U4__) +#define _useTimer1 +typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t; + +#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) +#define _useTimer3 +#define _useTimer1 +typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t; + +#elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega2561__) +#define _useTimer3 +#define _useTimer1 +typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t; + +#else // everything else +#define _useTimer1 +typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t; +#endif + diff --git a/Servo/src/mbed/Servo.cpp b/Servo/src/mbed/Servo.cpp new file mode 100644 index 0000000..85a9be8 --- /dev/null +++ b/Servo/src/mbed/Servo.cpp @@ -0,0 +1,132 @@ +#if defined(ARDUINO_ARCH_MBED) + +#include +#include + +class ServoImpl { + mbed::DigitalOut *pin; + mbed::Timeout timeout; // calls a callback once when a timeout expires + mbed::Ticker ticker; // calls a callback repeatedly with a timeout + +public: + ServoImpl(PinName _pin) { + pin = new mbed::DigitalOut(_pin); + } + + ~ServoImpl() { + ticker.detach(); + timeout.detach(); + delete pin; + } + + void start(uint32_t duration_us) { + duration = duration_us; + ticker.attach(mbed::callback(this, &ServoImpl::call), 0.02f); + } + + void call() { + timeout.attach(mbed::callback(this, &ServoImpl::toggle), duration / 1e6); + toggle(); + } + + void toggle() { + *pin = !*pin; + } + + int32_t duration = -1; +}; + +static ServoImpl* servos[MAX_SERVOS]; // static array of servo structures +uint8_t ServoCount = 0; // the total number of attached servos + +#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min) // minimum value in uS for this servo +#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max) // maximum value in uS for this servo + +#define TRIM_DURATION 15 //callback overhead (35 uS) -> 15uS if toggle() is called after starting the timeout + +Servo::Servo() +{ + if (ServoCount < MAX_SERVOS) { + this->servoIndex = ServoCount++; + } 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) +{ + pinMode(pin, OUTPUT); // set servo pin to output + servos[this->servoIndex] = new ServoImpl(digitalPinToPinName(pin)); + + this->min = (MIN_PULSE_WIDTH - min); + this->max = (MAX_PULSE_WIDTH - max); + return this->servoIndex; +} + +void Servo::detach() +{ + delete servos[this->servoIndex]; + servos[this->servoIndex] = NULL; +} + +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) +{ + if (!servos[this->servoIndex]) { + return; + } + // 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; + if (servos[this->servoIndex]->duration == -1) { + servos[this->servoIndex]->start(value); + } + servos[this->servoIndex]->duration = value; + } +} + +int Servo::read() // return the value as degrees +{ + return map(readMicroseconds(), SERVO_MIN(), SERVO_MAX(), 0, 180); +} + +int Servo::readMicroseconds() +{ + if (!servos[this->servoIndex]) { + return 0; + } + return servos[this->servoIndex]->duration; +} + +bool Servo::attached() +{ + return servos[this->servoIndex] != NULL; +} + +#endif \ No newline at end of file diff --git a/Servo/src/mbed/ServoTimers.h b/Servo/src/mbed/ServoTimers.h new file mode 100644 index 0000000..fc52bf5 --- /dev/null +++ b/Servo/src/mbed/ServoTimers.h @@ -0,0 +1 @@ +#define _Nbr_16timers 32 \ No newline at end of file diff --git a/Servo/src/megaavr/Servo.cpp b/Servo/src/megaavr/Servo.cpp new file mode 100644 index 0000000..4551fa1 --- /dev/null +++ b/Servo/src/megaavr/Servo.cpp @@ -0,0 +1,214 @@ +#if defined(ARDUINO_ARCH_MEGAAVR) + +#include +#include + +#define usToTicks(_us) ((clockCyclesPerMicrosecond() / 16 * _us) / 4) // converts microseconds to tick +#define ticksToUs(_ticks) (((unsigned) _ticks * 16) / (clockCyclesPerMicrosecond() / 4)) // converts from ticks back to microseconds + +#define TRIM_DURATION 5 // 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 currentServoIndex[_Nbr_16timers]; // index 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 + +#undef REFRESH_INTERVAL +#define REFRESH_INTERVAL 16000 + +void ServoHandler(int timer) +{ + if (currentServoIndex[timer] < 0) { + // Write compare register + _timer->CCMP = 0; + } else { + if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { + digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated + } + } + + // Select the next servo controlled by this timer + currentServoIndex[timer]++; + + if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) { + if (SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { // check if activated + digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high + } + + // Get the counter value + uint16_t tcCounterValue = 0; //_timer->CCMP; + _timer->CCMP = (uint16_t) (tcCounterValue + SERVO(timer, currentServoIndex[timer]).ticks); + } + else { + // finished all channels so wait for the refresh period to expire before starting over + + // Get the counter value + uint16_t tcCounterValue = _timer->CCMP; + + if (tcCounterValue + 4UL < usToTicks(REFRESH_INTERVAL)) { // allow a few ticks to ensure the next OCR1A not missed + _timer->CCMP = (uint16_t) usToTicks(REFRESH_INTERVAL); + } + else { + _timer->CCMP = (uint16_t) (tcCounterValue + 4UL); // at least REFRESH_INTERVAL has elapsed + } + + currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel + } + + /* Clear flag */ + _timer->INTFLAGS = TCB_CAPT_bm; +} + +#if defined USE_TIMERB0 +ISR(TCB0_INT_vect) +#elif defined USE_TIMERB1 +ISR(TCB1_INT_vect) +#elif defined USE_TIMERB2 +ISR(TCB2_INT_vect) +#endif +{ + ServoHandler(0); +} + +static void initISR(timer16_Sequence_t timer) +{ + //TCA0.SINGLE.CTRLA = (TCA_SINGLE_CLKSEL_DIV16_gc) | (TCA_SINGLE_ENABLE_bm); + + _timer->CTRLA = TCB_CLKSEL_CLKTCA_gc; + // Timer to Periodic interrupt mode + // This write will also disable any active PWM outputs + _timer->CTRLB = TCB_CNTMODE_INT_gc; + // Enable interrupt + _timer->INTCTRL = TCB_CAPTEI_bm; + // Enable timer + _timer->CTRLA |= TCB_ENABLE_bm; +} + +static void finISR(timer16_Sequence_t timer) +{ + // Disable interrupt + _timer->INTCTRL = 0; +} + +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; +} + +#endif \ No newline at end of file diff --git a/Servo/src/megaavr/ServoTimers.h b/Servo/src/megaavr/ServoTimers.h new file mode 100644 index 0000000..56746dc --- /dev/null +++ b/Servo/src/megaavr/ServoTimers.h @@ -0,0 +1,54 @@ +/* + Copyright (c) 2018 Arduino LLC. 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 +*/ + +/* + * Defines for 16 bit timers used with Servo library + * + */ + +#ifndef __SERVO_TIMERS_H__ +#define __SERVO_TIMERS_H__ + +#include + +//#define USE_TIMERB1 // interferes with PWM on pin 3 +#define USE_TIMERB2 // interferes with PWM on pin 11 +//#define USE_TIMERB0 // interferes with PWM on pin 6 + +#if !defined(USE_TIMERB1) && !defined(USE_TIMERB2) && !defined(USE_TIMERB0) + # error "No timers allowed for Servo" + /* Please uncomment a timer above and rebuild */ +#endif + +static volatile TCB_t* _timer = +#if defined(USE_TIMERB0) +&TCB0; +#endif +#if defined(USE_TIMERB1) +&TCB1; +#endif +#if defined(USE_TIMERB2) +&TCB2; +#endif + +typedef enum { + timer0, + _Nbr_16timers } timer16_Sequence_t; + + +#endif /* __SERVO_TIMERS_H__ */ diff --git a/Servo/src/nrf52/Servo.cpp b/Servo/src/nrf52/Servo.cpp new file mode 100644 index 0000000..a49f093 --- /dev/null +++ b/Servo/src/nrf52/Servo.cpp @@ -0,0 +1,134 @@ +/* + Copyright (c) 2016 Arduino. 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 +*/ + +#if defined(ARDUINO_ARCH_NRF52) + +#include +#include + + +static servo_t servos[MAX_SERVOS]; // static array of servo structures + +uint8_t ServoCount = 0; // the total number of attached servos + + + +uint32_t group_pins[3][NRF_PWM_CHANNEL_COUNT]={{NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}, {NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}, {NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}}; +static uint16_t seq_values[3][NRF_PWM_CHANNEL_COUNT]={{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; + +Servo::Servo() +{ + if (ServoCount < MAX_SERVOS) { + this->servoIndex = ServoCount++; // assign a servo index to this instance + } else { + this->servoIndex = INVALID_SERVO; // too many servos + } + +} + +uint8_t Servo::attach(int pin) +{ + + return this->attach(pin, 0, 2500); +} + + +uint8_t Servo::attach(int pin, int min, int max) +{ + int servo_min, servo_max; + if (this->servoIndex < MAX_SERVOS) { + pinMode(pin, OUTPUT); // set servo pin to output + servos[this->servoIndex].Pin.nbr = pin; + + if(min < servo_min) min = servo_min; + if (max > servo_max) max = servo_max; + this->min = min; + this->max = max; + + servos[this->servoIndex].Pin.isActive = true; + + } + return this->servoIndex; +} + +void Servo::detach() +{ + servos[this->servoIndex].Pin.isActive = false; +} + + +void Servo::write(int value) +{ + if (value < 0) + value = 0; + else if (value > 180) + value = 180; + value = map(value, 0, 180, MIN_PULSE, MAX_PULSE); + + writeMicroseconds(value); +} + + +void Servo::writeMicroseconds(int value) +{ + uint8_t channel, instance; + uint8_t pin = servos[this->servoIndex].Pin.nbr; + //instance of pwm module is MSB - look at VWariant.h + instance=(g_APinDescription[pin].ulPWMChannel & 0xF0)/16; + //index of pwm channel is LSB - look at VWariant.h + channel=g_APinDescription[pin].ulPWMChannel & 0x0F; + group_pins[instance][channel]=g_APinDescription[pin].ulPin; + NRF_PWM_Type * PWMInstance = instance == 0 ? NRF_PWM0 : (instance == 1 ? NRF_PWM1 : NRF_PWM2); + //configure pwm instance and enable it + seq_values[instance][channel]= value | 0x8000; + nrf_pwm_sequence_t const seq={ + seq_values[instance], + NRF_PWM_VALUES_LENGTH(seq_values), + 0, + 0 + }; + nrf_pwm_pins_set(PWMInstance, group_pins[instance]); + nrf_pwm_enable(PWMInstance); + nrf_pwm_configure(PWMInstance, NRF_PWM_CLK_125kHz, NRF_PWM_MODE_UP, 2500); // 20ms - 50Hz + nrf_pwm_decoder_set(PWMInstance, NRF_PWM_LOAD_INDIVIDUAL, NRF_PWM_STEP_AUTO); + nrf_pwm_sequence_set(PWMInstance, 0, &seq); + nrf_pwm_loop_set(PWMInstance, 0UL); + nrf_pwm_task_trigger(PWMInstance, NRF_PWM_TASK_SEQSTART0); +} + +int Servo::read() // return the value as degrees +{ + return map(readMicroseconds(), MIN_PULSE, MAX_PULSE, 0, 180); +} + +int Servo::readMicroseconds() +{ + uint8_t channel, instance; + uint8_t pin=servos[this->servoIndex].Pin.nbr; + instance=(g_APinDescription[pin].ulPWMChannel & 0xF0)/16; + channel=g_APinDescription[pin].ulPWMChannel & 0x0F; + // remove the 16th bit we added before + return seq_values[instance][channel] & 0x7FFF; +} + +bool Servo::attached() +{ + return servos[this->servoIndex].Pin.isActive; +} + +#endif // ARDUINO_ARCH_NRF52 \ No newline at end of file diff --git a/Servo/src/nrf52/ServoTimers.h b/Servo/src/nrf52/ServoTimers.h new file mode 100644 index 0000000..51759be --- /dev/null +++ b/Servo/src/nrf52/ServoTimers.h @@ -0,0 +1,38 @@ +/* + Copyright (c) 2016 Arduino. 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 +*/ + +/* + * NRF52 doesn't use timer, but pwm. This file include definitions to keep + * compatibility with the Servo library standards. + */ + +#ifndef __SERVO_TIMERS_H__ +#define __SERVO_TIMERS_H__ + +/** + * NRF52 Only definitions + * --------------------- + */ + +#define MIN_PULSE 55 +#define MAX_PULSE 284 + +// define one timer in order to have MAX_SERVOS = 12 +typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t; + +#endif // __SERVO_TIMERS_H__ \ No newline at end of file diff --git a/Servo/src/sam/Servo.cpp b/Servo/src/sam/Servo.cpp new file mode 100644 index 0000000..21f901f --- /dev/null +++ b/Servo/src/sam/Servo.cpp @@ -0,0 +1,283 @@ +/* + Copyright (c) 2013 Arduino LLC. 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 +*/ + +#if defined(ARDUINO_ARCH_SAM) + +#include +#include + +#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 ***********************/ + +//------------------------------------------------------------------------------ +/// 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; +} + +#endif // ARDUINO_ARCH_SAM + diff --git a/Servo/src/sam/ServoTimers.h b/Servo/src/sam/ServoTimers.h new file mode 100644 index 0000000..13f736a --- /dev/null +++ b/Servo/src/sam/ServoTimers.h @@ -0,0 +1,88 @@ +/* + Copyright (c) 2013 Arduino LLC. 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 +*/ + +/* + * 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. + */ + +/** + * SAM Only definitions + * -------------------- + */ + +// 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 ; + diff --git a/Servo/src/samd/Servo.cpp b/Servo/src/samd/Servo.cpp new file mode 100644 index 0000000..42a3877 --- /dev/null +++ b/Servo/src/samd/Servo.cpp @@ -0,0 +1,297 @@ +/* + Copyright (c) 2015 Arduino LLC. 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 +*/ + +#if defined(ARDUINO_ARCH_SAMD) + +#include +#include + +#define usToTicks(_us) ((clockCyclesPerMicrosecond() * _us) / 16) // converts microseconds to tick +#define ticksToUs(_ticks) (((unsigned) _ticks * 16) / clockCyclesPerMicrosecond()) // converts from ticks back to microseconds + +#define TRIM_DURATION 5 // 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 currentServoIndex[_Nbr_16timers]; // index 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 + +#define WAIT_TC16_REGS_SYNC(x) while(x->COUNT16.STATUS.bit.SYNCBUSY); + +/************ static functions common to all instances ***********************/ + +void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel, uint8_t intFlag); +#if defined (_useTimer1) +void HANDLER_FOR_TIMER1(void) { + Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, INTFLAG_BIT_FOR_TIMER_1); +} +#endif +#if defined (_useTimer2) +void HANDLER_FOR_TIMER2(void) { + Servo_Handler(_timer2, TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, INTFLAG_BIT_FOR_TIMER_2); +} +#endif + +void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel, uint8_t intFlag) +{ + if (currentServoIndex[timer] < 0) { + tc->COUNT16.COUNT.reg = (uint16_t) 0; + WAIT_TC16_REGS_SYNC(tc) + } else { + if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { + digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated + } + } + + // Select the next servo controlled by this timer + currentServoIndex[timer]++; + + if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) { + if (SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { // check if activated + digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high + } + + // Get the counter value + uint16_t tcCounterValue = tc->COUNT16.COUNT.reg; + WAIT_TC16_REGS_SYNC(tc) + + tc->COUNT16.CC[channel].reg = (uint16_t) (tcCounterValue + SERVO(timer, currentServoIndex[timer]).ticks); + WAIT_TC16_REGS_SYNC(tc) + } + else { + // finished all channels so wait for the refresh period to expire before starting over + + // Get the counter value + uint16_t tcCounterValue = tc->COUNT16.COUNT.reg; + WAIT_TC16_REGS_SYNC(tc) + + if (tcCounterValue + 4UL < usToTicks(REFRESH_INTERVAL)) { // allow a few ticks to ensure the next OCR1A not missed + tc->COUNT16.CC[channel].reg = (uint16_t) usToTicks(REFRESH_INTERVAL); + } + else { + tc->COUNT16.CC[channel].reg = (uint16_t) (tcCounterValue + 4UL); // at least REFRESH_INTERVAL has elapsed + } + WAIT_TC16_REGS_SYNC(tc) + + currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel + } + + // Clear the interrupt + tc->COUNT16.INTFLAG.reg = intFlag; +} + +static inline void resetTC (Tc* TCx) +{ + // Disable TCx + TCx->COUNT16.CTRLA.reg &= ~TC_CTRLA_ENABLE; + WAIT_TC16_REGS_SYNC(TCx) + + // Reset TCx + TCx->COUNT16.CTRLA.reg = TC_CTRLA_SWRST; + WAIT_TC16_REGS_SYNC(TCx) + while (TCx->COUNT16.CTRLA.bit.SWRST); +} + +static void _initISR(Tc *tc, uint8_t channel, uint32_t id, IRQn_Type irqn, uint8_t gcmForTimer, uint8_t intEnableBit) +{ + // Enable GCLK for timer 1 (timer counter input clock) + GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(gcmForTimer)); + while (GCLK->STATUS.bit.SYNCBUSY); + + // Reset the timer + // TODO this is not the right thing to do if more than one channel per timer is used by the Servo library + resetTC(tc); + + // Set timer counter mode to 16 bits + tc->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16; + + // Set timer counter mode as normal PWM + tc->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_NPWM; + + // Set the prescaler factor to GCLK_TC/16. At nominal 48MHz GCLK_TC this is 3000 ticks per millisecond + tc->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV16; + + // Count up + tc->COUNT16.CTRLBCLR.bit.DIR = 1; + WAIT_TC16_REGS_SYNC(tc) + + // First interrupt request after 1 ms + tc->COUNT16.CC[channel].reg = (uint16_t) usToTicks(1000UL); + WAIT_TC16_REGS_SYNC(tc) + + // Configure interrupt request + // TODO this should be changed if more than one channel per timer is used by the Servo library + NVIC_DisableIRQ(irqn); + NVIC_ClearPendingIRQ(irqn); + NVIC_SetPriority(irqn, 0); + NVIC_EnableIRQ(irqn); + + // Enable the match channel interrupt request + tc->COUNT16.INTENSET.reg = intEnableBit; + + // Enable the timer and start it + tc->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE; + WAIT_TC16_REGS_SYNC(tc) +} + +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, GCM_FOR_TIMER_1, INTENSET_BIT_FOR_TIMER_1); +#endif +#if defined (_useTimer2) + if (timer == _timer2) + _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2, GCM_FOR_TIMER_2, INTENSET_BIT_FOR_TIMER_2); +#endif +} + +static void finISR(timer16_Sequence_t timer) +{ +#if defined (_useTimer1) + // Disable the match channel interrupt request + TC_FOR_TIMER1->COUNT16.INTENCLR.reg = INTENCLR_BIT_FOR_TIMER_1; +#endif +#if defined (_useTimer2) + // Disable the match channel interrupt request + TC_FOR_TIMER2->COUNT16.INTENCLR.reg = INTENCLR_BIT_FOR_TIMER_2; +#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; +} + +#endif // ARDUINO_ARCH_SAMD diff --git a/Servo/src/samd/ServoTimers.h b/Servo/src/samd/ServoTimers.h new file mode 100644 index 0000000..17acfb5 --- /dev/null +++ b/Servo/src/samd/ServoTimers.h @@ -0,0 +1,71 @@ +/* + Copyright (c) 2015 Arduino LLC. 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 +*/ + +/* + * 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. + */ + +#ifndef __SERVO_TIMERS_H__ +#define __SERVO_TIMERS_H__ + +/** + * SAMD Only definitions + * --------------------- + */ + +// For SAMD: +#define _useTimer1 +//#define _useTimer2 // <- TODO do not activate until the code in Servo.cpp has been changed in order + // to manage more than one channel per timer on the SAMD architecture + +#if defined (_useTimer1) +#define TC_FOR_TIMER1 TC4 +#define CHANNEL_FOR_TIMER1 0 +#define INTENSET_BIT_FOR_TIMER_1 TC_INTENSET_MC0 +#define INTENCLR_BIT_FOR_TIMER_1 TC_INTENCLR_MC0 +#define INTFLAG_BIT_FOR_TIMER_1 TC_INTFLAG_MC0 +#define ID_TC_FOR_TIMER1 ID_TC4 +#define IRQn_FOR_TIMER1 TC4_IRQn +#define HANDLER_FOR_TIMER1 TC4_Handler +#define GCM_FOR_TIMER_1 GCM_TC4_TC5 +#endif +#if defined (_useTimer2) +#define TC_FOR_TIMER2 TC4 +#define CHANNEL_FOR_TIMER2 1 +#define INTENSET_BIT_FOR_TIMER_2 TC_INTENSET_MC1 +#define INTENCLR_BIT_FOR_TIMER_2 TC_INTENCLR_MC1 +#define ID_TC_FOR_TIMER2 ID_TC4 +#define IRQn_FOR_TIMER2 TC4_IRQn +#define HANDLER_FOR_TIMER2 TC4_Handler +#define GCM_FOR_TIMER_2 GCM_TC4_TC5 +#endif + +typedef enum { +#if defined (_useTimer1) + _timer1, +#endif +#if defined (_useTimer2) + _timer2, +#endif + _Nbr_16timers } timer16_Sequence_t; + +#endif // __SERVO_TIMERS_H__ diff --git a/Servo/src/stm32f4/Servo.cpp b/Servo/src/stm32f4/Servo.cpp new file mode 100644 index 0000000..01d05d9 --- /dev/null +++ b/Servo/src/stm32f4/Servo.cpp @@ -0,0 +1,194 @@ +/****************************************************************************** + * The MIT License + * + * Copyright (c) 2010, LeafLabs, LLC. + * + * Permission is hereby granted, free of charge, to any person + * obtaining a copy of this software and associated documentation + * files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, copy, + * modify, merge, publish, distribute, sublicense, and/or sell copies + * of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + *****************************************************************************/ + +#if defined(ARDUINO_ARCH_STM32F4) + +#include "ServoTimers.h" + +#include "boards.h" +#include "io.h" +#include "pwm.h" +#include "math.h" + +// 20 millisecond period config. For a 1-based prescaler, +// +// (prescaler * overflow / CYC_MSEC) msec = 1 timer cycle = 20 msec +// => prescaler * overflow = 20 * CYC_MSEC +// +// This picks the smallest prescaler that allows an overflow < 2^16. +#define MAX_OVERFLOW ((1 << 16) - 1) +#define CYC_MSEC (1000 * CYCLES_PER_MICROSECOND) +#define TAU_MSEC 20 +#define TAU_USEC (TAU_MSEC * 1000) +#define TAU_CYC (TAU_MSEC * CYC_MSEC) +#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1) +#define SERVO_OVERFLOW ((uint16)round((double)TAU_CYC / SERVO_PRESCALER)) + +// Unit conversions +#define US_TO_COMPARE(us) ((uint16)map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW)) +#define COMPARE_TO_US(c) ((uint32)map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC)) +#define ANGLE_TO_US(a) ((uint16)(map((a), this->minAngle, this->maxAngle, \ + this->minPW, this->maxPW))) +#define US_TO_ANGLE(us) ((int16)(map((us), this->minPW, this->maxPW, \ + this->minAngle, this->maxAngle))) + +Servo::Servo() { + this->resetFields(); +} + +bool Servo::attach(uint8 pin, uint16 minPW, uint16 maxPW, int16 minAngle, int16 maxAngle) +{ + // SerialUSB.begin(115200); + // SerialUSB.println(MAX_OVERFLOW); + + + timer_dev *tdev = PIN_MAP[pin].timer_device; + + analogWriteResolution(16); + + int prescaler = 6; + int overflow = 65400; + int minPW_correction = 300; + int maxPW_correction = 300; + + pinMode(pin, OUTPUT); + + + if (tdev == NULL) { + // don't reset any fields or ASSERT(0), to keep driving any + // previously attach()ed servo. + return false; + } + + if ( (tdev == TIMER1) || (tdev == TIMER8) || (tdev == TIMER10) || (tdev == TIMER11)) + { + prescaler = 54; + overflow = 65400; + minPW_correction = 40; + maxPW_correction = 50; + } + + if ( (tdev == TIMER2) || (tdev == TIMER3) || (tdev == TIMER4) || (tdev == TIMER5) ) + { + prescaler = 6; + overflow = 64285; + minPW_correction = 370; + maxPW_correction = 350; + } + + if ( (tdev == TIMER6) || (tdev == TIMER7) ) + { + prescaler = 6; + overflow = 65400; + minPW_correction = 0; + maxPW_correction = 0; + } + + if ( (tdev == TIMER9) || (tdev == TIMER12) || (tdev == TIMER13) || (tdev == TIMER14) ) + { + prescaler = 6; + overflow = 65400; + minPW_correction = 30; + maxPW_correction = 0; + } + + if (this->attached()) { + this->detach(); + } + + this->pin = pin; + this->minPW = (minPW + minPW_correction); + this->maxPW = (maxPW + maxPW_correction); + this->minAngle = minAngle; + this->maxAngle = maxAngle; + + timer_pause(tdev); + timer_set_prescaler(tdev, prescaler); // prescaler is 1-based + timer_set_reload(tdev, overflow); + timer_generate_update(tdev); + timer_resume(tdev); + + return true; +} + +bool Servo::detach() { + if (!this->attached()) { + return false; + } + + timer_dev *tdev = PIN_MAP[this->pin].timer_device; + uint8 tchan = PIN_MAP[this->pin].timer_channel; + timer_set_mode(tdev, tchan, TIMER_DISABLED); + + this->resetFields(); + + return true; +} + +void Servo::write(int degrees) { + degrees = constrain(degrees, this->minAngle, this->maxAngle); + this->writeMicroseconds(ANGLE_TO_US(degrees)); +} + +int Servo::read() const { + int a = US_TO_ANGLE(this->readMicroseconds()); + // map() round-trips in a weird way we mostly correct for here; + // the round-trip is still sometimes off-by-one for write(1) and + // write(179). + return a == this->minAngle || a == this->maxAngle ? a : a + 1; +} + +void Servo::writeMicroseconds(uint16 pulseWidth) { + if (!this->attached()) { + ASSERT(0); + return; + } + pulseWidth = constrain(pulseWidth, this->minPW, this->maxPW); + analogWrite(this->pin, US_TO_COMPARE(pulseWidth)); +} + +uint16 Servo::readMicroseconds() const { + if (!this->attached()) { + ASSERT(0); + return 0; + } + + stm32_pin_info pin_info = PIN_MAP[this->pin]; + uint16 compare = timer_get_compare(pin_info.timer_device, + pin_info.timer_channel); + + return COMPARE_TO_US(compare); +} + +void Servo::resetFields(void) { + this->pin = NOT_ATTACHED; + this->minAngle = MIN_ANGLE; + this->maxAngle = MAX_ANGLE; + this->minPW = MIN_PULSE_WIDTH; + this->maxPW = MAX_PULSE_WIDTH; +} + +#endif diff --git a/Servo/src/stm32f4/ServoTimers.h b/Servo/src/stm32f4/ServoTimers.h new file mode 100644 index 0000000..12d55b7 --- /dev/null +++ b/Servo/src/stm32f4/ServoTimers.h @@ -0,0 +1,207 @@ +/****************************************************************************** + * The MIT License + * + * Copyright (c) 2010, LeafLabs, LLC. + * + * Permission is hereby granted, free of charge, to any person + * obtaining a copy of this software and associated documentation + * files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, copy, + * modify, merge, publish, distribute, sublicense, and/or sell copies + * of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + *****************************************************************************/ + + /* + * Arduino srl - www.arduino.org + * 2017 Feb 23: Edited by Francesco Alessi (alfran) - francesco@arduino.org + */ +#ifndef _SERVO_H_ +#define _SERVO_H_ + +#include "types.h" +#include "timer.h" + +#include "wiring.h" /* hack for IDE compile */ + +/* + * Note on Arduino compatibility: + * + * In the Arduino implementation, PWM is done "by hand" in the sense + * that timer channels are hijacked in groups and an ISR is set which + * toggles Servo::attach()ed pins using digitalWrite(). + * + * While this scheme allows any pin to drive a servo, it chews up + * cycles and complicates the programmer's notion of when a particular + * timer channel will be in use. + * + * This implementation only allows Servo instances to attach() to pins + * that already have a timer channel associated with them, and just + * uses pwmWrite() to drive the wave. + * + * This introduces an incompatibility: while the Arduino + * implementation of attach() returns the affected channel on success + * and 0 on failure, this one returns true on success and false on + * failure. + * + * RC Servos expect a pulse every 20ms. Since periods are set for + * entire timers, rather than individual channels, attach()ing a Servo + * to a pin can interfere with other pins associated with the same + * timer. As always, your board's pin map is your friend. + */ + +// Pin number of unattached pins +#define NOT_ATTACHED (-1) + +#define _Nbr_16timers 14 // mumber of STM32F469 Timers +#define SERVOS_PER_TIMER 4 // Number of timer channels + + +// Default min/max pulse widths (in microseconds) and angles (in +// degrees). Values chosen for Arduino compatibility. These values +// are part of the public API; DO NOT CHANGE THEM. +#define MIN_ANGLE 0 +#define MAX_ANGLE 180 + +#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo +#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo + +/** Class for interfacing with RC servomotors. */ +class Servo { +public: + /** + * @brief Construct a new Servo instance. + * + * The new instance will not be attached to any pin. + */ + Servo(); + + /** + * @brief Associate this instance with a servomotor whose input is + * connected to pin. + * + * If this instance is already attached to a pin, it will be + * detached before being attached to the new pin. This function + * doesn't detach any interrupt attached with the pin's timer + * channel. + * + * @param pin Pin connected to the servo pulse wave input. This + * pin must be capable of PWM output. + * + * @param minPulseWidth Minimum pulse width to write to pin, in + * microseconds. This will be associated + * with a minAngle degree angle. Defaults to + * SERVO_DEFAULT_MIN_PW = 544. + * + * @param maxPulseWidth Maximum pulse width to write to pin, in + * microseconds. This will be associated + * with a maxAngle degree angle. Defaults to + * SERVO_DEFAULT_MAX_PW = 2400. + * + * @param minAngle Target angle (in degrees) associated with + * minPulseWidth. Defaults to + * SERVO_DEFAULT_MIN_ANGLE = 0. + * + * @param maxAngle Target angle (in degrees) associated with + * maxPulseWidth. Defaults to + * SERVO_DEFAULT_MAX_ANGLE = 180. + * + * @sideeffect May set pinMode(pin, PWM). + * + * @return true if successful, false when pin doesn't support PWM. + */ + + bool attach(uint8 pin, + uint16 minPulseWidth=MIN_PULSE_WIDTH, + uint16 maxPulseWidth=MAX_PULSE_WIDTH, + int16 minAngle=MIN_ANGLE, + int16 maxAngle=MAX_ANGLE); + /** + * @brief Stop driving the servo pulse train. + * + * If not currently attached to a motor, this function has no effect. + * + * @return true if this call did anything, false otherwise. + */ + bool detach(); + + /** + * @brief Set the servomotor target angle. + * + * @param angle Target angle, in degrees. If the target angle is + * outside the range specified at attach() time, it + * will be clamped to lie in that range. + * + * @see Servo::attach() + */ + void write(int angle); + + /** + * @brief Set the pulse width, in microseconds. + * + * @param pulseWidth Pulse width to send to the servomotor, in + * microseconds. If outside of the range + * specified at attach() time, it is clamped to + * lie in that range. + * + * @see Servo::attach() + */ + void writeMicroseconds(uint16 pulseWidth); + + /** + * Get the servomotor's target angle, in degrees. This will + * lie inside the range specified at attach() time. + * + * @see Servo::attach() + */ + int read() const; + + /** + * Get the current pulse width, in microseconds. This will + * lie within the range specified at attach() time. + * + * @see Servo::attach() + */ + uint16 readMicroseconds() const; + + + /** + * @brief Check if this instance is attached to a servo. + * @return true if this instance is attached to a servo, false otherwise. + * @see Servo::attachedPin() + */ + bool attached() const { return this->pin != NOT_ATTACHED; } + + /** + * @brief Get the pin this instance is attached to. + * @return Pin number if currently attached to a pin, NOT_ATTACHED + * otherwise. + * @see Servo::attach() + */ + int attachedPin() const { return this->pin; } + +private: + int16 pin; + uint16 minPW; + uint16 maxPW; + int16 minAngle; + int16 maxAngle; + + void resetFields(void); +}; + + + +#endif /* _SERVO_H_ */