So I wanted to LEARN how PID control system work. Really learn and understand what is going on. So I watched THIS video and a few other like it, I read some articles, online tutorials, read the wikipedia and tried my best to understand the math. (I'm not great with this stuff). My goal was to code my own PID control system from nothing.
My test bench was a servo with a breadboard glued to the moving arm of the servo and the BNO055 sensor plugged into that breadboard. If my PID program can keep the sensor pointed at 90 degrees while I move the test bench around with my hand then I have successfully made a functional PID program. I began with P and then I and finally D. I finished that code and while tuning was a long annoying process of trial and error my test bench could use the servo to keep the BNO055 pointed at 90 degrees no matter what direction I moved it.
I want to move forward with my PID program. I want to test in in a scenario where I can really see it's effectiveness. My plan is to make a small car that will point at whatever angle I tell it to. If I give it a light tap and it moves off course it MUST correct for the error and get back on track.
The reason I am making this post is because I'm not sure how to use a single PID output to control 2 motors. Sure I can do something like this.
pidOutput = map(pidOutput, -150, 150, 0, 255);
I can just make the output the PWM of the motor but if I am using 2 motors wont that mean that BOTH motors will be doing the exact same thing? It wont be able to steer at all.
I am here to ask how can I convert the output of a PID system to something that can steer the car if my only way to steer the car is one motor spinning faster or slower than another motor.
Here is my PID code I used for the servo test stand. If anyone has anything to critique I'd glady take the advice. I've been doing this kind of robotics on and off for a few years now. Still learning.
//Add libraries required for certain tasks
#include <Wire.h> //allows us to use I2c communication (in this case for comminication with BNO055 sensor)
#include <Servo.h> //allows easy communication with servos
#include <Adafruit_Sensor.h> //required for BNO055 sensor
#include <Adafruit_BNO055.h> //required for BNO055 sensor
#include <utility/imumaths.h> //required for BNO055 sensor
Adafruit_BNO055 bno = Adafruit_BNO055(55); //i2c address for BNO055
Servo servo; //declare a servo and give it a name (servo)
float desiredAngle = 90; //The value (in degrees) we are trying to reach with our PID control system
float currentAngle; //The current value of our current degrees
float errorAmount; //The difference between our current degrees and our desired degrees (zero)
float proportionalGain = 0.004; //Gain term for the P in PID, adjust this to fine tune the system
float pidOutput; //Output of the proportional
float integralGain = 0.2; //Gain term for the I in PID, adjust this to fine tune the system
float errorSum; //Accumulate the errors over time to add to the integral
float derivativeGain = 1.3; //Gain term for the D in PID, adjust this to fine tune the system
float lastError = 0; //Subtract this from current error to get errorChange;
float errorChange; //Multply this by the Dgain for the D output
//Setup, everything in this bracket will happen once at the start before the main loop begins
void setup(void)
{
servo.attach(3); //Our servo's signal cable is plugged into pin 3 on the microcontroller
servo.write(180);//Reset Servo and calibrate position of degrees for BNO055 IMU
delay(1000); //Wait 1 second
Serial.begin(9600); //Initiate Serial Communication (9600 baud rate)
//Check to see if everything is okay with the BNO055 sensor
if(!bno.begin())
{
Serial.print("BNO055 error");
while(1);
}
bno.setExtCrystalUse(true); //required for BNO055 sensor
Serial.println("3"); delay(250); Serial.println("2"); delay(250); Serial.println("1"); delay(250); //Lets me know program is running when I open Serial Monitor
}
//Main loop function
void loop()
{
proportionalOutput(); //Do the function written below
Serial.print("PID Output: "); Serial.print(pidOutput); Serial.print(" Current Angle: "); Serial.print(currentAngle); //print out in serial, useful for troubleshooting
pidOutput = map(pidOutput, 165, -165, 0, 180); //convert the PID signal to degrees from 0 to 180 (the amount the servo can travel in degrees)
Serial.print(" mapped output: ");Serial.print(pidOutput); //more data print to serial, again as said before useful for troubleshooting
Serial.print(" error amount: "); Serial.println(errorAmount); //more data print to serial, again as said before useful for troubleshooting
servo.write(pidOutput);//move the servo with the signal from the converted pid output
}
//Pid loop
float proportionalOutput()
{
//Get sensor event
sensors_event_t event;
bno.getEvent(&event);
currentAngle = (event.orientation.x); //Read the sensor value for X and save it to variable currentAngle
//IMU goes from 0-360, however nothing says 0 and 360 can't be 1 degree apart, this confuses the program, so convert the 2nd half passed 180 back to 179 and below
if(currentAngle > 180)
{
currentAngle = (360 - currentAngle);
}
//Proportional Calculation
errorAmount = desiredAngle - currentAngle;
pidOutput = errorAmount * proportionalGain;
//Integral Calculation
errorSum += errorAmount;
pidOutput += errorSum * integralGain;
//Derivative Calculation
errorChange = errorAmount - lastError;
pidOutput += errorChange * derivativeGain;
lastError = errorAmount;
}
//Gain values need to be tuned further, pain in the @ss
EDIT:
Its been around 1 day now and no replies and since then I have finished the car and began working on making my PID program work for the car.
Few things. First, while my OLD code (above) worked for fine for the servo test stand I built it seems to be broken now. If my integral gain is anything above 0 the error builds up over and over forever. My pidOutput goes way out of control. I haven't made any changes to it all so I have no idea what is going on. I'll revisit that later. For now my goal has been to get the CAR to work with the PID system with only having the P gain turned on, the I and D left at zero.
The way I have tried to solve my previous question (how to get a PID output to control 2 motors to turn a car) is to add the PID output to the speed of the wheel. For example. If the car is to the right of my desired angle, right motor's speed value will have the PID output added to it's sum. If the car is to the left of my desired angle the left motor's speed value will have the PID output added to its sum.
At the same time the left motor speed will = the right motor speed - pidOutput OR the right motor speed will = the left motor speed - pidOutput. It just depends on what side of my desired angle it is at.
Sadly right now the car just spins in circle haha
Here is the code.
//Add libraries required for certain tasks
#include <Wire.h> //allows us to use I2c communication (in this case for comminication with BNO055 sensor)
#include <Adafruit_Sensor.h> //required for BNO055 sensor
#include <Adafruit_BNO055.h> //required for BNO055 sensor
#include <utility/imumaths.h> //required for BNO055 sensor
Adafruit_BNO055 bno = Adafruit_BNO055(55); //i2c address for BNO055
float desiredAngle = 0; //The value (in degrees) we are trying to reach with our PID control system
float currentAngle; //The current value of our current degrees
float errorAmount; //The difference between our current degrees and our desired degrees (zero)
float proportionalGain = 1.4; //Gain term for the P in PID, adjust this to fine tune the system
float pidOutput; //Output of the proportional
float integralGain = 0; //Gain term for the I in PID, adjust this to fine tune the system
float errorSum; //Accumulate the errors over time to add to the integral
float derivativeGain = 0; //Gain term for the D in PID, adjust this to fine tune the system
float lastError = 0; //Subtract this from current error to get errorChange;
float errorChange; //Multply this by the Dgain for the D output
const int leftForward = 5;
const int leftBackward = 4;
const int rightForward = 3;
const int rightBackward = 2;
const int speedRightInput = 6;
const int speedLeftInput = 7;
int speedRight = 150;
int speedLeft = 150;
//Setup, everything in this bracket will happen once at the start before the main loop begins
void setup(void)
{
Serial.begin(9600); //Initiate Serial Communication (9600 baud rate)
//Check to see if everything is okay with the BNO055 sensor
if(!bno.begin())
{
Serial.print("BNO055 error");
while(1);
}
bno.setExtCrystalUse(true); //required for BNO055 sensor
pinMode(leftForward, OUTPUT);
pinMode(leftBackward, OUTPUT);
pinMode(rightForward, OUTPUT);
pinMode(rightBackward, OUTPUT);
pinMode(speedRightInput, OUTPUT);
pinMode(speedLeftInput, OUTPUT);
digitalWrite(speedRightInput, HIGH);
digitalWrite(speedLeftInput, HIGH);
Serial.println("3"); delay(250); Serial.println("2"); delay(250); Serial.println("1"); delay(250); //Lets me know program is running when I open Serial Monitor
}
//Main loop function
void loop()
{
pidLoop(); //execute function
//if we're to the right of 0 then add the PID value to the speed of right motor and keep left motor at steady speed
if(currentAngle > 0)
{
speedRight = 150; //make sure the number doesn't blow up into something in the thousands, reset it every loop
speedRight += pidOutput;
analogWrite(speedRightInput, speedRight);
speedLeft = speedRight - pidOutput;
}
//if we're to the left of 0 then add the PID value to the speed of left motor and keep right motor at steady speed
if(currentAngle < 0)
{
speedLeft = 150; //make sure the number doesn't blow up into something in the thousands, reset it every loop
speedLeft += pidOutput;
analogWrite(speedLeftInput, speedLeft);
speedRight = speedLeft - pidOutput;
}
printData();
forward();
delay(10);
}
//Pid loop
float pidLoop()
{
//Get sensor event
sensors_event_t event;
bno.getEvent(&event);
currentAngle = (event.orientation.x); //Read the sensor value for X and save it to variable currentAngle
//IMU goes from 0-360, however nothing says 0 and 360 can't be 1 degree apart, this confuses the program, so convert the 2nd half passed 180 back to 179 and below
if(currentAngle > 180)
{
currentAngle = (360 - currentAngle);
currentAngle = -currentAngle; //so instead of 3-2-1-0-1-2-3, we get -3, -2, -1, 0, 1, 2, 3, if you know what i mean, makes it easy to know if im to the right or to the left of 0
}
//Proportional Calculation
errorAmount = desiredAngle - currentAngle;
pidOutput = errorAmount * proportionalGain;
//Integral Calculation
errorSum += errorAmount;
pidOutput += errorSum * integralGain;
//Derivative Calculation
errorChange = errorAmount - lastError;
pidOutput += errorChange * derivativeGain;
lastError = errorAmount;
pidOutput = map(pidOutput, 45, -45, 0, 30); //convert the PID signal to small number that can be added to motor speed right or left depending on what side of angle 0 we are
}
void forward()
{
digitalWrite(leftForward, HIGH);
digitalWrite(leftBackward, LOW);
digitalWrite(rightForward, HIGH);
digitalWrite(rightBackward, LOW);
}
//Serial Print Data
void printData()
{
Serial.print("PID Output: "); Serial.print(pidOutput); Serial.print(" Current Angle: "); Serial.print(currentAngle); //print out in serial, useful for troubleshooting
Serial.print(" mapped output: ");Serial.print(pidOutput); //more data print to serial, again as said before useful for troubleshooting
Serial.print(" error amount: "); Serial.print(errorAmount); //more data print to serial, again as said before useful for troubleshooting
Serial.print(" speed RIGHT: "); Serial.print(speedRight);
Serial.print(" speed LEFT: "); Serial.println(speedLeft);
}
Of course I was able to record some of the data from my serial monitor while testing. Here (below) is currently what the car does the moment I turn it on. (shown with numbers) Image came out small, link to imgur: http://imgur.com/a/Phm2S
Here (below) is another image from my serial monitor that is running when the car is stationary and I'm just turning it around with my hand and reading sensor/pidOutput data. The image seems small, link to imgur: http://imgur.com/a/MtyDl
I wanted to see what the car thinks is happening when I am moving it with my hand. This screenshot makes sense to me. My desired angle is 0. Therefore the error is 0. My base speed for the car is 150, and so since it is at zero the car is moving (not actually moving remember, its stationary, this is basically just a simulation) straight, both motors same speed.
As the angle begins to go off course to the RIGHT of the desired angle the PID tries to compensate and as you can see the right motor speeds up to try to conpensate for the angle not being 0. So WHY in REAL application when the motors are spinning and I put it on the ground I get the result of the FIRST screenshot is beyond my understanding. If anyone is able to see what I have missed or maybe something I have forgotten that would be really great help.
Thanks.