View unanswered posts | View active topics It is currently Thu Dec 18, 2014 8:36 pm






Reply to topic  [ 1 post ] 
Wrong in Odometry Results 
Author Message
Rookie

Joined: Sun Jul 20, 2014 10:14 pm
Posts: 11
Post Wrong in Odometry Results
Hello all,

Actually , I have been since two weeks looking for convinced and final solution for my problem , actually I am completely lost , I am working on mobile robot (Rover 5) with 2 motors , 2 encoders . the controller that designed to the robot needs to know the odometery of mobile robot (X ,Y, Heading Angle ) , actually I am trying to function the encoders for this purpose , getting X ,Y, Heading Angle by measuring the traveled distance by each wheel , so to get the X ,Y, Heading Angle values , I should compute a accurate readings without missing any counts or ticks as could as possible .

The problem now is : In the code below , while I am testing the encoders counts , I noticed that odometry results computed by code are wrong and not identical the value on real world where robot is located .

In the test code the speed of right and left motors are feed up 50 PWM & 100 PWM respectively and at same time , but when I solve the odometry equations manually and compare results with code results , the two results are not identical , for example , lets take the output line

Left Encoder= 27 Right Encoder= 15 X= 0.01 Y= 0.03 Heading= 56.22


WheelDiameter = 0.062;
TrackWidth = 0.189;
CountsPerRevolution = 83;

deltaHeading = (double)(deltaRight - deltaLeft) / TrackWidth;
= ( 15 - 27 ) / 0.189
= -63.4920

so 56.22 not equal to -63.4920


Left Encoder= 705 Right Encoder= 571 X= -0.17 Y= -0.09 Heading= 531.41

deltaHeading = (double)(deltaRight - deltaLeft) / TrackWidth;
= ( 571 - 705 ) / 0.189
= -708.99

so 531.41 not equal -708.99

Actually , I don't know where is the problem , Is it in the code ? Is it in the hardware ? or what ?

the code below the output of code in the attachments

Code:
#define encoder0PinA 2
#define encoder0PinB 4
#define encoder1PinA 3
#define encoder1PinB 5

#define PI 3.14159265
#define TwoPI 6.28318531


volatile int LeftEncoderCounts = 0;
volatile int RightEncoderCounts = 0;
int WR=50;  // angular velocity of right wheel 
int WL=100;  // angular velocity of right wheel                       
     
     
     
       
    double _DistancePerCount;
    double _radiansPerCount;

    //long _leftEncoderCounts;
    long _PreviousLeftEncoderCounts;

   // long _RightEncoderCounts;
    long _PreviousRightEncoderCounts;
       
       
       
        double X;  // x coord in global frame
        double Y;  // y coord in global frame
        double Heading;  // heading (radians) in the global frame. The value lies in (-PI, PI]
       
       
       
        double WheelDiameter;
                double TrackWidth;
                double CountsPerRevolution;
                double DistancePerCount;
                double RadiansPerCount;





int ENA=8;    // SpeedPinA connected to Arduino's port 8 
int ENB=9;    // SpeedPinB connected to Arduino's port 9

int IN1=48;    // RightMotorWire1 connected to Arduino's port 48
int IN2=49;    // RightMotorWire2 connected to Arduino's port 49

int IN3=50;    // RightMotorWire1 connected to Arduino's port 48
int IN4=51;    // RightMotorWire2 connected to Arduino's port 49


void setup() {
 
    Serial.begin (9600);
 
 pinMode(ENA,OUTPUT);
 pinMode(ENB,OUTPUT);
 pinMode(IN1,OUTPUT);
 pinMode(IN2,OUTPUT);
 pinMode(IN3,OUTPUT);
 pinMode(IN4,OUTPUT);


 
 digitalWrite(ENA,HIGH);    //enable motorA
 digitalWrite(ENB,HIGH);    //enable motorB
 
  pinMode(encoder0PinA, INPUT);
  pinMode(encoder0PinB, INPUT);
  pinMode(encoder1PinA, INPUT);
  pinMode(encoder1PinB, INPUT);
 
// encoder pin on interrupt 0 (pin 2)
attachInterrupt(0, doEncoderA, CHANGE); 
 
// encoder pin on interrupt 1 (pin 3)

attachInterrupt(1, doEncoderB, CHANGE); 

   
                        WheelDiameter = 0.062;
                        TrackWidth = 0.189;
                        CountsPerRevolution = 83;

                        DistancePerCount = (PI * WheelDiameter) / (double)CountsPerRevolution;
                        RadiansPerCount = DistancePerCount / TrackWidth;
 
 
}

void loop(){ //Do stuff here

    int rightPWM;
  if (WR > 0) {
    //forward
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
   
  }  else if (WR < 0){
    //reverse
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  }
 
  if (WR == 0) {
   rightPWM = 0;
   analogWrite(ENA, rightPWM);
  } else {
    rightPWM = map(abs(WR), 1, 100, 1, 255);
   // rightPWM=WR;
    analogWrite(ENA, rightPWM);
  }

 int leftPWM;
 
  if (WL > 0) {
     //forward
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
  }  else if (WL < 0) {
     //reverse
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);}
 
  if (WL == 0) {
    leftPWM = 0;
    analogWrite(ENB, leftPWM);
  } else {
    leftPWM = map(abs(WL), 1, 100, 1, 255);
    //leftPWM=WL;
    analogWrite(ENB, leftPWM);
  }

 long deltaLeft = LeftEncoderCounts - _PreviousLeftEncoderCounts;
 long deltaRight = RightEncoderCounts - _PreviousRightEncoderCounts;


                double deltaDistance = 0.5f * (double)(deltaLeft + deltaRight) * DistancePerCount;
                double deltaX = deltaDistance * (double)cos(Heading);
                double deltaY = deltaDistance * (double)sin(Heading);

                double deltaHeading = (double)(deltaRight - deltaLeft) / TrackWidth;

                X += deltaX;
                Y += deltaY;
                Heading += deltaHeading;
       
                if (Heading > PI)
                {
                        Heading -= TwoPI;
                }
                else
                {
                        if (Heading <= -PI)
                        {
                                Heading += TwoPI;
                        }
                }
               

                _PreviousLeftEncoderCounts = LeftEncoderCounts;
                _PreviousRightEncoderCounts =RightEncoderCounts;
 
 
Serial.print("Left Encoder= ");
Serial.print(LeftEncoderCounts*-1);
Serial.print("\t\t");
Serial.print("Right Encoder= ");
Serial.print (RightEncoderCounts*-1);
Serial.print("\t\t");
Serial.print("X= ");
Serial.print(X);
Serial.print("\t\t");
Serial.print("Y= ");
Serial.print(Y);
Serial.print("\t\t");
Serial.print("Heading= ");
Serial.println(Heading);
   
}

void doEncoderA(){

  // look for a low-to-high on channel A
  if (digitalRead(encoder0PinA) == HIGH) {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinB) == LOW) { 
      LeftEncoderCounts = LeftEncoderCounts + 1;         // CW
    }
    else {
      LeftEncoderCounts = LeftEncoderCounts - 1;         // CCW
    }
  }
  else   // must be a high-to-low edge on channel A                                       
  {
    // check channel B to see which way encoder is turning 
    if (digitalRead(encoder0PinB) == HIGH) {   
      LeftEncoderCounts = LeftEncoderCounts + 1;          // CW
    }
    else {
      LeftEncoderCounts = LeftEncoderCounts - 1;          // CCW
    }
 
  }
   

}


void doEncoderB(){

  // look for a low-to-high on channel B
  if (digitalRead(encoder1PinB) == HIGH) {   
   // check channel A to see which way encoder is turning
    if (digitalRead(encoder1PinA) == HIGH) { 
      RightEncoderCounts = RightEncoderCounts + 1;         // CW
    }
    else {
      RightEncoderCounts = RightEncoderCounts- 1;         // CCW
    }
  }
  // Look for a high-to-low on channel B
  else {
    // check channel B to see which way encoder is turning 
    if (digitalRead(encoder1PinA) == LOW) {   
      RightEncoderCounts = RightEncoderCounts + 1;          // CW
    }
    else {
      RightEncoderCounts = RightEncoderCounts - 1;          // CCW
    }
  }

}


Attachments:
Output_of_the_code.rar [728 Bytes]
Downloaded 27 times
Fri Sep 19, 2014 7:10 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 1 post ] 

Who is online

Users browsing this forum: No registered users and 2 guests


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.