Incremental encoder speed calculation

Chamaut

New member
Hello,
I am working for the first time on an incremental encoder and on Teensy, so I would like to have your advice on my code because I don't know if it is optimal to calculate the motor speed. For each motor, I have an interrupt that calculates the direction of rotation as well as the frequency at which the encoder rotates. Then I calculate the speed in my loop and implement the values in buff. To have the most accurate frequency calculation possible, I use the processor cycle count but I don't find very accurate results (+/- 0.5 kHz). Can you help me with the optimisation of the code?
Thanks!

Code:
#define MOTOR_RL_PWM_F 4
#define MOTOR_RL_PWM_R 5
#define EN 28
#define MOTOR_RR_PWM_F 2
#define MOTOR_RR_PWM_R 3
#define ChannelA_LEFT 34
#define ChannelB_LEFT 35
#define ChannelA_RIGHT 14
#define ChannelB_RIGHT 38
#define GET_FREQ(int1,int2) 1/((float) (int1 - int2)* 1/(F_CPU_ACTUAL))/1000  // transform the number of cycle clock into freq in Khz [ 1/( (float) Delta_Cycle * 1/fCPU)]/1000

uint64_t cycles64()
{
    static uint32_t oldCycles = ARM_DWT_CYCCNT;
    static uint32_t highDWORD = 0;

    uint32_t newCycles = ARM_DWT_CYCCNT;
    if (newCycles < oldCycles)
    {
        ++highDWORD;
    }
    oldCycles = newCycles;
    return (((uint64_t)highDWORD << 32) | newCycles);
}

struct TIMER {
  float freq;
  uint64_t previous;
};

volatile int pos_L=0;
volatile int pos_R=0;
int postposR=pos_R;
int postposL=pos_L;
float buff[500][2];
struct TIMER risingR;
struct TIMER risingL;
volatile float wR,wL;
float sumR, sumL;
int i=0;

void setup() {
  // pin configuration
  pinMode(MOTOR_RL_PWM_F, OUTPUT);
  pinMode(MOTOR_RL_PWM_R, OUTPUT);
  pinMode(MOTOR_RR_PWM_F, OUTPUT);
  pinMode(MOTOR_RR_PWM_R, OUTPUT);
  pinMode(EN, OUTPUT);

  pinMode(ChannelA_LEFT, INPUT);
  pinMode(ChannelB_LEFT, INPUT);
  pinMode(ChannelA_RIGHT, INPUT);
  pinMode(ChannelB_RIGHT, INPUT);

  //direction of rotation
  attachInterrupt(digitalPinToInterrupt(ChannelA_RIGHT), interruptR, RISING);
  attachInterrupt(digitalPinToInterrupt(ChannelA_LEFT), interruptL, RISING);

  //analogWriteFrequency(pinMode(MOTOR_RR_PWM_R, OUTPUT);,20000);
  analogWriteFrequency(MOTOR_RL_PWM_F,20000);
  analogWriteFrequency(MOTOR_RL_PWM_R,20000);
  analogWriteFrequency(MOTOR_RR_PWM_F,20000);
  analogWriteFrequency(MOTOR_RR_PWM_R,20000);

  //initial value
  analogWrite(MOTOR_RL_PWM_F,0);
  analogWrite(MOTOR_RL_PWM_R,0);
  analogWrite(MOTOR_RR_PWM_F,0);
  analogWrite(MOTOR_RR_PWM_R,0);
  digitalWrite(EN,LOW);

  delay(100);
}
 
void loop() {
  // put your main code here, to run repeatedly:
  digitalWrite(EN,HIGH);  //The command is activated by the driver
  analogWrite(MOTOR_RR_PWM_F,160);    //counter-closewise rotation
  analogWrite(MOTOR_RR_PWM_R,0);  //closewise rotation
  analogWrite(MOTOR_RL_PWM_F,0);    //counter-closewise rotation
  analogWrite(MOTOR_RL_PWM_R,160);    //closewise rotation
  /*if (i<500){
    buff[i][0]=wL;
    buff[i][1]=wR;
    sumR+=wR;
    sumL+=wL;
    i++;
  }
  else {
    sumR = sumR/500;
    sumL=sumL/500;
    Serial.printf("SumL : %f \t sumR : %f \n",sumL,sumR);
    i=0;
  }*/
  
}

//pos=1 => closewise rotation             pos=-1 => coutner closewise rotation
void interruptR(){
  //frequency
  uint64_t currentR = cycles64();
  risingR.freq=(float)GET_FREQ(currentR,risingR.previous);
  risingR.previous=currentR;
  Serial.println(risingR.freq);
  //rotation
  if (digitalRead(ChannelB_RIGHT)==LOW)  pos_R=1; 
  else pos_R=-1;
  wR=2*PI*pos_R*risingR.freq/500;
}

void interruptL(){
  //frequency 
  uint64_t currentL = cycles64();
  risingL.freq=(float)GET_FREQ(currentL,risingL.previous);
  risingL.previous=currentL;
  //rotation
  if (digitalRead(ChannelB_LEFT)==LOW) pos_L=1;
  else pos_L=-1;
  wL=2*pos_L*PI*risingL.freq/500;
}
 
Are encoder interrupts going to be over 7 seconds apart?

If not using the cycles64() is wasted effort, if neither triggers in that ~7.15 second windows - then the cycles64() will not update correctly.

Having Serial.println(risingR.freq); in the _isr() is bad form, and may be causing missed interrupts on one or both of the _isr()'s. Perhaps make a "volatile float freqR;" for R and L and have the _isr() store the value there for the loop() code to print when it changes.

Until the problem is solved perhaps print the value in Hz rather than KHz where the extra math /1000 may be hiding something. And when used, putting it as /1000.0 will incline the compiler to treat the whole operation as (float).
 
Thank you for your reply defragster,
I just tested this code with a frequency generator and it works well in terms of accuracy. I think the problem is the noise generated by the encoder. Is there a way to fix this in the code (I already tried averaging but it doesn't solve the problem) or is the only way to implement a low pass filter?
 
Most encoders are open-collector, so try changing
Code:
  pinMode(ChannelA_LEFT, INPUT);
  pinMode(ChannelB_LEFT, INPUT);
  pinMode(ChannelA_RIGHT, INPUT);
  pinMode(ChannelB_RIGHT, INPUT);
to
Code:
  pinMode(ChannelA_LEFT, INPUT_PULLUP);
  pinMode(ChannelB_LEFT, INPUT_PULLUP);
  pinMode(ChannelA_RIGHT, INPUT_PULLUP);
  pinMode(ChannelB_RIGHT, INPUT_PULLUP);
 
Encoders like this are digital, they're not noisy as such. Are you not doing any type of debouncing of the signal from the mechanical encoder switches in the code? You can implement a low pass filter as part of the design (see page 3 - https://www.bourns.com/docs/Product-Datasheets/PEC11R.pdf), but debouncing code is usually mandatory if you want it to reliably work, there may be issues if it's used with interrupts however. I think polling is preferred.
 
Last edited:
Back
Top