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
#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
}
}
}
Output of the code .txt (4.5 KB)