View unanswered posts | View active topics It is currently Mon Jun 17, 2019 7:02 pm

 Page 1 of 1 [ 1 post ]
 Print view Previous topic | Next topic
Programming the Odometry of Rover 5
Author Message
Rookie

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

Thu Aug 14, 2014 9:38 am
Display posts from previous:  Sort by
 Page 1 of 1 [ 1 post ]

#### Who is online

Users browsing this forum: No registered users and 1 guest

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

Search for:
 Jump to:  Select a forum ------------------ ROBOTC Applications    ROBOTC for LEGO MINDSTORMS       Third-party sensors    ROBOTC for CORTEX & PIC    ROBOTC for VEX IQ    ROBOTC for Arduino    Robot Virtual Worlds    Multi-Robot Communications    Issues and Bugs Competitions & Partners    Mini Urban Challenge    CS2N Robot Virtual Worlds Competitions       VEX Skyrise Competition 2014-2015       VEX Toss Up 2013-2014       FTC Block Party! 2013-2014    Competitions using VEX - BEST, TSA, VEX, and RoboFest!    FTC Programming    RoboCup Junior and Other ROBOT Competitions Virtual Brick Robotics Discussions    General Discussions    Project Discussions Off-Topic ROBOTC Forum & ROBOTC.net Suggestions/Feedback    ROBOTC Forums Suggestions/Comments    ROBOTC.net Suggestions/Comments       NXT Programming: Tips for Beginning with ROBOTC       VEX Programming: Tips for Beginning with ROBOTC    2013 Robotics Summer Of Learning       VEX Toss Up Programming Challenge       FTC Ring It Up! Programming Challenge    International Forums       Spanish Forums          ROBOTC for MINDSTORMS          ROBOTC for VEX       French Forums          ROBOTC pour Mindstorms          ROBOTC pour IFI VEX       Japanese Forums （日本語のフォーラム）       German Forums    2015 Spring Carnival Event    PLTW (Project Lead The Way)    Robotics Merit Badge    2014 Robotics Academy Summer of Learning

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