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:

- Sparkfun Magician Chassis (motor gear ratio: 48:1)
- 4 Alkaline Batteries
- Arduino Uno
- L293d (for motor direction)
- DAGU Wheel Encoder Kit