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);
}
}