0
$\begingroup$

I attached two quadrature optical encoders to the wheel of the mobile robot. I also have imu6050 attached to the breadboard to calculate the orientation of the wheel mobile robot localization. I want to know if my code is right and also determine the starting point for the wheel mobile robot using these sensors.

  #include <MPU6050_tockn.h>

  MPU6050 mpu6050(Wire);

  #define ENC_A1  2
  #define ENC_B1  3

  #define ENC_A2  18
  #define ENC_B2  19

  #define wheelbase 88                                           // wheelbase corner-to-corner
  #define count_per_revolution 12                               //(mm)
  #define Lwheel_c 140                                          //(mm)
  #define Rwheel_c 139.9                                        //(mm)
  #define pi 3.142
  #define TWOPI 6.284
  
  // Main loop refresh period.
  #define REFRESH_MS  50

  // Main serial data connection to computer.
  #define BAUD_RATE   9600

  // Encoder signal line states
  volatile boolean state_a1 = 0;
  volatile boolean state_b1 = 0;

  // Encoder signal line states
  volatile boolean state_a2 = 0;
  volatile boolean state_b2 = 0;

  // Encoder position
  volatile int enc_pos_1 = 0;
  int enc_pos_prev_1 = 0;
  int enc_pos_change_1 = 0;

  volatile int enc_pos_2 = 0;
  int enc_pos_prev_2 = 0;
  int enc_pos_change_2 = 0;

  // normally on a differential drive system to when moving
  // forwards the wheels are rotating in opposite directions
  int encoder_direction_left = 1;
  int encoder_direction_right = 1;

  // Timing
  unsigned long micros_current = 0;
  unsigned long micros_prev = 0;
  long micros_change = 0;

  /* ----------------------------------------------------------------------- */
  /* local variables */
  float dleft; 
  float dright;
  float last_theta;
  
  /* ----------------------------------------------------------------------- */
  /* odometer maintains these global accumulator variables: */
  /* ----------------------------------------------------------------------- */
  float x_pos;                    // bot X position 
  float y_pos;                    // bot Y position 
  float wheel_distance;
  float theta;                     /* bot heading in radians minus theta_offset */
  float theta_global;
  
  /* rate of rotation measurements, used for stasis error detection */
  float wheel_theta; 
  float imu_theta;

  long timer;
  
  void setup() {

  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
    
    pinMode(ENC_A1, INPUT);
    pinMode(ENC_B1, INPUT); 

    pinMode(ENC_A2, INPUT);
    pinMode(ENC_B2, INPUT); 

    state_a1 = (boolean) digitalRead(ENC_A1);
    state_b1 = (boolean) digitalRead(ENC_B1);

    state_a2 = (boolean) digitalRead(ENC_A2);
    state_b2 = (boolean) digitalRead(ENC_B2);

    attachInterrupt(0, interrupt_enc_a1, CHANGE);
    attachInterrupt(1, interrupt_enc_b1, CHANGE);

    attachInterrupt(5, interrupt_enc_a2, CHANGE);
    attachInterrupt(4, interrupt_enc_b2, CHANGE); 

    micros_prev = micros();

    Serial.begin(BAUD_RATE);
  }

  void loop() {
   mpu6050.update();

  if(millis() - timer > 1000){
     timer = millis();
  }
    // Calculate elapsed time  
    micros_current = micros();
    if (micros_current < micros_prev) {
        micros_change = micros_current + (4294967295 - micros_prev);
    } else {
        micros_change = micros_current - micros_prev;
    }
      
    // Calculate change in encoder position.
    enc_pos_change_1 = enc_pos_1 - enc_pos_prev_1;
    
    enc_pos_change_2 = enc_pos_2 - enc_pos_prev_2;
  
    // Emit data
    Serial.print(enc_pos_change_1);
    Serial.print("\t");
    Serial.print(enc_pos_change_2);
    Serial.print("\t");
    Serial.print(dright);
    Serial.print("\t");
    Serial.print(dleft);
    Serial.print("\t");
    Serial.print(x_pos);
    Serial.print("\t");
    Serial.print(y_pos);
    Serial.print("\t");
    Serial.print(theta);
    Serial.print("\t");
    Serial.print(mpu6050.getAngleX());
    Serial.print("\t");
    Serial.print(mpu6050.getAngleY());
    Serial.print("\t");
    Serial.println(mpu6050.getAngleZ());
    
    // and update last sampling for next time 
    enc_pos_prev_1 = enc_pos_1;

    enc_pos_prev_2 = enc_pos_2;

    micros_prev = micros_current;
 
    dleft = (enc_pos_change_1) * Lwheel_c;

    dright = (enc_pos_change_2)* Rwheel_c;
  
    // calculate distance we have traveled since last sampling 
    
      wheel_distance = (dleft + dright)/2.0;

    // accumulate total rotation around our center 
   
    wheel_theta = ((dleft - dright)/wheelbase)*(180/pi); // You can multiply theta * (180.0/PI) to get the heading in degrees

   /* read the YAW value from the imu struct and convert to radians */
   
   theta_global = ((float)mpu6050.getAngleZ()*TWOPI)/65536.0;

    /* calculate rotation rate-of-change for stasis detector */
    
    imu_theta = theta_global - last_theta;
    
    last_theta = theta_global;

   // if (g->yaw_enable) {

    if (mpu6050.getAngleZ()) {
      
    theta = theta_global;

    } else {
    
    theta += wheel_theta;
    
    }
    
    //and clip the rotation to plus or minus 360 degrees
    
    theta -= (float)((int)(theta/TWOPI))*TWOPI;
    
    if (theta < -PI) { 
      
      theta += TWOPI;

      }else { 
        
        if (theta > PI) 
        
        theta -= TWOPI; 
        
        }

    x_pos =  x_pos + wheel_distance * cos(theta);

    y_pos =  y_pos + wheel_distance * sin(theta);
   
   delay(REFRESH_MS);
  }

  // Detect pulses from depth encoder.

  void interrupt_enc_a1() {
    
    if (!state_a1) {
    
        state_b1 ? enc_pos_1++: enc_pos_1--;         
    }
   
    state_a1 = !state_a1;
  }

  void interrupt_enc_b1() {
    state_b1 = !state_b1;
  }

  // Detect pulses from depth encoder.

  void interrupt_enc_a2() {
    if (!state_a2) {
        state_b2 ? enc_pos_2++: enc_pos_2--;         
    }
    state_a2 = !state_a2;
  }

  void interrupt_enc_b2() {
    state_b2 = !state_b2;
  }```
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.