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