1
$\begingroup$

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.

#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
    }
  }
}
$\endgroup$
1
  • $\begingroup$ You've posted some code - do you have a specific question? Is there an issue you're having with the code? $\endgroup$
    – Chuck
    Jun 12 '15 at 15:45
2
$\begingroup$

Your code won't work as you expect. The problem is that you use only one external interrupt, when you need two - one for each encode. With only one interrupt you will miss pulses from the second motor (for example when you brake one motor and still rotate the second to make a turn).

If you would like even better resolution, you can use two additinal interrupts, so you will have one for each encoder phase. If you are running short on external interrupt lines of uC, check out XOR logic ICs, or even dedicated quadrature decoders.

BTW. Lenght of the edge of a circle is circumference, not circumstance :)

$\endgroup$

Not the answer you're looking for? Browse other questions tagged or ask your own question.