Forum Rule: Always post complete source code & details to reproduce any issue!

# Thread: Incremental encoder speed calculation

1. ## Incremental encoder speed calculation

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;
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]=wL;
buff[i]=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
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
else pos_L=-1;
wL=2*pos_L*PI*risingL.freq/500;
}```  Reply With Quote

2. 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).  Reply With Quote

3. 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?  Reply With Quote

4. 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);```  Reply With Quote

5. 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-...ets/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.  Reply With Quote

#### Posting Permissions

• You may not post new threads
• You may not post replies
• You may not post attachments
• You may not edit your posts
•