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[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
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;
}```

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).