rooty/Arduino
1
0
Fork 0
mirror of https://github.com/arduino/Arduino.git synced 2025-03-14 11:29:26 +01:00
Code Issues Releases Activity

Integrating the new Servo library (MegaServo) by Michael Margolis. Uses timer 1, and, on the Mega, timers 3, 4, and 5 for up to 12 servos (48 on the Mega).

Browse Source
This commit is contained in:
David A. Mellis 2009-07-12 00:33:02 +00:00
parent a81628675d
commit 55df12a8bd
3 changed files with 345 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

401
hardware/libraries/Servo/Servo.cpp
View File

@ -1,133 +1,268 @@
#include <avr/interrupt.h> /*
#include <wiring.h> Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
#include <Servo.h> Copyright (c) 2009 Michael Margolis. All right reserved.
/* This library is free software; you can redistribute it and/or
Servo.h - Hardware Servo Timer Library modify it under the terms of the GNU Lesser General Public
Author: Jim Studt, jim@federated.com License as published by the Free Software Foundation; either
Copyright (c) 2007 David A. Mellis. All right reserved. version 2.1 of the License, or (at your option) any later version.
This library is free software; you can redistribute it and/or This library is distributed in the hope that it will be useful,
modify it under the terms of the GNU Lesser General Public but WITHOUT ANY WARRANTY; without even the implied warranty of
License as published by the Free Software Foundation; either MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
version 2.1 of the License, or (at your option) any later version. Lesser General Public License for more details.
This library is distributed in the hope that it will be useful, You should have received a copy of the GNU Lesser General Public
but WITHOUT ANY WARRANTY; without even the implied warranty of License along with this library; if not, write to the Free Software
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
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.
uint8_t Servo::attached9 = 0; Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
uint8_t Servo::attached10 = 0;
The methods are:
void Servo::seizeTimer1()
{ Servo - Class for manipulating servo motors connected to Arduino pins.
uint8_t oldSREG = SREG;
attach(pin ) - Attaches a servo motor to an i/o pin.
cli(); attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds
TCCR1A = _BV(WGM11); /* Fast PWM, ICR1 is top */ default min is 544, max is 2400
TCCR1B = _BV(WGM13) | _BV(WGM12) /* Fast PWM, ICR1 is top */
| _BV(CS11) /* div 8 clock prescaler */ 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
OCR1A = 3000; read() - Gets the last written servo pulse width as an angle between 0 and 180.
OCR1B = 3000; readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
ICR1 = clockCyclesPerMicrosecond()*(20000L/8); // 20000 uS is a bit fast for the refresh, 20ms, but attached() - Returns true if there is a servo attached.
// it keeps us from overflowing ICR1 at 20MHz clocks detach() - Stops an attached servos from pulsing its i/o pin.
// That "/8" at the end is the prescaler.
#if defined(__AVR_ATmega8__) */
TIMSK &= ~(_BV(TICIE1) | _BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) );
#else #include <avr/interrupt.h>
TIMSK1 &= ~(_BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) ); #include <WProgram.h>
#endif
SREG = oldSREG; // undo cli() #include "Servo.h"
}
#define TICKS_PER_uS (clockCyclesPerMicrosecond() / 8) // number of timer ticks per microsecond with prescale of 8
void Servo::releaseTimer1() {}
#define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer
#define NO_ANGLE (0xff) #define TRIM_DURATION (SERVOS_PER_TIMER/2) // compensation ticks to trim adjust for digitalWrite delays
Servo::Servo() : pin(0), angle(NO_ANGLE) {} #define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER)
uint8_t Servo::attach(int pinArg) static servo_t servos[MAX_SERVOS]; // static array of servo structures
{ static volatile int8_t Channel[NBR_TIMERS]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
return attach(pinArg, 544, 2400); #if defined(__AVR_ATmega1280__)
} typedef enum { _timer5, _timer1, _timer3, _timer4 } servoTimer_t; // this is the sequence for timer utilization on mega
#else
uint8_t Servo::attach(int pinArg, int min, int max) typedef enum { _timer1 } servoTimer_t; // this is the sequence for timer utilization on other controllers
{ #endif
if (pinArg != 9 && pinArg != 10) return 0;
uint8_t ServoCount = 0; // the total number of attached servos
min16 = min / 16;
max16 = max / 16; // convenience macros
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((servoTimer_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
pin = pinArg; #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
angle = NO_ANGLE; #define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
digitalWrite(pin, LOW); #define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
pinMode(pin, OUTPUT);
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
if (!attached9 && !attached10) seizeTimer1(); #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
if (pin == 9) { /************ static functions common to all instances ***********************/
attached9 = 1;
TCCR1A = (TCCR1A & ~_BV(COM1A0)) | _BV(COM1A1); static inline void handle_interrupts(servoTimer_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
} {
if( Channel[timer] < 0 )
if (pin == 10) { *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
attached10 = 1; else{
TCCR1A = (TCCR1A & ~_BV(COM1B0)) | _BV(COM1B1); 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
return 1; }
}
Channel[timer]++; // increment to the next channel
void Servo::detach() if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
{ *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
// muck with timer flags if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated
if (pin == 9) { digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
attached9 = 0; }
TCCR1A = TCCR1A & ~_BV(COM1A0) & ~_BV(COM1A1); else {
pinMode(pin, INPUT); // finished all channels so wait for the refresh period to expire before starting over
} if( (unsigned)*TCNTn < (((unsigned int)REFRESH_INTERVAL * TICKS_PER_uS) + 4) ) // allow a few ticks to ensure the next OCR1A not missed
*OCRnA = (unsigned int)REFRESH_INTERVAL * TICKS_PER_uS;
if (pin == 10) { else
attached10 = 0; *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
TCCR1A = TCCR1A & ~_BV(COM1B0) & ~_BV(COM1B1); Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
pinMode(pin, INPUT); }
} }
if (!attached9 && !attached10) releaseTimer1(); SIGNAL (TIMER1_COMPA_vect)
} {
handle_interrupts(_timer1, &TCNT1, &OCR1A);
void Servo::write(int angleArg) }
{
uint16_t p; #if defined(__AVR_ATmega1280__)
SIGNAL (TIMER3_COMPA_vect)
if (angleArg < 0) angleArg = 0; {
if (angleArg > 180) angleArg = 180; handle_interrupts(_timer3, &TCNT3, &OCR3A);
angle = angleArg; }
SIGNAL (TIMER4_COMPA_vect)
// bleh, have to use longs to prevent overflow, could be tricky if always a 16MHz clock, but not true {
// That 8L on the end is the TCNT1 prescaler, it will need to change if the clock's prescaler changes, handle_interrupts(_timer4, &TCNT4, &OCR4A);
// but then there will likely be an overflow problem, so it will have to be handled by a human. }
p = (min16*16L*clockCyclesPerMicrosecond() + (max16-min16)*(16L*clockCyclesPerMicrosecond())*angle/180L)/8L; SIGNAL (TIMER5_COMPA_vect)
if (pin == 9) OCR1A = p; {
if (pin == 10) OCR1B = p; handle_interrupts(_timer5, &TCNT5, &OCR5A);
} }
#endif
uint8_t Servo::read()
{ static void initISR(servoTimer_t timer)
return angle; {
} if(timer == _timer1) {
TCCR1A = 0; // normal counting mode
uint8_t Servo::attached() TCCR1B = _BV(CS11); // set prescaler of 8
{ TCNT1 = 0; // clear the timer count
if (pin == 9 && attached9) return 1; #if defined(__AVR_ATmega8__)
if (pin == 10 && attached10) return 1; TIFR = _BV(OCF1A); // clear any pending interrupts;
return 0; TIMSK = _BV(OCIE1A) ; // enable the output compare interrupt
} #else
TIFR1 = _BV(OCF1A); // clear any pending interrupts;
TIMSK1 = _BV(OCIE1A) ; // enable the output compare interrupt
#endif
}
#if defined(__AVR_ATmega1280__)
else if(timer == _timer3) {
TCCR3A = 0; // normal counting mode
TCCR3B = _BV(CS31); // set prescaler of 8
TCNT3 = 0; // clear the timer count
TIFR3 = _BV(OCF3A); // clear any pending interrupts;
TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
}
else 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
}
else 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 boolean isTimerActive(servoTimer_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 = DEFAULT_PULSE_WIDTH * TICKS_PER_uS; // 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)
{
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
servoTimer_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;
#ifdef FREE_TIMERS
if(isTimerActive(SERVO_INDEX_TO_TIMER(servoIndex)) == false) {
;// call to unimplimented function in wiring.c to re-init timer (set timer back to PWM mode) TODO?
}
#endif
}
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 >= 0) && (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) * TICKS_PER_uS; // convert to ticks after compensating for interrupt overhead
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 = (servos[this->servoIndex].ticks / TICKS_PER_uS) + TRIM_DURATION ;
else
pulsewidth = 0;
return pulsewidth;
}
bool Servo::attached()
{
return servos[this->servoIndex].Pin.isActive ;
}

98
hardware/libraries/Servo/Servo.h
View File

@ -1,10 +1,6 @@
#ifndef Servo_h
#define Servo_h
/* /*
Servo.h - Hardware Servo Timer Library Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
Author: Jim Studt, jim@federated.com Copyright (c) 2009 Michael Margolis. All right reserved.
Copyright (c) 2007 David A. Mellis. All right reserved.
This library is free software; you can redistribute it and/or This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
@ -21,32 +17,76 @@
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 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 siezed 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.
*/
#ifndef Servo_h
#define Servo_h
#include <inttypes.h> #include <inttypes.h>
#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
#if defined(__AVR_ATmega1280__)
#define MAX_SERVOS 48 // the maximum number of servos (valid range is from 1 to 48)
#else
#define MAX_SERVOS 12 // this library supports up to 12 on a standard Arduino
#endif
#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;
unsigned int ticks;
} servo_t;
class Servo class Servo
{ {
private: public:
uint8_t pin; Servo();
uint8_t angle; // in degrees 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 min16; // minimum pulse, 16uS units (default is 34) uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
uint8_t max16; // maximum pulse, 16uS units, 0-4ms range (default is 150) void detach();
static void seizeTimer1(); void write(int value); // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds
static void releaseTimer1(); void writeMicroseconds(int value); // Write pulse width in microseconds
static uint8_t attached9; int read(); // returns current pulse width as an angle between 0 and 180 degrees
static uint8_t attached10; int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release)
public: bool attached(); // return true if this servo is attached, otherwise false
Servo(); private:
uint8_t attach(int); uint8_t servoIndex; // index into the channel data for this servo
// pulse length for 0 degrees in microseconds, 544uS default int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH
// pulse length for 180 degrees in microseconds, 2400uS default int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH
uint8_t attach(int, int, int);
// attach to a pin, sets pinMode, returns 0 on failure, won't
// position the servo until a subsequent write() happens
// Only works for 9 and 10.
void detach();
void write(int); // specify the angle in degrees, 0 to 180
uint8_t read();
uint8_t attached();
}; };
#endif #endif

14
hardware/libraries/Servo/keywords.txt
View File

@ -6,16 +6,18 @@
# Datatypes (KEYWORD1) # Datatypes (KEYWORD1)
####################################### #######################################
Servo KEYWORD1 Servo KEYWORD1
####################################### #######################################
# Methods and Functions (KEYWORD2) # Methods and Functions (KEYWORD2)
####################################### #######################################
attach KEYWORD2 attach KEYWORD2
detach KEYWORD2 detach KEYWORD2
write KEYWORD2 write KEYWORD2
read KEYWORD2 read KEYWORD2
attached KEYWORD2 attached KEYWORD2
writeMicroseconds KEYWORD2
readMicroseconds KEYWORD2
####################################### #######################################
# Constants (LITERAL1) # Constants (LITERAL1)