Forum Rule: Always post complete source code & details to reproduce any issue!
Results 1 to 3 of 3

Thread: Incremental encoder speed calculation

  1. #1
    Junior Member
    Join Date
    Jun 2022
    Posts
    2

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

  2. #2
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    16,027
    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).

  3. #3
    Junior Member
    Join Date
    Jun 2022
    Posts
    2
    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?

Posting Permissions

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