The most important element of any robot is the controller. Especially for a self-balancing robot, the control program is vital as it interprets the sensor data and decides how much the motors need to be moved in order for the robot to remain upright. The most common controller used for stabilisation systems is the PID controller. So let’s look at how it works:

## The PID Controller

PID stands for proportional, integral and derivative, referring to the mathematical equations used to calculate the output.

The P-component simply takes in the current angle of the robot and makes the motors move in the same direction as the robot is falling. Therefore the further the robot falls off target, the faster the motors move. If the P-component is used on its own, the robot might stabilise for a while, but the system will tend to overshoot, oscillate and ultimately fall over.

The I-component is used to accumulate any errors. For example if the robot tends to fall over to one side, it knows that it needs to move in the opposite direction in order to keep the on target and to prevent drifting left or right.

Finally, the D-component is responsible for dampening any oscillations and ensures that the robot does not vibrate too much. It simply acts against any movement.

## Implementing the Controller

Now that we know the basic theory, we can start to write this in code. Here is the basic layout:

// Declare variables
float Kp = 7;          // (P)roportional Tuning Parameter
float Ki = 6;          // (I)ntegral Tuning Parameter
float Kd = 3;          // (D)erivative Tuning Parameter
float iTerm;           // Used to accumulate error (integral)
float lastTime = 0;    // Records the last time function was called
float maxPID = 255;    // The maximum value that can be output

// ================================================================
// ===                   PID CONTROLLER                         ===
// ================================================================
float PID(float target, float current) {
// Calculate the time since function was last called
float thisTime = millis();
float dT = thisTime - lastTime;
lastTime = thisTime;

// Calculate error between target and current values
float error = target - current;
float pid = 0;

// Calculate the integral and derivative terms
iTerm += error * dT;
float dTerm = (current - oldCurrent) / dT;

// Set old variable to equal new ones
oldCurrent = current;

// Multiply each term by its constant, and add it all up
pid = (error * Kp) + (iTerm * Ki) - (dTerm * Kd);

// Limit PID value to maximum values
if (maxPID > 0 && pid > maxPID) pid = maxPID;
else if (maxPID > 0 && pid < -maxPID) pid = -maxPID;

return pid;
}

#### What does this program do?

First of all, the algorithm calculates the time since the last loop was called, using the “millis()” function. The error is then calculated; this is the difference between the current value (the angle recorded by the sensor), and the target value (the angle of 0 degrees we are aiming to reach).

The PID values are then calculated and summed up to give an output for the motors. The derivative term is subtracted from the sum as it is meant to dampen any movements. The output is then restrained to ±255 as this is the maximum PWM value that can be output to the motors.

Although this program is almost complete, I found that my robot only worked well once I included a timing function. This is a system that ensures the PID controller function is called at regular intervals. In my self-balancing robot, I set the loop time to be 10ms (meaning 100 cycles per second). Here is the timer code and a sample loop function:

// Any variables for the PID controller go here!
float target = 0;

// Variables for Time Keeper function:
#define STD_LOOP_TIME 10
unsigned long nextTime = 0;

/******** SETUP ************/
void setup() {
// Put all of your setup code here
}

/******* MAIN LOOP *********/
void loop() {
// Only run the controller once the time interval has passed
if (nextTime < millis()) {
nextTime = millis() + STD_LOOP_TIME;
angle = getAngle();
motorOutput = PID(target, angle);
moveMotors(motorOutput);
}
}

/****** PID CONTROLLER *****/
float PID(float target, float current) {

// PID code from above goes in here!
return pid;
}

Unfortunately this is not the end of the story! Although the PID controller code is complete, suitable PID constants still need to be for your own robot. These constants depend on things such as weight, motor speed and the shape of the robot, and therefore they can vary significantly from robot to robot. Here is a quick explanation of how you should go about calibrating your PID values:

1. Create some way in which you can change the PID constant of your robot while it is running. One option is to use a potentiometer or some other analogue input to be able to increase or decrease the PID constant. I personally used the USB connection and the serial monitor to send new PID values. This is important as you can then see straight away how well the new PID values are working, and you won’t have to re-upload the code hundreds of times!
2. Set all PID constants to zero. This is as good a place to start as any…
3. Slowly increase the P-constant value. While you are doing this, hold the robot to make sure it doesn’t fall over and smash into a million pieces! You should increase the P-constant until the robot responds quickly to any tilting, and then just makes the robot overshoot in the other direction.
4. Now increase the I-constant. This component is a bit tricky to get right. You should keep this relatively low, as it can accumulate errors very quickly. In theory, the robot should be able to stabilise with only the P and I constants set, but will oscillate a lot and ultimately fall over.
5. Raise the D-constant. A lot. The derivative components works against any motion, so it helps to dampen any oscillations and reduce overshooting. I found that this constant has to be set significantly higher than the other two (x10 to x100 more) in order to have any effect. All the same, don’t set it too high, as it will reduce the robot’s ability to react to external forces (aka. being pushed around).
6. Spend many fruitless hours slightly modifying the PID values. This is probably the longest part of the procedure, as there isn’t much of a method to it. You just have to increase and decrease the values until you reach that perfect sweet-spot for your robot!

Updated: 23rd May 2019 – Reformatted post and improved the code snippets.

Guest
dilshan fernando

nice tutorial to beginners .thank you very much .you are good teacher

Guest
AJAY

hello, I acquired sensor values from mpu 6050 …but how will give it to a pid controller in labview

Guest
Ramius

Hi. Wonderful project and nice and clear guiding. Looking through the code I couldn’t find where the move and rotate commands are handled. Other question is what about encoders? Do you think they would really help?

Regards

Guest
Vivek V

Hello Simon,
I got to the part which says ‘Target Angle Set’ and ‘Initializing’. I added
Serial.println(ypr[1] * 180/M_PI);
just before
PID(ypr[1] * 180/M_PI, ypr[0] * 180/M_PI);

However on the serial, I just obtain a single value and the motors begin to rotate continuously in one direction, without stopping. I guess the sensor stops transmitting values to the controller once the motor begins to move (Values not updated, hence motor rotates continuously?)

Any help for this? Also please do check out the comment I sent via the About page.

Guest
Vivek V

Hello Simon,
Your tutorial has been wonderful. Could you post the final bit too? I am stuck at the last part; the optimization of the PID factors as well as debugging using the IDE.

Guest
Paul

Hi your tutorial is fantastic, thanks for putting it out there for us to try and build!
Any news when part 5 will be available?

Guest
sarina

Hi!!