Arduino Interrupt-Driven Servo Routine

Why do I keep developing for Arduino?!? Anyway, unlike my recent endeavor into high-power Arduino shield design (HexBridge, HexBridge BLDC), this is post is pure software.

I've been tasked with creating an Arduino servo library that doesn't suck. I'm not saying that the existing servo libraries suck or don't suck; I'm just saying that my semi-self-imposed task was to make one that doesn't suck. In 2.007, we used BASIC Stamps to generate servo pulses with the wonderful PULSOUT command. It's a single-line command that generates a single servo pulse. But I should go back one step. This is a servo pulse:

It's a periodic signal sent to an RC servo or speed controller (on the white/yellow wire) that sets the position of the servo, or the speed if it's a continuous rotation servo or speed controller. A 1.0ms pulse represents full reverse, a 1.5ms pulse represents neutral, and a 2.0ms pulse represents full forward. Any position or speed in between can be achieved, though the output isn't linear. The pulse signal has no physical meaning; i.e. it does not represent the voltage being applied to the motor inside the servo. It's just conveying information.

One very important thing about this signal: It must be sent to each servo approximately every 20ms. Most servos aren't picky about the exact timing, but if you wait too long, they will time out and turn off. For this reason, a microcontroller commanding servos must dedicate some effort to continuously sending out these pulses. Furthermore, if you use a command like PULSOUT on the BASIC Stamp, the microcontroller can do nothing else for the duration of the pulse. So if, for example, you needed to control 8 servos using PULSOUT, the BASIC Stamp would be tied up for up to 16ms just generating pulses, leaving very little time to do anything else before the pulses have to be sent out again.

Before I get yelled at: There is a servo library for Arduino and it is much more efficient than PULSOUT on a BASIC Stamp. Best I can tell (and please correct me if I'm wrong), it sorts the servo pulses from shortest to longest, starts them all simultaneously, and ends them in reverse length order using a hardware timer to keep track of the duration. So at most, it ties up your processor for 2ms (for all the servos). Pretty good. I would say this doesn't suck. But I would also say that I can do better.

There is obviously no reason to sit around doing nothing during the off time of a servo pulse. But there's also no reason to sit around doing nothing during the on time. It's only the transitions that require the processor to actually do anything. And with a hardware timer and interrupts, it's possible to set up a servo routine that runs entirely in the background, taking control only briefly to start and end a pulse. This is what I call an interrupt-driven servo routine. Here's the full code, only about 50 non-comment lines to run 8 servos in background interrupts. For the rest of the post, I'll step through it line-by line. It's pretty alpha, so feel free to yell at me for "doing it wrong."

// Interrupt-based Arduino Servo Template
// v1.0 | 7/28/2010
// scolton@mit.edu

// servo PWM value
//  |-----------|-----------|
// 250         375         500
// 1ms        1.5ms        2ms
unsigned int servopwm[10];

// servo index, used to multiplex Timer1
unsigned char servoindex;

Declaring some global variables. servopwm[10] is an array that stores the servo commands. Because of the way the timer will be set up, the value 250 corresponds to 1ms, 375 to 1.5ms, and 500 to 2.0ms. So, you effectively get 8-bit PWM resolution. But, an int data type is still required to hold numbers greater than 255. servoindex stores a looping index to which servo is being commanded at any instant in time.

// INTERRUPT SERVICE ROUTINES
// --------------------------
ISR(TIMER1_COMPA_vect)
{
  // interrupt on Timer1 compare match
  // (end of the servo pulse)
 
  // end servo pulse on the appropriate pin
  // faster version of digitalWrite(servoindex, LOW);
  if(servoindex <= 7)
  {
    PORTD &= ~(1 << servoindex);
  }
  else
  {
    PORTB &= ~(1 << (servoindex - 8));
  }
 
  // move on to the next servo (limit to 2-8)
  servoindex++;
  if(servoindex == 10)
  {
    servoindex = 2;
  }
 
  // set the compare match to the new servo pulse duration
  OCR1AH = servopwm[servoindex] >> 8;
  OCR1AL = servopwm[servoindex] & 0xFF;

  return;
}

This is the section that is not at all part of standard Arduino syntax. (As my friend pointed out, "You're not really programming an Arduino servo libarary; you're programming an AVR servo libary.") While Arduino does have some limited interrupt support, it's for external interrupts, such as a piece of code that is instantly triggered when a button is pressed.

The ATmega328 chip has the capability to generate internal interrupts as well. Here, I use one of Timer1's compare match interrupts. Timer1 is a hardware counter that ticks at a regular interval, which is configured later in the code. When it ticks to the 16-bit value stared in the OCR1AH and OCR1AL registers, it will trigger this compare match interrupt. Upon triggering, this bit of code takes over, interrupting whatever other code was running, and sets the servo pin low using a faster,more direct version of digitalWrite(). After setting the current servo pin low, it increments to the next servo and resets the compare match register to a new value. Then, the program returns to whatever code was previously running. The whole operation takes only a few microseconds.

ISR(TIMER1_OVF_vect)
{
  // interrupt on Timer1 overflow
  // start of the next servo pulse
 
  // start servo pulse on the appropriate pin
  // faster version of digitalWrite(servoindex, HIGH);
  if(servoindex <= 7)
  {
    PORTD |= (1 << servoindex);
  }
  else
  {
    PORTB |= (1 << (servoindex - 8));
  }
     
  return;
}
// --------------------------

This is another Timer1 interrupt that triggers on a Timer1 "overflow." As will later be configured, Timer1 counts to the number 511, then loops back around to 0. At the same time it loops, the new servo pin is set high. So, with the two interrupt routines, the servo pin turns on at 0, then turns off at some value between 250 and 500, which sets the pulse length. Because it loops through 8 servos, a servo that was just pulsed will sit idle while the next seven are commanded. Thus, the total sequence takes a bit over 16ms.

// REGULAR ARDUINO CODE
// --------------------

void setup()
{
  // TIMER1 SETUP FOR SERVOS
  // -----------------------
  // set Timer1 to clk/64 and 9-bit fast PWM
  // this is a period of 2.048ms for 16MHz clk
  // when multiplexed on 8 servos, this gives a 16ms period
  TCCR1A = 0x02;
  TCCR1B = 0x0B;
  // enable Timer1 compare match and overflow interrupts
  TIMSK1 = 0x03;
  // -----------------------
 
  // set initial servo index 
  servoindex = 2;
 
  // set all servos to neutral (375 = 1.500ms)
  // set all servo pins to outputs
  for(int i = 2; i <= 10; i++)
  {
    servopwm[i] = 375;
    pinMode(i, OUTPUT);
  }
}

Good old Arduino code. Oh wait, what's this hexadecimal crap? Well, it's required to setup up Timer1 at the right frequency, and resolution, and to request interrupts on compare match and overflow. It's basically setting some switches that define how Timer1 works. Arduino does this for you most of the time. If you really want to know more, RTFM (starting on page 114 of 562). This would be a good time to point out that because I take over Timer1, Arduino functions that depend on it, such as Pin 9 and 10 PWM, won't work.

void loop()
{
  // Your loop code here.
  // No need to worry about delays being longer than 20ms!
 
  // Set PWM values like this:
  //  |-----------|-----------|
  // 250         375         500
  // 1ms        1.5ms        2ms
 
  servopwm[2] = 250;
  servopwm[3] = 275;
  servopwm[4] = 300;
  servopwm[5] = 325;
  servopwm[6] = 350;
  servopwm[7] = 375;
  servopwm[8] = 400;
  delay(1000);
}
// --------------------

This is your job. Setting the value of servopwm[i] between 250 and 500 is only required once. The interrupts will continue generating servo pulses at that value until you command a new value. So, your loop is free to use delays of longer than 20ms, or functions that take a long time such as pulseIn() and Serial.print().

And that's it. Not that hard, right?