Hello all,
I was here since two weeks ago in this great and wonderful forum and I want to thank all the members for their support, I really get more benefits since I belonged to this family .
Now 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, becase 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
}}}