View unanswered posts | View active topics It is currently Mon Nov 24, 2014 2:00 pm






Reply to topic  [ 1 post ] 
Programming the Odometry of Rover 5 
Author Message
Rookie

Joined: Sun Jul 20, 2014 10:14 pm
Posts: 11
Post 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

    }}}


Thu Aug 14, 2014 9:38 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 1 post ] 

Who is online

Users browsing this forum: No registered users and 1 guest


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.