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 rotations`I 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 3`What 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 * dia`Consider 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.
*/

{
encoderLPos++;
}
else
{
encoderLPos--;
}

{
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 rotations`I 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 3`What 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 * dia`Consider 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(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?