ROBOTC.net forumshttp://www.robotc.net/forums/ Programming the Odometry of Rover 5http://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 mmfloat 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 Grouphttp://www.phpbb.com/