Wednesday, July 28, 2010

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?

21 comments:

  1. Just curious - if I wanted to just output or control an ESC which I have just discovered accepts as low as 11ms low between pulses as opposed to the 20ms - could I create this effect with the use of a single timer to allow arduino pwm timer to continue?

    The idea is I need to provide throttle control as fast as possible to prevent over current conditions and thus FET damage in high end RC controllers - I've measured current transient detection time and have it down to 3ms with a dedicated comparator - the normal ppm would require between 21ms and 22ms per pulse, since servos are forgiving I thought I'd check how low I could set the constant for low pulse and it's near half - would this not in effect translate to nearly twice as fast reaction (since they won't support I2C bus control)?

    Nice stuff btw - and love the BLDC bridge solution - do you ever consider offering your PCBs for shields and other things for the general public - there would be a market for it and it could help fund your research and development?

    ReplyDelete
  2. Sending pulses faster than 20ms is definitely possible with this interrupt setup, especially if you only want to control a single ESC. As written, the time between pulses depends on the number of servos multiplexed onto the timer, just over 2ms per channel. So if you use only 5-6 channels, the period would be 10-12ms. If you only need one channel, you can use "dummy" channels that don't actually go to a pin to make the gap whatever you want.

    Either way you dedicate Timer 1 (and only Timer 1) to running servo pulses, which means it won't be able to use analogWrite() on pins 9 or 10. The other PWMs are on Timer 0 and Timer 2, so they would still work fine. If you want to keep all six PWMs operational and control servo pulses, delay() and delayMicroseconds() might be the better option. This method ties up the processor for the duration of the pulse, but for only a single 1-2ms pulse every 10ms, that's not bad.

    I don't have the capital to produce and sell boards, but I do provide the EAGLE and Gerber files so that anyone can get them printed. I would be more than happy if somebody (or some site like Sparkfun) wanted to take on board production and sell them at a reasonable cost. I just can't do it myself. I also can't provide support for them beyond the initial documentation: It's sort-of build at your own risk. :)

    ReplyDelete
  3. As a quick update: In going through the most up-to-date Arduino servo library (servo.h) it does in fact use interrupts on Timer 1 to generate the servo values. Only the old "software servo library" uses the less efficient method. The new version also has the pin 9/10 PWM conflict.

    So, Arduino did eventually make a servo library that doesn't suck. I just wish more people understood why. ;)

    ReplyDelete
  4. A quick thanks and response:
    I get that the newer arduino servo libs use Timer1 and so they don't "SUCK" but in my particular case... To control a single ESC should tie up approx 2ms-3ms of processor time using your method... I've implemented it in singleton fashion (forget arrays) and used it convert either hall or 0-5v throttles to RC ESC with great success, efficiency and since my particular ESC reacts with as little as 6ms of OFF time it means my refresh rate is as fast as 8ms even at WOT. By timing my A2D sampling right, I estimate total analog action->ppm reaction time to be on the figure of 6-10ms maximum. Thanks again!
    Thanks!

    ReplyDelete
  5. What ESC do you use? I'm on the lookout for quick-reacting ESCs for quadrotors. Anything <10ms would probably be good. And small (~10A).

    ReplyDelete
  6. Just wanted to thank you so much for the idea (and implementation). I've been looking for something like this for a while now (for Quadcopter). Anyways, I think I found a way to make the update rate (period) faster and thought I should also contribute (based on your idea). The period of the PWM signals are directly proportional to the number of servos. It is possible, however, to decrease these periods by 1/2 by using OCR1B as well (not only OCR1A) in the following way. Use OCR1A to control half the servos of the array, while the other half is controlled by OCR1B.

    I am getting 4ms period for 4 servos using this new method (useful for faster ESC update rates).

    For 8 servos, in your example above, this implementation would yield 8 ms period (instead of 16 ms).

    Please let me know if you want the full arduino code. I'd be happy to share it.

    Sincerely,
    Alex
    aamartir@gmail.com

    ReplyDelete
  7. That is a great idea. It could also let you control a large number of servos (basically every digital pin could be a servo). And it still doesn't interfere with any of the other timers.

    Thanks!

    ReplyDelete
    Replies
    1. This Post is a bit old, but I also waned to thank you for the idea. I ended up going a bit of a different way though. I took the Arduno interupt driven servo library and the Software servo library and made a hybrid simultaneous interupt driven servo library out of them. Result, a digital servo/esc capable simultaneous servo library. At the moment only on PortB, so 6 Servos sumultaneously (up to 333Hz possible) I only use it at 200Hz though.

      Theory: Sorts all the PPMs in ascending order, shift the non simultaneous and following ones by 25micro seconds. Uses Timer 1.

      Just thought something like this would improve your print Quad dynamic response.

      I also wanted to thank you for your simplified balancing robot IMU filter, allows me to run my Pro-mini at 200Hz loop speed. Still a work in progress though.

      If you are interesed in the library let me know on this post.

      Regards.
      Jason

      Delete
    2. That sounds great - I would definitely like to see it.

      The PCB quad uses a brushless motor control chip with 0-5V analog input for speed, which is nice. I set the timers to run a 62.5kHz PWM and filter it at about 1kHz bandwidth. Very nice option for a small quadrotor.

      What are you using the IMU filter for? (A balancing robot?)

      Delete
    3. OK, I'll send it to the email on your Resumé. I don't know how to otherwise. You can post it here for anyone else who is interesed if you like.

      Its still a work in progress and could easily ported to other Ports as well. Tell me what you think and if I have any major no-no's. Programing is only a hobby for me, not an occupation.

      I can see that a 1kHz refresh would be much better than 333Hz.

      Ja, I'm building a balancing robot. I guess everyone has to sometime... I thought it would be an excellent learning platform where I am completely dependent on the controller for stability, but still close to the ground so that it does't hurt that much when it Falls. Once thats done I will let myself graduate to flying things.

      By the way have you seen the Frankenstein of balancing robots...
      http://www.youtube.com/watch?v=mWDFp1-twNU&list=UUgdOnrr1VzRW_AxUoZPbDnQ&index=14&feature=plcp

      I'm not building one like that though, just a "normal" one with a couple of twists.

      One thing I'm using though that might be interesting for you if you want to save some weight on your 4PCB is a DSM2 Orange RX100 Satelite reciever 11$ connected directly to the RX pin of the arduino, with code adapted from the ArduNXT project. (Binding beforhand with a normal RX610)

      Delete
    4. Please share Jatra's 333Hz code, I am interested in running a single RC servo at precisely this frequency.

      Delete
  8. Got it, thanks! Also, that Frankensegway is awesome! I love the tail rotor for steering.

    I don't think the satellite receiver would be much lighter than the XBee, do you? Also the XBee is nice because I get two-way data with it.

    ReplyDelete
  9. Hey Shane, I wonder if you can give me some advice here. I am using the standard Servo library that comes with the Arduino IDE to sweep two servos from 0 to 180 degrees. At the same time, by using an Ethernet shield, I can send a query from a browser that then tells me what the angle of the servo is at the time that request came in (through servo.read() ). The problem is, this request, process, and subsequent response, blocks the servo sweep for a very brief moment. Doing successive requests causes the servo to become jittery. I need to somehow get a response back to the browser without interrupting the servo.

    Someone then suggested using an interrupt based solution but seeing as how the Servo library already does this, I went looking for a different solutions, implemented differently, and came across your code. I compiled it, uploaded and I can have my servos sweep back and forth from servopwm(250) to servopwm(500). However, I noticed this isn't a full 180 degrees sweep, but a 90 degrees sweep.

    So my first question is, how can I change this to a full 180 degrees sweep? The servo documentation says it can if you feed it a pulse ranging from 600usec (0 degrees) to 2400usec (180 degrees).

    Second, how can I go about getting an angle reading from a servo, very much like servo.read() does it (which in and by itself isn't ideal because the servo would have to have moved first since it simply returns whatever the last instruction was that was sent to the servo, but hey, it's better than nothing.)

    Any suggestions?

    ReplyDelete
    Replies
    1. I can't see how servo.read() could halt the servo interrupt running in the background, although I admit I don't know exactly what the function does. I'm more suspicious of the other Ethernet shield functions. Have you done any tests isolate the two and see which is causing the delay?

      This code isn't set up to go to 2400us. You'd have to modify the timer to reset with a different period by changing the Timer 1 setup registers.

      Delete
    2. It's not the Servo.read() that's doing the blocking. It's the Ethernet process itself. Between it processing the request from the browser and sending back the data, it causes enough of a delay to disturb the servo's movement.

      Delete
    3. The other thing that just now hit me too is that using your code, I have no control over the speed in which the servo is moving from one position to the next. At least, I can't figure out how to do that. However, if I can't get it to do a full 180 degrees sweep, it becomes a moot point as I would have to go back to the standard Servo library.

      Delete
  10. This comment has been removed by the author.

    ReplyDelete
  11. I used the multiplex concept in your code to implement servos on ATTiny84:

    http://forum.arduino.cc/index.php?topic=198337.0

    ReplyDelete
  12. Hey Shane, i cant download your code anymore, wich im verry interested in for a school project.

    ReplyDelete
  13. Shane,
    I'd like to hire you for an Arduino servo control project. You have some availability in the next week or so? Please contact me at jimbeckham@zeroreels.com.
    I look forward to hearing back from you.
    Jim Beckham

    ReplyDelete