View unanswered posts | View active topics It is currently Sun Oct 20, 2019 4:31 am  Page 1 of 1 [ 2 posts ]
 Print view Previous topic | Next topic
Wrong in Odometry Results
Author Message
Rookie

Joined: Sun Jul 20, 2014 10:14 pm
Posts: 11 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.28318531volatile 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 48int IN2=49;    // RightMotorWire2 connected to Arduino's port 49int IN3=50;    // RightMotorWire1 connected to Arduino's port 48int IN4=51;    // RightMotorWire2 connected to Arduino's port 49void 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 629 times    Fri Sep 19, 2014 7:10 am Rookie

Joined: Wed Mar 18, 2015 9:24 am
Posts: 1 First, your interrupt routines are not determining the direction correctly.
You need the "A" encoder and the PREVIOUS "B" encoder to tell direction, not the CURRENT "B" encoder level.
So you are not getting the right totals, since sometimes you're adding when you should be subtracting.

Secondly, you should be interrupting on all 4 encoder pins, not just 2 of them.

And thirdly, I and others, have had problems with the interrupts if you have one for each pin or one for each wheel. What I have done that works is to have one interrupt routine for all 4 pins (2 quad-encoders), or one interrupt for all 8 pins (4 quad-encoders).

Wed Mar 18, 2015 9:30 am
Display posts from previous:  Sort by Page 1 of 1 [ 2 posts ]

Who is online

Users browsing this forum: No registered users and 2 guests

 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