Programming the Odometry of Rover 5

Hello all, :)

I have started in the programming stage of my project , and my first step is to made and test the odometry of my Rover 5 robot on Arduino Uno by using encoders to determine position and orientation .

I wrote this code and I don’t know if that code right or there are some mistakes, because I am novice to Arduino and Robotic field so I need for some suggestions and corrections if there were .

Arduino codes posted below.

#define encoder1A  0       //signal A of left encoder  (white wire)

#define encoder1B  1      //signal B of left encoder  (yellow wire)

#define encoder2A  2      //signal A of right encoder  (white wire)

#define encoder2B  3      //signal B of right encoder  (yellow wire)

volatile int encoderLeftPosition = 0;      // counts of left encoder 

volatile int encoderRightPosition = 0;     // counts of right encoder 

float  DIAMETER  = 61  ;         // wheel diameter (in mm)  

float distanceLeftWheel, distanceRightWheel, Dc, Orientation_change;

float ENCODER_RESOLUTION = 333.3;      //encoder resolution (in pulses per revolution)  where in Rover 5,  1000 state changes per 3 wheel rotations 

int x = 0;           // x initial coordinate of mobile robot 

int y = 0;           // y initial coordinate of mobile robot 

float Orientation  = 0;       // The initial orientation of mobile robot 

float WHEELBASE=183  ;       //  the wheelbase of the mobile robot in mm

float CIRCUMSTANCE =PI * DIAMETER  ;

void setup() 

{ 

  pinMode(encoder1A, INPUT); 

  digitalWrite(encoder1A, HIGH);       // turn on pullup resistor

  pinMode(encoder1B, INPUT); 

  digitalWrite(encoder1B, HIGH);       // turn on pullup resistor

  pinMode(encoder2A, INPUT); 

  digitalWrite(encoder2A, HIGH);       // turn on pullup resistor

  pinMode(encoder2B, INPUT); 

  digitalWrite(encoder2B, HIGH);       // turn on pullup resistor

  attachInterrupt(0, doEncoder, CHANGE);       // encoder pin on interrupt 0 - pin 3

  Serial.begin (9600);

} 



void loop()

{



  distanceLeftWheel = CIRCUMSTANCE * (encoderLeftPosition / ENCODER_RESOLUTION);       //  travel distance for the left and right wheel respectively 

  distanceRightWheel = CIRCUMSTANCE * (encoderRightPosition / ENCODER_RESOLUTION);     // which equal to pi * diameter of wheel * (encoder counts / encoder resolution ) 



  Dc=(distanceLeftWheel + distanceRightWheel) /2 ;            // incremental linear displacement of the robot's centerpoint C



  Orientation_change =(distanceRightWheel - distanceLeftWheel)/WHEELBASE;    // the robot's incremental change of orientation , where b is the wheelbase of the mobile robot ,



  Orientation = Orientation + Orientation_change ;          //  The robot's new relative orientation 



  x = x + Dc * cos(Orientation);                            // the relative position of the centerpoint for mobile robot 

  y = y + Dc * sin(Orientation);

}



void doEncoder(){



  //  ---------- For Encoder 1 (Left)  -----------



  if (digitalRead(encoder1A) == HIGH) {   // found a low-to-high on channel A

    if (digitalRead(encoder1B) == LOW) {  // check channel B to see which way

                                             // encoder is turning

      encoderLeftPosition = encoderLeftPosition - 1;         // CCW

    } 

    else {

      encoderLeftPosition = encoderLeftPosition + 1;         // CW

    }

  }

  else                                        // found a high-to-low on channel A

  { 

    if (digitalRead(encoder1B) == LOW) {   // check channel B to see which way

                                              // encoder is turning  

     encoderLeftPosition = encoderLeftPosition + 1;          // CW

    } 

    else {

      encoderLeftPosition = encoderLeftPosition - 1;          // CCW

    } }





  //  ------------ For Encoder 2 (Right)-------------



  if (digitalRead(encoder2A) == HIGH) {   // found a low-to-high on channel A

    if (digitalRead(encoder2B) == LOW) {  // check channel B to see which way  encoder is turning                                      

      encoderRightPosition = encoderRightPosition - 1;         // CCW

    } 

    else {

      encoderRightPosition = encoderRightPosition + 1;         // CW

    }

  }

  else                                        // found a high-to-low on channel A

  { 

    if (digitalRead(encoder2B) == LOW) {   // check channel B to see which way  encoder is turning



     encoderRightPosition = encoderRightPosition + 1;          // CW

    } 

    else {

     encoderRightPosition = encoderRightPosition - 1;          // CCW

    }}}

Does the program produce the results as you would expect ? You are not outputting any of the values calculated so how do you know what is happening ?

  pinMode(encoder1A, INPUT); 
  digitalWrite(encoder1A, HIGH);       // turn on pullup resistor

is easier to understand if you write it as

  pinMode(encoder1A, INPUT_PULLUP);

float ER = 333.3;      //encoder resolution (in pulses per revolution)  where in Rover 5,  1000 state changes per 3 wheel rotationsI am intrigued by this as it implies that you can have a third of a pulse. Can you please provide a link to the hardware that you are using.

  attachInterrupt(0, doEncoder, CHANGE);      // encoder pin on interrupt 0 - pin 3What is attached to pin 3 ? It would appear to be signal B of the right encoder. Shouldn't the left encoder also trigger the interrupt ?

Pi * diaConsider making the result of this multiplication a constant to avoid repeatedly calculating it as it is not going to change whilst the program is running.

 /* If pinA and pinB are both high or both low, it is spinning   
   * forward. If they're different, it's going backward.
   */

Are you sure that this is true ? I am not saying that it isn't, because I don't know, but you need to be sure.

Is there any way in which you can avoid the floating point calculations which by the nature of the Arduino are slow. Multiply all units by 100 and work in integers perhaps ?

I have taken the liberty of removing the unnecessary blank lines from your program to make it easier to read and it now looks like this

#define encoder1A  0       //signal A of left encoder  (white wire)
#define encoder1B  1      //signal B of left encoder  (yellow wire)
#define encoder2A  2      //signal A of right encoder  (white wire)
#define encoder2B  3      //signal B of right encoder  (yellow wire)
volatile int encoderLPos = 0;      // counts of left encoder 
volatile int encoderRPos = 0;     // counts of right encoder 
float  dia = 61  ;         // wheel diameter (in mm)  
float Dl, Dr, Dc, Ori_ch;
float ER = 333.3;      //encoder resolution (in pulses per revolution)  where in Rover 5,  1000 state changes per 3 wheel rotations 
int x = 0;           // x initial coordinate of mobile robot 
int y = 0;           // y initial coordinate of mobile robot 
float Ori  = 0;       // The initial orientation of mobile robot 
float Pi = 3.14;
float b=183  ;       // b is the wheelbase of the mobile robot in mm

void setup() 
{ 
  pinMode(encoder1A, INPUT); 
  digitalWrite(encoder1A, HIGH);       // turn on pullup resistor
  pinMode(encoder1B, INPUT); 
  digitalWrite(encoder1B, HIGH);       // turn on pullup resistor
  pinMode(encoder2A, INPUT); 
  digitalWrite(encoder2A, HIGH);       // turn on pullup resistor
  pinMode(encoder2B, INPUT); 
  digitalWrite(encoder2B, HIGH);       // turn on pullup resistor
  attachInterrupt(0, doEncoder, CHANGE);       // encoder pin on interrupt 0 - pin 3
  Serial.begin (9600);
} 

void loop()
{
  Dl= Pi * dia * (encoderLPos/ ER);       // Dl & Dr are travel distance for the left and right wheel respectively 
  Dr= Pi * dia * (encoderRPos/ ER);     // which equal to pi * diameter of wheel * (encoder counts / encoder resolution ) 
  Dc=( Dl + Dr) /2 ;            // incremental linear displacement of the robot's centerpoint C
  Ori_ch=(Dr - Dl)/b;          // the robot's incremental change of orientation , where b is the wheelbase of the mobile robot ,
  Ori = Ori + Ori_ch ;          //  The robot's new relative orientation 
  x = x + Dc * cos (Ori);      // the relative position of the centerpoint for mobile robot 
  y = y + Dc * sin(Ori);
}

void doEncoder() 
{

  /* If pinA and pinB are both high or both low, it is spinning   
   * forward. If they're different, it's going backward.
   */

  if (digitalRead(encoder1A) == digitalRead(encoder1B)) 
  {
    encoderLPos++;
  } 
  else 
  {
    encoderLPos--;
  }

  if (digitalRead(encoder2A) == digitalRead(encoder2B))
  {
    encoderRPos++;
  } 
  else 
  {
    encoderRPos--;
  }
}

Code:

/* If pinA and pinB are both high or both low, it is spinning * forward. If they're different, it's going backward. */

Are you sure that this is true ? I am not saying that it isn't, because I don't know, but you need to be sure.

It is not true, and that logic won't work. See the section on "incremental rotary encoder", state diagram, here https://en.wikipedia.org/wiki/Rotary_encoder

UKHeliBob: Does the program produce the results as you would expect ? You are not outputting any of the values calculated so how do you know what is happening ?

  pinMode(encoder1A, INPUT); 
  digitalWrite(encoder1A, HIGH);       // turn on pullup resistor

is easier to understand if you write it as

  pinMode(encoder1A, INPUT_PULLUP);

float ER = 333.3;      //encoder resolution (in pulses per revolution)  where in Rover 5,  1000 state changes per 3 wheel rotationsI am intrigued by this as it implies that you can have a third of a pulse. Can you please provide a link to the hardware that you are using.

  attachInterrupt(0, doEncoder, CHANGE);       // encoder pin on interrupt 0 - pin 3What is attached to pin 3 ? It would appear to be signal B of the right encoder. Shouldn't the left encoder also trigger the interrupt ?

Pi * diaConsider making the result of this multiplication a constant to avoid repeatedly calculating it as it is not going to change whilst the program is running.

 /* If pinA and pinB are both high or both low, it is spinning   
   * forward. If they're different, it's going backward.
   */

Are you sure that this is true ? I am not saying that it isn't, because I don't know, but you need to be sure.

Is there any way in which you can avoid the floating point calculations which by the nature of the Arduino are slow. Multiply all units by 100 and work in integers perhaps ?

I have taken the liberty of removing the unnecessary blank lines from your program to make it easier to read and it now looks like this

#define encoder1A  0       //signal A of left encoder  (white wire)
#define encoder1B  1      //signal B of left encoder  (yellow wire)
#define encoder2A  2      //signal A of right encoder  (white wire)
#define encoder2B  3      //signal B of right encoder  (yellow wire)
volatile int encoderLPos = 0;      // counts of left encoder 
volatile int encoderRPos = 0;     // counts of right encoder 
float  dia = 61  ;         // wheel diameter (in mm)  
float Dl, Dr, Dc, Ori_ch;
float ER = 333.3;      //encoder resolution (in pulses per revolution)  where in Rover 5,  1000 state changes per 3 wheel rotations 
int x = 0;           // x initial coordinate of mobile robot 
int y = 0;           // y initial coordinate of mobile robot 
float Ori  = 0;       // The initial orientation of mobile robot 
float Pi = 3.14;
float b=183  ;       // b is the wheelbase of the mobile robot in mm

void setup() {   pinMode(encoder1A, INPUT);   digitalWrite(encoder1A, HIGH);       // turn on pullup resistor   pinMode(encoder1B, INPUT);   digitalWrite(encoder1B, HIGH);       // turn on pullup resistor   pinMode(encoder2A, INPUT);   digitalWrite(encoder2A, HIGH);       // turn on pullup resistor   pinMode(encoder2B, INPUT);   digitalWrite(encoder2B, HIGH);       // turn on pullup resistor   attachInterrupt(0, doEncoder, CHANGE);       // encoder pin on interrupt 0 - pin 3   Serial.begin (9600); }

void loop() {   Dl= Pi * dia * (encoderLPos/ ER);       // Dl & Dr are travel distance for the left and right wheel respectively   Dr= Pi * dia * (encoderRPos/ ER);     // which equal to pi * diameter of wheel * (encoder counts / encoder resolution )   Dc=( Dl + Dr) /2 ;            // incremental linear displacement of the robot's centerpoint C   Ori_ch=(Dr - Dl)/b;          // the robot's incremental change of orientation , where b is the wheelbase of the mobile robot ,   Ori = Ori + Ori_ch ;          //  The robot's new relative orientation   x = x + Dc * cos (Ori);      // the relative position of the centerpoint for mobile robot   y = y + Dc * sin(Ori); }

void doEncoder() {

  /* If pinA and pinB are both high or both low, it is spinning       * forward. If they're different, it's going backward.    */

  if (digitalRead(encoder1A) == digitalRead(encoder1B))   {     encoderLPos++;   }   else   {     encoderLPos--;   }

  if (digitalRead(encoder2A) == digitalRead(encoder2B))   {     encoderRPos++;   }   else   {     encoderRPos--;   } }

Thanks a lot for the tips , and sorry for this mistake , I modified the code above .

this is the datasheet of my Rover 5 robot https://www.sparkfun.com/datasheets/Robotics/Rover%205%20Introduction.pdf

Maria88: Thanks a lot for the tips , and sorry for this mistake , I modified the code above .

this is the datasheet of my Rover 5 robot https://www.sparkfun.com/datasheets/Robotics/Rover%205%20Introduction.pdf

Hi Maria,

Can you post your final (working) code?