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