Here is updated code for running the servos that runs completely in the interrupt handler. The pulse width timing on this version is accurate to around one percent. I changed the names of some of the functions in preparation for turning this into a library.
#define NBR_CHANNELS 4 // the maximum number of channels.
#define MIN_PULSE_WIDTH 750 // the shortest pulse sent to a servo
#define MAX_PULSE_WIDTH 2250 // the longest pulse sent to a servo
#define DEFAULT_PULSE_WIDTH 1500 // default pulse width at startup
#define FRAME_SYNC_INDEX NBR_CHANNELS // frame sync delay is last entry in array
#define FRAME_SYNC_PERIOD 20000 // total frame sync time in us
#define FRAME_SYNC_DELAY (FRAME_SYNC_PERIOD - ( NBR_CHANNELS * DEFAULT_PULSE_WIDTH))
byte servo = 3; // these could also be: #define servo 3
byte servoRoll = 5;
byte servoPitch = 6;
byte servoYaw = 11;
// arrays for channel info, note that the first channel is at index 0
byte counter[NBR_CHANNELS+1]; // holds pulse width / 128
byte remainder[NBR_CHANNELS+1]; // holds pulse width % 128
byte pins[NBR_CHANNELS+1] = {servo,servoRoll,servoPitch,servoYaw}; //holds pins associated with servos
volatile uint8_t Channel = 0; // counter holding the channel being pulsed
volatile uint8_t ISRCount; // counter used in the interrupt routines;
ISR (TIMER2_OVF_vect)
{
++ISRCount; // increment the overlflow counter
if (ISRCount == counter[Channel] ) // are we on the final iteration for this channel
{
TCNT2 = remainder[Channel]; // yes, set count for overflow after remainder ticks
}
else if(ISRCount > counter[Channel])
{
// we have finished timing the channel so pulse it low and move on
digitalWrite(pins[Channel],LOW); // pulse this channel low
Channel++; // now increment to the next channel
ISRCount = 0; // reset the isr iteration counter
TCNT2 = 0; // reset the clock counter register
if(Channel < NBR_CHANNELS){ // check if we need to pulse this channel
digitalWrite(pins[Channel],HIGH); // not done yet so pulse the next channel high
}
else if(Channel == NBR_CHANNELS){ // check if this is the synch delay
// all channels pulsed, this is the sync delay, do nothing in this version
}
else {
Channel = 0; // all done so start over
digitalWrite(pins[Channel],HIGH); // pulse the first channel high
}
}
}
void ServoWrite(byte channel, unsigned int pulsewidth){
// calculate and store the values for the given channel
if(channel <= NBR_CHANNELS) { // ensure channel is valid
if (channel < FRAME_SYNC_INDEX){ // if not frame sync, ensure pulse width is valid
if( pulsewidth < MIN_PULSE_WIDTH )
pulsewidth = MIN_PULSE_WIDTH;
else if( pulsewidth > MAX_PULSE_WIDTH )
pulsewidth = MAX_PULSE_WIDTH;
}
counter[channel] = pulsewidth / 128;
remainder[channel] = 255 - (2 * (pulsewidth - (counter[channel] * 128))); // the number of 0.5us ticks for timer overflow
}
}
unsigned int ServoRead(byte channel){
// returns the pulse width for the given channel
unsigned int pulsewidth = counter[channel] * 128 ;
pulsewidth += ((-remainder[channel] - 255) / 2);
return pulsewidth;
}
void ServoActivate(){
for(int i=0; i < NBR_CHANNELS; i++) {
pinMode( pins[i], OUTPUT) ; // set servo pins to output
ServoWrite(i, DEFAULT_PULSE_WIDTH); // store default values
}
ServoWrite(FRAME_SYNC_INDEX, FRAME_SYNC_DELAY); // store the frame sync period
Channel = 0; // start from the first channel
ISRCount = 0; // clear the value of the ISR counter;
/* setup for timer 2 */
TIMSK2 = 0; // disable interrupts
TCCR2A = 0; // normal counting mode
TCCR2B = _BV(CS21); // set prescaler of 8
TCNT2 = 0; // clear the timer2 count
TIFR2 = _BV(TOV2); // clear pending interrupts;
TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt
}
void setup()
{
Serial.begin(19200);
for(int i=0; i < NBR_CHANNELS; i++) {
ServoWrite(i, 1000 + (i*250)); // test pulses from 1ms to 1.75ms in 250us steps
}
ServoActivate(); // start the interrupt handler pulsing the servos
}
void loop()
{
unsigned int val = analogRead(0); // use a pot to set the pulse width for the first servo
ServoWrite(0,val + 1000); // write the pot value + 1000 to the first servo
}
