ROBOTC.net forums
http://www.robotc.net/forums/

Programming the Odometry of Rover 5
http://www.robotc.net/forums/viewtopic.php?f=15&t=9211
Page 1 of 1

Author:  Maria88 [ Thu Aug 14, 2014 9:38 am ]
Post subject:  Programming the Odometry of Rover 5

Hello for 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 .


thanks a lot


Arduino codes posted below.



Code:

   
#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

    }}}

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/