Moving robot in straight line

hi,

I have been working on building a differential wheeled robot with two motors and two encoders (one for each wheel). As expected, one wheel runs significantly faster then the other even though the PWM values are the same so I used PID class to correct error. I don’t know if I wrote it correctly so I’d appreciate it if someone could look over it and check for mistakes or ways I can improve it. I have been adjusting the P, I and D but the robot still drift to the right. My code is below and might be confusing so if you need clarification I’ll try and quickly reply.

thanks a lot

#include <PID_v1.h>
#include <RobotParams.h>
#include <OdometricLocalizer.h>
#include "Arduino.h"
#include <digitalWriteFast.h>  
#define PI 3.14159265 
#define TwoPI 6.28318531


//PID variables. 
double trackAdjustValue;
double trackSetpoint;
double trackError;
long ll= 0;
long rr= 0;
double Kp = 3;  //Determines how aggressively the PID reacts to the current amount of error (Proportional)
double Ki = 2;  //Determines how aggressively the PID reacts to error over time (Integral)
double Kd = 1;  //Determines how aggressively the PID reacts to the change in error (Derivative)


PID trackPID(&trackError, &trackAdjustValue, &trackSetpoint, Kp, Ki, Kd, DIRECT);


RobotParams _RobotParams = RobotParams();       // Class to Initialize robot parameters :  wheelDiameter , countsPerRevolution , trackWidth 
OdometricLocalizer _OdometricLocalizer(&_RobotParams);   // Class to compute X ,Y , Angle of robot 

int ENA=8;    // SpeedPinA  
int ENB=9;    // SpeedPinB 

int IN1=48;    // RightMotor1
int IN2=49;    // RightMotor2
 
int IN3=50;    // LeftMotor1
int IN4=51;    // LeftMotor2


// Quadrature encoders
// Left encoder
#define encoder0PinA 18  // interrupt 0

#define encoder0PinB 2  // interrupt 5

volatile signed int encoder0Pos = 0;



#define encoder1PinA 3  // interrupt 1

#define encoder1PinB 19  // interrupt 4

volatile signed int encoder1Pos = 0;



float RightWheel,LeftWheel;
float  Difference ;  
        
bool _IsInitialized = false;


void setup() {


Serial.begin(115200);
  trackAdjustValue = 0; 
  trackSetpoint = 0;
  trackError = 0;

  trackPID.SetMode(AUTOMATIC);
  trackPID.SetSampleTime(200);
  trackPID.SetOutputLimits(-20,20);


//Motors Pins
 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
 

 //Encoders Pins
 pinMode(encoder0PinA, INPUT); 
 pinMode(encoder0PinB, INPUT); 
// encoder pin on interrupt 0 (pin 2)

 attachInterrupt(5, doEncoderA, CHANGE);
// encoder pin on interrupt 5 (pin 3)

  attachInterrupt(0, doEncoderB, CHANGE);  
  
pinMode(encoder1PinA, INPUT); 
pinMode(encoder1PinB, INPUT); 
// encoder pin on interrupt 0 (pin 2)

 attachInterrupt(1, doEncoderA1, CHANGE);
// encoder pin on interrupt 5 (pin 3)

attachInterrupt(4, doEncoderB1, CHANGE); 
  
  
  
 InitializeDriveGeometry();
 RequestInitialization();

}




void loop() 
{


doEncoderA();
doEncoderB();
doEncoderA1();
doEncoderB1();


 _OdometricLocalizer.Update(encoder0Pos, encoder1Pos);     // Passing Encoders Counts to get X , Y , Heading
 
 moveForward();


Difference=encoder1Pos-encoder0Pos;

Serial.print("Difference");
Serial.print("\t");
Serial.println(Difference);

Serial.println(encoder1Pos);
Serial.println(encoder0Pos);
    
    delay(300);
}





void moveForward() 
{

    LeftWheel=20;
    RightWheel=20;

 //  Converting To PWM
   toPWM (RightWheel,LeftWheel);
   
  rr=encoder1Pos;
  ll=encoder0Pos;
  trackError = rr - ll;
if (trackError<=10){
  if (trackPID.Compute()) //true if PID has triggered
  {Serial.print(trackError); 
    RightWheel += trackAdjustValue;
   ll = 0;
   rr = 0;}}
   
}





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) {  
      encoder0Pos = encoder0Pos + 1;         // CW
    } 
    else {
      encoder0Pos = encoder0Pos - 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) {   
      encoder0Pos = encoder0Pos + 1;          // CW
    } 
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }
 // Serial.println (encoder0Pos, DEC);          
  // use for debugging - remember to comment out
}

void doEncoderB(){

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


void doEncoderA1(){

  // look for a low-to-high on channel A
  if (digitalRead(encoder1PinA) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder1PinB) == LOW) {  
      encoder1Pos = encoder1Pos + 1;         // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 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(encoder1PinB) == HIGH) {   
      encoder1Pos = encoder1Pos + 1;          // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }
//  Serial.println (encoder1Pos, DEC);          
  // use for debugging - remember to comment out
}

void doEncoderB1(){

  // 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) {  
      encoder1Pos = encoder1Pos + 1;         // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 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) {   
      encoder1Pos = encoder1Pos + 1;          // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }
}





void RequestInitialization()
{
    _IsInitialized = true;

    if (!_RobotParams.IsInitialized)
    {
      _IsInitialized = false;


}}

void InitializeDriveGeometry()
{
  float wheelDiameter =6.5 ; // in CM
  float trackWidth =18.9; // in CM
  int countsPerRevolution =333.3; //
  
  _RobotParams.Initialize(wheelDiameter, trackWidth, countsPerRevolution);
}







// Coverting to PWM

void toPWM (float Wr,float Wl)
{
     RightWheel=Wr;
     LeftWheel=Wl;
  
    int rightPWM, leftPWM;  
  if (RightWheel > 0) {
    //forward
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
    
  }  else if (RightWheel < 0){
    //reverse
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  }
  
  if (RightWheel == 0) {
   rightPWM = 0;
   analogWrite(ENA, rightPWM);
  } else {
    rightPWM = map(abs(RightWheel), 1, 100, 1, 255);

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

    analogWrite(ENB, leftPWM);
  }
  
  
}

What do your debug prints tell you is going on?

Are you sure the wheel circumferences are the same? You might be getting the same number of revolutions correctly, but if the left wheel is a bit smaller then the distances will differ.

AWOL:
What do your debug prints tell you is going on?

Actually the difference between left and right encoders is increased ascending as motors run, where left encoder counts faster than right encoder.

JimboZA:
Are you sure the wheel circumferences are the same? You might be getting the same number of revolutions correctly, but if the left wheel is a bit smaller then the distances will differ.

Yes, the wheel circumferences are the same. Actually I use Rover 5 robot, but the motors do not respond identically to PWM levels

Dears , any one can answer me I really need help

Actually I also check to just use the proportional part of a PID algorithm but still the two motors don’t turn at the same speed with the same PWM , the new code in the attachment I’d appreciate it if someone could look over it and check for mistakes or ways I can improve it.

New Code:

#include <RobotParams.h>
#include <OdometricLocalizer.h>
#include "Arduino.h"
#include <digitalWriteFast.h>  
#define PI 3.14159265 
#define TwoPI 6.28318531

RobotParams _RobotParams = RobotParams();       // Class to Initialize robot parameters :  wheelDiameter , countsPerRevolution , trackWidth 
OdometricLocalizer _OdometricLocalizer(&_RobotParams);   // Class to compute X ,Y , Angle of robot 

int ENA=8;    // SpeedPinA  
int ENB=9;    // SpeedPinB 

int IN1=48;    // RightMotor1
int IN2=49;    // RightMotor2
 
int IN3=50;    // LeftMotor1
int IN4=51;    // LeftMotor2

float P= 0.2;

long difference ;

// Quadrature encoders
// Left encoder
#define encoder0PinA 18  // interrupt 0

#define encoder0PinB 2  // interrupt 5

volatile signed int encoder0Pos = 0;



#define encoder1PinA 3  // interrupt 1

#define encoder1PinB 19  // interrupt 4

volatile signed int encoder1Pos = 0;



int RightWheel,LeftWheel;
int  Difference ;  
        
bool _IsInitialized = false;


void setup() {


Serial.begin(115200);

//Motors Pins
 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
 

 //Encoders Pins
 pinMode(encoder0PinA, INPUT); 
 pinMode(encoder0PinB, INPUT); 
// encoder pin on interrupt 0 (pin 2)

 attachInterrupt(5, doEncoderA, CHANGE);
// encoder pin on interrupt 5 (pin 3)

  attachInterrupt(0, doEncoderB, CHANGE);  
  
pinMode(encoder1PinA, INPUT); 
pinMode(encoder1PinB, INPUT); 
// encoder pin on interrupt 0 (pin 2)

 attachInterrupt(1, doEncoderA1, CHANGE);
// encoder pin on interrupt 5 (pin 3)

attachInterrupt(4, doEncoderB1, CHANGE); 
  
  
 InitializeDriveGeometry();
 RequestInitialization();

}



void loop() 
{


doEncoderA();
doEncoderB();
doEncoderA1();
doEncoderB1();


 _OdometricLocalizer.Update(encoder0Pos, encoder1Pos);     // Passing Encoders Counts to get X , Y , Heading
 
   float P= 2;
long difference =long (encoder1Pos - encoder0Pos);
if (difference > 0)
{LeftWheel -= int(P * difference);
RightWheel += int(P * difference);}
else
{LeftWheel += int(P * difference);
RightWheel -= int(P * difference);}

 moveForward();

}


void moveForward() 
{

    LeftWheel=20;
    RightWheel=20;

 //  Converting To PWM
   toPWM (RightWheel,LeftWheel);
   
   
}


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) {  
      encoder0Pos = encoder0Pos + 1;         // CW
    } 
    else {
      encoder0Pos = encoder0Pos - 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) {   
      encoder0Pos = encoder0Pos + 1;          // CW
    } 
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }
 // Serial.println (encoder0Pos, DEC);          
  // use for debugging - remember to comment out
}

void doEncoderB(){

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


void doEncoderA1(){

  // look for a low-to-high on channel A
  if (digitalRead(encoder1PinA) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder1PinB) == LOW) {  
      encoder1Pos = encoder1Pos + 1;         // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 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(encoder1PinB) == HIGH) {   
      encoder1Pos = encoder1Pos + 1;          // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }
//  Serial.println (encoder1Pos, DEC);          
  // use for debugging - remember to comment out
}

void doEncoderB1(){

  // 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) {  
      encoder1Pos = encoder1Pos + 1;         // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 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) {   
      encoder1Pos = encoder1Pos + 1;          // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }
}





void RequestInitialization()
{
    _IsInitialized = true;

    if (!_RobotParams.IsInitialized)
    {
      _IsInitialized = false;


}}

void InitializeDriveGeometry()
{
  float wheelDiameter =6.5 ; // in CM
  float trackWidth =18.9; // in CM
  int countsPerRevolution =333.3; //
  
  _RobotParams.Initialize(wheelDiameter, trackWidth, countsPerRevolution);
}







// Coverting to PWM

void toPWM (float Wr,float Wl)
{
     RightWheel=Wr;
     LeftWheel=Wl;
       //Serial.println( "PMW");
    int rightPWM, leftPWM;  
  if (RightWheel > 0) {
    //forward
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
    
  }  else if (RightWheel < 0){
    //reverse
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  }
  
  if (RightWheel == 0) {
   rightPWM = 0;
   analogWrite(ENA, rightPWM);
  } else {
    rightPWM = map(abs(RightWheel), 1, 100, 1, 255);

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

    analogWrite(ENB, leftPWM);
  }
  
  
}

Have a long hard look at this code and work out why its completely wrong!!:

    long difference =long (encoder1Pos - encoder0Pos);
  if (difference > 0)
  { 
    LeftWheel -= int(P * difference);
    RightWheel += int(P * difference);
  }
  else
  {
    LeftWheel += int(P * difference);
    RightWheel -= int(P * difference);
  }

MarkT:
Have a long hard look at this code and work out why its completely wrong!!:

    long difference =long (encoder1Pos - encoder0Pos);

if (difference > 0)
  {
    LeftWheel -= int(P * difference);
    RightWheel += int(P * difference);
  }
  else
  {
    LeftWheel += int(P * difference);
    RightWheel -= int(P * difference);
  }

Thanks but what is the wrong thing is ?