# Using PID to ensure two wheel robot goes in straight line

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 wrote a PID formula to correct error. I believe I wrote it correctly but 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 K values which scale the P, I and D and they seem to make a difference but the bot still runs crooked. I'm not sure if I just need to keep adjusting the values or if I have a mistake. My code is below and might be confusing so if you need clarification I'll try and quickly reply. Thanks for your time.

``````//============================================ IO Pins ============================================
// Pins used on Arduino Uno
#define rEncoder 2
#define lEncoder 3

#define lpwm 6
#define ldir1 4
#define ldir2 7

#define rpwm 5
#define rdir1 8
#define rdir2 9

//======================================== Global Variables =======================================
// Variables used in interrupts
volatile unsigned long rightWheel;

volatile unsigned long leftWheel;

// Left and Right Initial start speeds
byte rightSpeed = 225;
byte leftSpeed = 225;

// PID Values
double input, output, leftLastError, poportional, derivative, rightLastError;
double rightIntegral = 0;
double leftIntegral = 0;

// pidcount is used to divide the total error (integral formula)
int pidcount = 1;

// PID Multipliers
double kp = 2;
double ki = 0.3;
double kd = 0.5;

// The setpoint is used in the PID equation
double setPoint = 30;

//============================================= Setup =============================================
void setup(){

Serial.begin(9600);

pinMode(rEncoder, INPUT);
pinMode(lEncoder, INPUT);

pinMode(rpwm, OUTPUT);
pinMode(rdir1, OUTPUT);
pinMode(rdir2, OUTPUT);

pinMode(lpwm, OUTPUT);
pinMode(ldir1, OUTPUT);
pinMode(ldir2, OUTPUT);

// Enable the pull up resistors for the encoders
digitalWrite(rEncoder, HIGH);
digitalWrite(lEncoder, HIGH);
// Enable interrupts 0 and 1 on pins D2 and D3
attachInterrupt(0, rightEncoderISR, FALLING);
attachInterrupt(1, leftEncoderISR, FALLING);

digitalWrite(ldir1, LOW);
digitalWrite(ldir2, HIGH);

digitalWrite(rdir1, LOW);
digitalWrite(rdir2, HIGH);

}// End Setup

//============================================= Loop ==============================================
void loop(){

//========== Right ==========
// Move encoder count to input and reset to 0
input = rightWheel;
rightWheel = 0;

// Calculate the PID values
poportional = setPoint - input;
derivative = poportional - rightLastError;
rightIntegral = (rightIntegral + poportional)/pidcount;

// Scale the PID values and save total as output
output = kp * poportional + kd * derivative + ki * rightIntegral;

// Save variables for next time
rightLastError = poportional;

// Add fanal value to speed only if value is lower then 255
if((rightSpeed + output) > 255) rightSpeed = 255;
else rightSpeed = output + rightSpeed;

// Finally, set the updated value as new speed
analogWrite(rpwm, rightSpeed);

//========== Left ==========
// Move encoder count to input and reset to 0
input = leftWheel;
leftWheel = 0;

// Calculate the PID values
poportional = setPoint - input;
derivative = poportional - leftLastError;
leftIntegral = (leftIntegral + poportional)/pidcount;

// Scale the PID values and save total as output
output = kp * poportional + kd * derivative + ki * leftIntegral;

// Save variables for next time
leftLastError = poportional;
pidcount++;

// Add fanal value to speed only if value is lower then 255
if((leftSpeed + output) > 255) leftSpeed = 255;
else leftSpeed = output + leftSpeed;

// Finally, set the updated value as new speed
analogWrite(lpwm, leftSpeed);

delay(100);

} // End Loop

//============================================== PID ==============================================

// Space to add whats in the loop later on

//========================================== Encoder ISRs =========================================
// Interrupts
void rightEncoderISR(){
rightWheel++;
}

void leftEncoderISR(){
leftWheel++;
}
//=================================================================================================
``````

Parts used on robot:

The PID algorithm looks vaguely OK, but how do you know it is correctly tuned?

It is very important to print out intermediate values so you see that the calculations make sense, in particular that the corrections made by the algorithm are even in the right direction. Having the wrong sign for a correction is a common problem. And, if both motors max out at 255, you will never see any corrective action.

There are many tutorials on the web regarding PID tuning. Start with just the proportional term and add the others as necessary.

Even if both wheels turn the exact same angle you robot will not run straight. The only way to achieve that is to measure heading or position

Have you tried the simple fix of just finding a fraction, which when applied to the speed setting of the faster wheel, makes the robot run straight? This trick can work over a fairly wide range of speeds.

jremington: The PID algorithm looks vaguely OK, but how do you know it is correctly tuned?

It is very important to print out intermediate values so you see that the calculations make sense, in particular that the corrections made by the algorithm are even in the right direction. Having the wrong sign for a correction is a common problem. And, if both motors max out at 255, you will never see any corrective action.

There are many tutorials on the web regarding PID tuning. Start with just the proportional term and add the others as necessary.

When I coded the PID I did just what you described, starting with the proportional part, testing and then adding on to it. I often printed to the serial to make sure values made since. You briefly mentioned about how having the wrong sign for a correction is a common problem, could you elaborate on that. in addition, i'm also not sure if I did the integral part correctly because really i'm just taking the average of all the error values.

nilton61: Even if both wheels turn the exact same angle you robot will not run straight. The only way to achieve that is to measure heading or position

I completely agree with you but reading encoders should get the bot to go straight within at least a few degrees variation which is all I need right now. Thanks for your suggestion.

jremington: Have you tried the simple fix of just finding a fraction, which when applied to the speed setting of the faster wheel, makes the robot run straight? This trick can work over a fairly wide range of speeds.

If I understand you correctly, you are talking about taking the final PID output for the wheel that is going faster and multiplying it by a fraction so that it more closely matches the other wheel? I considered doing this as a last resort but I wasn't sure if having a fraction as a fixed value would resolve the problem. How would I go about finding the value, just trial and error?

A optical mouse does measure local heading against almost any surface. It should be possible to hack one and mount the sensor underneath you robot. I think that be give you better results than measuring wheel angle

If you tuned the PID algorithm and it still isn’t working, there is a serious error somewhere. Without print statements, you (and we) have no chance of finding it.

you are talking about taking the final PID output for the wheel that is going faster and multiplying it by a fraction so that it more closely matches the other wheel?

No, I was suggesting to experiment with not using PID at all. The main routine sets the speeds of both motors to be the same, but a fraction is applied to the faster one. You find that fraction by trial and error.

But, as nilton61 suggested above, the best use of PID is to follow a defined bearing, which will take care of the problem automatically. For that you will need at least an electronic compass, or better, an IMU/AHRS like this one (currently on closeout sale!): Pololu - AltIMU-10 v3 Gyro, Accelerometer, Compass, and Altimeter (L3GD20H, LSM303D, and LPS331AP Carrier)

you've sold me on buying that IMU/AHRS, I'll be ordering one soon but what is the difference between the v3 and the newer v4. As far as print statements, I went ahead and Serial printed the values for the right wheel and copied the data into the excel sheet. I only tested for a short period of time but the values just seemed to oscillate. I can add the left wheel data if it is needed but I assume it would behave the same way. Hopefully the values seem reasonable as they do to me. My setPoint is set at 30, kp = 6, ki = 2, kd = 2. Everything else is the same as the code originally posted.

Without seeing the code (with print statements) responsible for those spreadsheet values, they don't make much sense to me. In general, if the output of the PID algorithm or the so-called "process value" is oscillating, the PID constants are not correct.

When tuning a pid this procedure works often: