# 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 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

// 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);
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);
ll = 0;
rr = 0;}}

}

void doEncoderA(){

// look for a low-to-high on channel A
// check channel B to see which way encoder is turning
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
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
// check channel A to see which way encoder is turning
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
encoder0Pos = encoder0Pos + 1;          // CW
}
else {
encoder0Pos = encoder0Pos - 1;          // CCW
}
}
}

void doEncoderA1(){

// look for a low-to-high on channel A
// check channel B to see which way encoder is turning
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
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
// check channel A to see which way encoder is turning
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
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 ;

// 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
// check channel B to see which way encoder is turning
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
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
// check channel A to see which way encoder is turning
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
encoder0Pos = encoder0Pos + 1;          // CW
}
else {
encoder0Pos = encoder0Pos - 1;          // CCW
}
}
}

void doEncoderA1(){

// look for a low-to-high on channel A
// check channel B to see which way encoder is turning
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
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
// check channel A to see which way encoder is turning
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
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 ?