# Programming the Odometry of Rover 5 [closed]

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 .

thanks a lot

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

• You've posted some code - do you have a specific question? Is there an issue you're having with the code?
– Chuck
Jun 12, 2015 at 15:45