I would like to propose an alternate implementation.
This code supports 3 servo motors on chips with 3 timer1 PWM channels (such as the Arduino Mega).
It also defines names SERVO_PIN_A and SERVO_PIN_B which can be used instead of 9 and 10, if you want to write sketches that work on multiple boards. Of course, it's fully compatible with the old API and you can still use hard-coded pin numbers in your sketch, if you like.
The changes to the original code are based on Zach's approach. This code supports all '168 and '328 based Arduinos, the Arduino Mega, Sanguino, and both Teensy boards, and it is designed to make adding other boards easy. The board-specific #defines are in only one location inside servo.h, rather than scattered throughout the actual code.
I'll post this to the developer list too.
Servo.cpp
#include <avr/interrupt.h>
#include <wiring.h>
#include <Servo.h>
/*
Servo.h - Hardware Servo Timer Library
Author: Jim Studt, jim@federated.com
Copyright (c) 2007 David A. Mellis. 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
*/
uint8_t Servo::attachedA = 0;
uint8_t Servo::attachedB = 0;
#ifdef SERVO_PIN_C
uint8_t Servo::attachedC = 0;
#endif
void Servo::seizeTimer1()
{
uint8_t oldSREG = SREG;
cli();
TCCR1A = _BV(WGM11); /* Fast PWM, ICR1 is top */
TCCR1B = _BV(WGM13) | _BV(WGM12) /* Fast PWM, ICR1 is top */
| _BV(CS11) /* div 8 clock prescaler */
;
OCR1A = 3000;
OCR1B = 3000;
ICR1 = clockCyclesPerMicrosecond()*(20000L/8); // 20000 uS is a bit fast for the refresh, 20ms, but
// it keeps us from overflowing ICR1 at 20MHz clocks
// That "/8" at the end is the prescaler.
#if defined(__AVR_ATmega8__)
TIMSK &= ~(_BV(TICIE1) | _BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) );
#else
TIMSK1 &= ~(_BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) );
#endif
SREG = oldSREG; // undo cli()
}
void Servo::releaseTimer1() {}
#define NO_ANGLE (0xff)
Servo::Servo() : pin(0), angle(NO_ANGLE) {}
uint8_t Servo::attach(int pinArg)
{
return attach(pinArg, 544, 2400);
}
uint8_t Servo::attach(int pinArg, int min, int max)
{
#ifdef SERVO_PIN_C
if (pinArg != SERVO_PIN_A && pinArg != SERVO_PIN_B && pinArg != SERVO_PIN_C) return 0;
#else
if (pinArg != SERVO_PIN_A && pinArg != SERVO_PIN_B) return 0;
#endif
min16 = min / 16;
max16 = max / 16;
pin = pinArg;
angle = NO_ANGLE;
digitalWrite(pin, LOW);
pinMode(pin, OUTPUT);
#ifdef SERVO_PIN_C
if (!attachedA && !attachedB && !attachedC) seizeTimer1();
#else
if (!attachedA && !attachedB) seizeTimer1();
#endif
if (pin == SERVO_PIN_A) {
attachedA = 1;
TCCR1A = (TCCR1A & ~_BV(COM1A0)) | _BV(COM1A1);
}
if (pin == SERVO_PIN_B) {
attachedB = 1;
TCCR1A = (TCCR1A & ~_BV(COM1B0)) | _BV(COM1B1);
}
#ifdef SERVO_PIN_C
if (pin == SERVO_PIN_C) {
attachedC = 1;
TCCR1A = (TCCR1A & ~_BV(COM1C0)) | _BV(COM1C1);
}
#endif
return 1;
}
void Servo::detach()
{
// muck with timer flags
if (pin == SERVO_PIN_A) {
attachedA = 0;
TCCR1A = TCCR1A & ~_BV(COM1A0) & ~_BV(COM1A1);
pinMode(pin, INPUT);
}
if (pin == SERVO_PIN_B) {
attachedB = 0;
TCCR1A = TCCR1A & ~_BV(COM1B0) & ~_BV(COM1B1);
pinMode(pin, INPUT);
}
#ifdef SERVO_PIN_C
if (pin == SERVO_PIN_C) {
attachedC = 0;
TCCR1A = TCCR1A & ~_BV(COM1C0) & ~_BV(COM1C1);
pinMode(pin, INPUT);
}
#endif
#ifdef SERVO_PIN_C
if (!attachedA && !attachedB && !attachedC) releaseTimer1();
#else
if (!attachedA && !attachedB) releaseTimer1();
#endif
}
void Servo::write(int angleArg)
{
uint16_t p;
if (angleArg < 0) angleArg = 0;
if (angleArg > 180) angleArg = 180;
angle = angleArg;
// 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,
// 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;
if (pin == SERVO_PIN_A) OCR1A = p;
if (pin == SERVO_PIN_B) OCR1B = p;
#ifdef SERVO_PIN_C
if (pin == SERVO_PIN_C) OCR1C = p;
#endif
}
uint8_t Servo::read()
{
return angle;
}
uint8_t Servo::attached()
{
if (pin == SERVO_PIN_A && attachedA) return 1;
if (pin == SERVO_PIN_B && attachedB) return 1;
#ifdef SERVO_PIN_C
if (pin == SERVO_PIN_C && attachedC) return 1;
#endif
return 0;
}
Servo.h
#ifndef Servo_h
#define Servo_h
/*
Servo.h - Hardware Servo Timer Library
Author: Jim Studt, jim@federated.com
Copyright (c) 2007 David A. Mellis. 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
*/
#include <inttypes.h>
#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) // Arduino
#define SERVO_PIN_A 9
#define SERVO_PIN_B 10
#elif defined(__AVR_ATmega1280__) // Arduino Mega
#define SERVO_PIN_A 11
#define SERVO_PIN_B 12
#define SERVO_PIN_C 13
#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) // Sanguino
#define SERVO_PIN_A 13
#define SERVO_PIN_B 12
#elif defined(__AVR_AT90USB162__) // Teensy
#define SERVO_PIN_A 17
#define SERVO_PIN_B 18
#define SERVO_PIN_C 15
#elif defined(__AVR_AT90USB646__) // Teensy++
#define SERVO_PIN_A 25
#define SERVO_PIN_B 26
#define SERVO_PIN_C 27
#else
#define SERVO_PIN_A 9
#define SERVO_PIN_B 10
#endif
class Servo
{
private:
uint8_t pin;
uint8_t angle; // in degrees
uint8_t min16; // minimum pulse, 16uS units (default is 34)
uint8_t max16; // maximum pulse, 16uS units, 0-4ms range (default is 150)
static void seizeTimer1();
static void releaseTimer1();
static uint8_t attachedA;
static uint8_t attachedB;
#ifdef SERVO_PIN_C
static uint8_t attachedC;
#endif
public:
Servo();
uint8_t attach(int);
// pulse length for 0 degrees in microseconds, 544uS default
// pulse length for 180 degrees in microseconds, 2400uS default
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