Servo jitters when using interrupts to read quadrature encoder

Using Arduino Programming Questions
1

Hello. I made a simple robot which uses a DC motor (driven by an h-bridge, controlled by the PWM from the Arduino) for driving and a servo motor for steering. The DC motor is a Pololu motor with a quadrature encoder Pololu - 19:1 Metal Gearmotor 37Dx52L mm 12V with 64 CPR Encoder (No End Cap).

In my code, the steering angle is a function of the encoder count. The problem is that the servo jitters when I use the interrupt to "read" the encoder output and increment the encoder counter. I think it's because the encoder's interrupt handler prevents the Servo library's interrupt handler from sending the control signal, hence the jitter.

Would enabling the interrupts by placing "interrupts()" in "handleEncAInterrupt()" solve my problem? Or will it cause problems because "handleEncAInterrupt()" will become re-entrant?

#include <digitalWriteFast.h>
#include <Servo.h>
float sm[] = {0,3148.1199,8647.021};
float targetAlpha[] = {1535,1677.3481};

float tacc = 1000;
float tconst = 5000;
int tASize = 2;



//outputs

Servo steering;

//variables for reading encoder
volatile bool encAState; //volatile so it can be changed outside the interrupt
volatile bool encBState;
volatile long count = 0;


//variables needed by computeSteeringAngle()
const float vmax=50000; //counts/sec
const float wmax=20000; //micros/second


float s1,s2;
float alpha;
float alphaAbs;

int i=1; //counter for computeSteeringAngle()

const float accel=2.5;

float velocity;
float velocityConst;
float tAccStart,tConstStart,tDecStart,tEnd;

boolean chk=true;

void setup(){
  steering.attach(9);
  Serial.begin(115200);
  //steering angle variables  
  alphaAbs = abs(targetAlpha[i]-targetAlpha[i-1]);
  s1=sm[i]-alphaAbs*vmax/(2*wmax);
  s2=sm[i]+alphaAbs*vmax/(2*wmax);
  
  //for the output
  pinMode(11,OUTPUT);
  pinMode(12,OUTPUT);
  pinMode(13,OUTPUT);
  digitalWrite(12,LOW);
  digitalWrite(13,HIGH);
  
  //for the quadrature encoder
  pinMode(2, INPUT);
  pinMode(4, INPUT);
  digitalWrite(2, LOW);
  digitalWrite(4, LOW);
  attachInterrupt(0, handleEncAInterrupt, CHANGE);


  computeSteeringAngle(count);
  steering.writeMicroseconds(alpha);
  
  delay(2000);
  analogWrite(11,60);
}

void loop(){
  computeSteeringAngle(count);
  if(count>=18340.656){
    analogWrite(11,0);
  }
}



void computeSteeringAngle(long s){
  if(i<tASize){
        if(s<s1 && chk){
            alpha=targetAlpha[i-1];
            steering.writeMicroseconds((int)alpha);
            chk=false;
        }
        else if(s>=s1 && s<s2){
            if((targetAlpha[i]-targetAlpha[i-1])>0)
                alpha=s*(wmax/vmax)-sm[i]*(wmax/vmax)+(targetAlpha[i]+targetAlpha[i-1])/2;
            else
                alpha=s*(-wmax/vmax)-sm[i]*(-wmax/vmax)+(targetAlpha[i]+targetAlpha[i-1])/2;
          steering.writeMicroseconds((int)alpha);
              
          }
        else if(s>=s2){
            i=i+1;
            alphaAbs = abs(targetAlpha[i]-targetAlpha[i-1]);
            s1=sm[i]-alphaAbs*vmax/(2*wmax);
            s2=sm[i]+alphaAbs*vmax/(2*wmax);
            alpha=targetAlpha[i-1];
            chk=true;
            steering.writeMicroseconds((int)alpha);
            
        }
  }
  

}



void handleEncAInterrupt(){
  encAState = digitalReadFast(2);
  encBState = digitalReadFast(4);
  count+= encAState==encBState ? 1 : -1;
}
2

It would be very helpful if you could add comments to the statements describing what they do.

Does the servo turn or just jitter? You might need to add a delay after the "write" command to allow the servo to move into position.

computeSteeringAngle(count);
steering.writeMicroseconds(alpha);

I assume you know "alpha" can be only in the 0 - 179 range.

3

First of all, avoid floats. We have been spoiled by modern 32/64 bit cpu's with fast floating point built in. Floating point on an 8bit Arduino is VERY,VERY SLOW.

4

EDIT

I assume you know "alpha" can be only in the 0 - 179 range.

OOPS. I forgot about Microseconds.

It would be interesting to know if "alpha" is in the proper range.

5

In my program, there are target steering "angles" (I used writeMicroseconds for more accuracy) for a certain distance traveled. What "computeSteeringAngle()" does is that it ramps the change in the steering angle since the the servo motor turns too fast if I don't ramp it. I used float because I originally wrote the code in Scilab so I could check the output of the function, but I do intend to change it in the future. (But I did read in another post that in the Arduino, float operations take the same time as integer operations since the code is very optimized. Is this true?)

The program does make the servo turn in response to the traveled distance, but the servo jitters every few seconds. I tried removing the parts that read the encoder, somewhat simulating the encoder output using millis(), and the jitters vanished.

#include <Servo.h>
float sm[] = {0,3148.1199,8647.021,14146};
float targetAlpha[] = {1544,1677.3481,1544};;

float tacc = 1000;
float tconst = 5000;
int tASize = 3;

Servo steering;


//variables needed by computeSteeringAngle()
const float vmax=50000; //counts/sec
//const float wmax=2833.333; //micros/second
const float wmax=20000; //micros/second

//float sm[]={0,6000,10000,20000,28000,35000};
//float targetAlpha[]={90,70,80,20,40,100};

float s1,s2;
float alpha;
float alphaAbs;

int i=1; //counter for computeSteeringAngle()

//variables needed by computeVelocity()
const float accel=2.5;

//float tacc=408.33695;
//float tconst=19183.326;

float velocity;
float velocityConst;
float tAccStart,tConstStart,tDecStart,tEnd;




unsigned long count=0;

boolean chk=true;



void setup(){
  steering.attach(9);
  Serial.begin(115200);
  //steering angle variables  
  alphaAbs = abs(targetAlpha[i]-targetAlpha[i-1]);
  s1=sm[i]-alphaAbs*vmax/(2*wmax);
  s2=sm[i]+alphaAbs*vmax/(2*wmax);
  
  //for the output
  pinMode(11,OUTPUT);
  pinMode(12,OUTPUT);
  pinMode(13,OUTPUT);
  digitalWrite(12,LOW);
  digitalWrite(13,HIGH);
  


  count=millis()/1.6;
  computeSteeringAngle(count);

  steering.writeMicroseconds(alpha);
  
  delay(2000);
  analogWrite(11,100);
}

void loop(){
  count=millis()/1.6;
  computeSteeringAngle(count);
  //steering.writeMicroseconds(alpha);
  Serial.println(count);
  //Serial.print(" ");
  //Serial.println(alpha);
  if(count>=14146.656){
    analogWrite(11,0);
  }
}



void computeSteeringAngle(long s){
  if(i<tASize){
        if(s<s1 && chk){
            alpha=targetAlpha[i-1];
            steering.writeMicroseconds((int)alpha);
            chk=false;
        }
        else if(s>=s1 && s<s2){
            if((targetAlpha[i]-targetAlpha[i-1])>0)
                alpha=s*(wmax/vmax)-sm[i]*(wmax/vmax)+(targetAlpha[i]+targetAlpha[i-1])/2;
            else
                alpha=s*(-wmax/vmax)-sm[i]*(-wmax/vmax)+(targetAlpha[i]+targetAlpha[i-1])/2;
          steering.writeMicroseconds((int)alpha);
              
          }
        else if(s>=s2){
            i=i+1;
            alphaAbs = abs(targetAlpha[i]-targetAlpha[i-1]);
            s1=sm[i]-alphaAbs*vmax/(2*wmax);
            s2=sm[i]+alphaAbs*vmax/(2*wmax);
            alpha=targetAlpha[i-1];
            chk=true;
            steering.writeMicroseconds((int)alpha);
            
        }
  }
  

}

Edit: I just realized... If the Servo.h library also uses interrupts, then it might also prevent the update of the encoder counter. I guess I can't control a servo motor and have an accurate encoder counter using one Arduino then.

6

I suppose the question to ask is, do you need to use an interrupt to keep track of the encoder count?

7

I think I do since the encoder frequency is quite fast. The maximum speed of the motor is 500rpm, multiplying that with 600counts/rev is 5000 counts per second, which means I need to increment the counter every 200 microseconds ("worst" case). If I don't use interrupts, I would need to make the Arduino keep on reading the state of the pin. If I do that, I probably can't have an accurate measurements if I have to run other codes.

I tried modifying the interrupt handler:

void handleEncAInterrupt(){
  interrupts(); //enable all interrupts
  EIMSK &= 0b11111110; //disable external interrupts
  encAState = digitalReadFast(2);
  encBState = digitalReadFast(4);
  count+= encAState==encBState ? 1 : -1;
  EIMSK |= 0b00000001; //enable external interrupts
}

The jittering stopped for the most part (I saw it jitter a little, near the beginning of the program, but it was fine otherwise). Is there a better way to fix this than what I've done?

8

Sounds like your encoder was generating a lot of bounces, that code alteration should only really
have the effect of suppressing some fast glitches (or rather reducing the number of times the interrupt
routine fires when fast glitches are present). The possibility is that while the ISR was running another
edge came in to re-trigger the external interrupt. By disabling interrupts this might have prevented
the re-trigger (actually I'm not certain about this, have to re-read the relevant parts of the datasheet).

What kind of encoder is this? If its an opto encoder perhaps the outputs are not schmitt-triggered?

The ATmega's interrupt priorities are such that all external interrupts and pin-change interrupts take
priority over the counter/timer interrupts (which are used by the Servo library), and so inevitable cause
some jitter on the servo outputs.

I think the digitalReadFast library will be about as good as you can get with speeding up the encoder
ISR...

9

The encoder is a Hall effect encoder.

That's the output from Pololu's site. I haven't viewed the output of my encoder using an oscilloscope, yet.

Just to clarify, the modifications were:

  interrupts(); //enable all interrupts
  EIMSK &= 0b11111110; //disable external interrupts
  ...
  EIMSK |= 0b00000001; //enable external interrupts

Before I added the interrupts() part, the servo jittered. The binary operations on EIMSK to disable the external interrupts were added so an infinite loop will hopefully be avoided. (I'm not sure if interrupts that occur when interrupts are disabled are postponed or ignored..?)

10

The maximum speed of the motor is 500rpm, multiplying that with 600counts/rev is 5000 counts per second, which means I need to increment the counter every 200 microseconds ("worst" case). If I don't use interrupts, I would need to make the Arduino keep on reading the state of the pin. If I do that, I probably can't have an accurate measurements if I have to run other codes.

Why do you need 600 counts/wheel rev? You cannot get that precision since wheels are likely to have occassional slippage. I realise that this is dictated by Pololu's design, but I am not clear on the use case.