Problem with Servos and Interrupts

Hi,

I'm experiencing a strange problem with Arduino and servo control. I use the Servo / SoftwareServo libraries to drive a Devantech MD22 motor controller, and measure the motor speed using a quadrature encoder.

I'd like to drive my servos from an interrupt routine on Timer 2, setup to run once every 1ms, like so:

#if defined(MC_TIMER_INTERRUPT)
  // prepare timer: prescaler to 1 0 0 and enable overflow interrupt
  TCCR2A &= ~((1 << WGM21) | (1 << WGM20));
  TCCR2B |= (1 << CS22);
  TCCR2B &= ~((1 << CS21) | (1 << CS20));

  TIMSK2 &= ~((1 << OCIE2A) | (1 << OCIE2B));
  TIMSK2 |= (1 << TOIE2);
#endif

This causes the timer overflow routine to be called once every 1ms. Since I want it to run at 100Hz, I have a counter in it that just returns 9 out of 10 times. The routine is called correctly; right now, I don't do anything else in there than just set the servo value according to a potentiometer's position.

I find that with this configuration, I get very noisy speed from the motor and a strange "twitching" in the speed, both using Servo and SoftwareServo libraries. Here is a plot of how it looks (red: potentiometer value, green: motor speed, not up to scale but that's not important).

Servo library:

SoftwareServo library:

If I don't have the routine setup as an interrupt handler, but call it periodically from within loop(), the whole thing looks like this:

Still not super smooth, but much much better.

Is there a problem with updating servo values from within an ISR? Can anyone please enlighten me?

Thank you very much in advance!

Regards,
Björn

I'd like to drive my servos from an interrupt routine on Timer 2

Are you trying to use the servo library and timer2? Perhaps you can say more about what your sketch is doing and post your timer2 interrupt handler code

If you just need to run your servos from timer2 (leaving timer1 free for some other purpose) there is a servo library here that uses timer2: Arduino Playground - HomePage
And a thread discussing it here: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1230479947

No, I'm just calling servo.write(x) from my interrupt handler on timer 2. The actual servo driving is done from timer 1, I suppose?

My interrupt routine is this (slightly cleaned up):

#if defined(MC_TIMER_INTERRUPT)
// ISR for timer 2, called once per 1ms
ISR(TIMER2_OVF_vect)
#else
void Control()
#endif
{
  // we only want to do something every 10ms
  static int overflow_counter = 0;
  if(overflow_counter % 10 != 0) return;
  
  // leftservohard is written to by the main loop from a potentiometer
  leftservo.write(leftservohard); 
#if defined(MC_SOFTWARESERVO)
  leftservo.refresh(); 
#endif

  // speed control PID will follow here.
}

The problem with updating servo values using an interrupt is that the interrupt handler will interfere with the servo pulse timing, and that is what you are seeing. There is not much point updating the servo values more frequently than every 20 ms or so. Can you do the updating in loop?

If not, can you say more about the functionality and structure of your application. Perhaps post the code.

Well, I'm trying to set up a motor speed controller working at 100Hz, and since I'm going to be doing potentially slow but non-realtime-critical stuff in loop() (such as graphic output or i2c communication), I'd rather have the speed control in a separate interrupt that's called in realtime.

Here's my complete code, but warning, it's long and not really cleaned up:

// to make compiler happy :-)
void dummy(void) {}

#include <SoftwareServo.h>
#include <Servo.h>

#include <pins_arduino.h>
#include <limits.h> // for ULONG_MAX

/* 
  User defines:
  MC_PROTOBOARD -- protoboard or "real"
  MC_SOFTWARESERVO -- whether to use software servos or hardware servos
  MC_TIMER_INTERRUPT -- whether to use timer interrupt for control (otherwise, call MotorController::Control() periodically
*/

#define MC_PROTOBOARD
//#define MC_SOFTWARESERVO
//#define MC_TIMER_INTERRUPT


extern unsigned long timer0_overflow_count;

namespace MotorController {

#if defined(MC_PROTOBOARD)

#define PIN_EncLA 2
#define PIN_EncLB 4
#define PIN_EncRA 4
#define PIN_EncRB 2
#define PIN_MotL 9
#define PIN_MotR 10

#else

#define PIN_EncLA 18
#define PIN_EncLB 22
#define PIN_EncRA 19
#define PIN_EncRB 23
#define PIN_MotL 3
#define PIN_MotR 2

#endif

// experimental
#define MINPULSE 1050
#define MAXPULSE 1950

uint8_t PORT_EncLB, PORT_EncRB, BIT_EncLB, BIT_EncRB;
#define READPORT(port, bitmask) (*portInputRegister(port) & bitmask)

#define CPUCOUNT() ((timer0_overflow_count << 8) + TCNT0)
#define CPUCOUNT_TO_USECS(t) ((t) * (64 / clockCyclesPerMicrosecond()))
#define USECS_TO_CPUCOUNT(t) ((t) * (clockCyclesPerMicrosecond() / 64))
#define CPT_TO_RPM10(x) (1000000 / (3*CPUCOUNT_TO_USECS(x)))
#define RPM10_TO_CPT(x) (1000000 / (3*CPUCOUNT_TO_USECS(x)))
#define CALCCPT(t1, t2, num, res) do{ t1<=t2 ? res = (t2-t1) / num : res = (ULONG_MAX-t1+t2) / num; }while(0);


typedef struct {
  unsigned long lastticktime;
  unsigned long tickcount_abs;
  long tickcount;
} MotorStateInternal;

typedef struct {
  unsigned long tgtcpt, curcpt; // CPU clock cycles per tick. Negative => backward, 0 not moving, positive => forward
  long tickcount, numtimerssincelasttick;
} MotorStateExternal;

// Do not use leftint or rightint, they're internal; use left and right 
MotorStateInternal leftint, rightint, li1, ri1, li2, ri2;
MotorStateExternal left, right;
int leftservohard, rightservohard;

#if defined(MC_SOFTWARESERVO)
SoftwareServo leftservo, rightservo;
#else
Servo leftservo, rightservo;
#endif

// ISR for left motor
#if defined(MC_PROTOBOARD)
ISR(INT0_vect)
#else
ISR(INT5_vect)
#endif
{
  leftint.lastticktime = CPUCOUNT();
  leftint.tickcount_abs++;

  if(READPORT(PORT_EncLB, BIT_EncLB)) {
    leftint.tickcount--;
  } else {
    leftint.tickcount++;
  }
}

// ISR for right motor
#if defined(MC_PROTOBOARD)
ISR(INT1_vect)
#else
ISR(INT4_vect)
#endif
{
  rightint.lastticktime = CPUCOUNT();
  rightint.tickcount_abs++;

  if(READPORT(PORT_EncRB, BIT_EncRB)) {
    rightint.tickcount--;
  } else {
    rightint.tickcount++;
  }
}

#if defined(MC_TIMER_INTERRUPT)
// ISR for timer 2, called once per 1ms
ISR(TIMER2_OVF_vect)
#else
void Control()
#endif
{
  // we only want to do something every 10ms
  static int overflow_counter = 0;
  if(overflow_counter % 10 != 0) return;
  
  leftservo.write(leftservohard); rightservo.write(rightservohard);
#if defined(MC_SOFTWARESERVO)
  leftservo.refresh(); rightservo.refresh();
#endif
  return;
    
  li1 = li2; ri1 = ri2;

  cli();
  li2 = leftint; ri2 = rightint;
  sei();

  // determine current clocks per tick for left and right motor
  unsigned long firstticktime, lastticktime; 
  unsigned long tickcount = (li2.tickcount_abs >= li1.tickcount_abs ? 
                              (li2.tickcount_abs - li1.tickcount_abs) :
                              (ULONG_MAX - li1.tickcount_abs + li2.tickcount_abs));

  // have any ticks occured at all?
  if(tickcount <= 1) {
    if(left.numtimerssincelasttick < ULONG_MAX) {
      left.numtimerssincelasttick++;
    }
  } else {
    left.numtimerssincelasttick = 0;
    CALCCPT(li1.lastticktime, li2.lastticktime, tickcount, left.curcpt);
  }
  
  tickcount = (ri2.tickcount_abs >= ri1.tickcount_abs ? 
                (ri2.tickcount_abs - ri1.tickcount_abs) : 
                (ULONG_MAX - ri1.tickcount_abs + ri2.tickcount_abs));

  // have any ticks occured at all?
  if(tickcount <= 1) {
    if(right.numtimerssincelasttick < ULONG_MAX) {
      right.numtimerssincelasttick++;
    }
  } else {
    right.numtimerssincelasttick = 0;
    CALCCPT(ri1.lastticktime, ri2.lastticktime, tickcount, right.curcpt);
  }
    
  // determine direction (careful: averages over what happened in 10ms)
  if(left.numtimerssincelasttick > 20) left.curcpt = 0;  
  else if(li2.tickcount < left.tickcount) left.curcpt *= -1;  
  if(right.numtimerssincelasttick > 20) right.curcpt = 0;  
  else if(ri2.tickcount < right.tickcount) right.curcpt *= -1;

  left.tickcount = li2.tickcount; right.tickcount = ri2.tickcount;
}

void Init() {
  // prepare pins  
  PORT_EncLB = digitalPinToPort(PIN_EncLB);
  BIT_EncLB = digitalPinToBitMask(PIN_EncLB);  
  PORT_EncRB = digitalPinToPort(PIN_EncRB);
  BIT_EncRB = digitalPinToBitMask(PIN_EncRB);  
  pinMode(PIN_EncLB, INPUT);
  pinMode(PIN_EncRB, INPUT);
  
  memset(&left, 0, sizeof(MotorStateExternal));
  memset(&right, 0, sizeof(MotorStateExternal));
  memset(&leftint, 0, sizeof(MotorStateInternal));
  memset(&rightint, 0, sizeof(MotorStateInternal));

  // prepare servos
#if defined(MC_SOFTWARESERVO)
  leftservo.setMinimumPulse(MINPULSE);
  leftservo.setMaximumPulse(MAXPULSE);
  rightservo.setMinimumPulse(MINPULSE);
  rightservo.setMaximumPulse(MAXPULSE);
  leftservo.attach(PIN_MotL);
  rightservo.attach(PIN_MotR);
#else
  leftservo.attach(PIN_MotL, MINPULSE, MAXPULSE);
  rightservo.attach(PIN_MotR, MINPULSE, MAXPULSE);
#endif

  leftservohard = rightservohard = 90;

#if defined(MC_TIMER_INTERRUPT)
  // prepare timer: prescaler to 1 0 0 and enable overflow interrupt
  TCCR2A &= ~((1 << WGM21) | (1 << WGM20));
  TCCR2B |= (1 << CS22);
  TCCR2B &= ~((1 << CS21) | (1 << CS20));

  TIMSK2 &= ~((1 << OCIE2A) | (1 << OCIE2B));
  TIMSK2 |= (1 << TOIE2);
#endif

#if defined(MC_PROTOBOARD)
  EICRA = 0x03; // INT0 - Rising edge 
  EIMSK |= 0x01; // enable only int0
#else
  EICRB |= (0x3 << 2 | 0x3); // INT4 + INT5 - Rising edge
  EIMSK |= (1 << 5 | 1); // enable INT4 and INT5
#endif
}

void GetMotorState(MotorStateExternal &left_rtn, MotorStateExternal &right_rtn) {
  cli();
  left_rtn = left;
  right_rtn = right;
  sei();
}
} // namespace


/* 
 * Arduino stuff 
 */

void setup(void)
{
  Serial.begin(115200);
  MotorController::Init();
}

void loop(void)
{
  static int counter = 0;
  counter++;
  
#if !defined(MC_TIMER_INTERRUPT)
  // not handled by interrupt; call this every 10ms
  MotorController::Control();
#endif
  
  if(counter % 4 == 0) {
    // outer control is done every 40ms
    int potval = analogRead(2);
    int servoval = map(potval, 0, 1023, 0, 180);
    
#if defined(MC_TIMER_INTERRUPT)
    cli();
#endif
    MotorController::leftservohard = MotorController::rightservohard = servoval;
#if defined(MC_TIMER_INTERRUPT)
    sei();
#endif

    MotorController::MotorStateExternal leftext, rightext;
    MotorController::GetMotorState(leftext, rightext);
  
    float currpm10 = CPT_TO_RPM10(leftext.curcpt);
    float tgtrpm10 = CPT_TO_RPM10(leftext.tgtcpt);
  
    Serial.print(servoval);
    Serial.print(" ");
    Serial.print(currpm10);
    Serial.print(" ");
    Serial.println(tgtrpm10);
  }
  delay(10);
}

How long does that loop code actually take. If its under 10ms then you would be much better off doing the servo refresh there and get rid of the timer2 interrupt

Well, it doesn't do much now, but I'm going to drive graphic output on a S65Shield from there, which takes very long. And I still don't understand why calling servo.write() from a timer 2 interrupt handler disturbs PWM if the servo is driven by timer 1?

And I still don't understand why calling servo.write() from a timer 2 interrupt handler disturbs PWM if the servo is driven by timer 1?

I'd let more experienced software types see if the following could be a possiblity or not.

When your timer2 interrupts into it's ISR all other interrupts are disabled until that ISR completes and returns to the main loop. Perhaps you are spending too much time inside the ISR and therefore missing interrupts from timer1.

Generally one is recommended to keep ISR routines as simple and short as possible, calling other functions from inside a ISR is not recommended practice. Just test or input for some condition and set a global volatile variable or flag and let the main loop check that variable or flag and do the actual action you require.

Lefty

When your timer2 interrupts into it's ISR all other interrupts are disabled until that ISR completes and returns to the main loop. Perhaps you are spending too much time inside the ISR and therefore missing interrupts from timer1.

OK, I've just looked into the Atmel datasheets. Seems that interrupt service routines really globally prevent each other from being called, so Servo won't work. That still doesn't explain why SoftwareServo doesn't work, because that doesn't use interrupts at all... but I've taken a look at SoftwareServo's code, and that does a busy-wait for the slowest servo. Blergh.

OK, I'll handle my motor control without interrupts.