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:

What does this program do?

First of all, the function 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 angle, and the angle we are aiming to reach (0 degrees).

The PID values are then calculated and summed up to give an output for the motors. The derivative 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. The output is converted into an integer and returned.

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:

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 straightaway 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!

In Part 5: Putting it all together View Part 5

Please leave a comment below if you have any questions or suggestions. You could consider subscribing if you want to be notified the moment I post the next part of this series!

### This Post Has 12 Comments

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

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

3. 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

1. Hi Ramius,
The code shown is not complete; I’ve only written it to show how the PID controller can be used. A full version of my self-balancing code can be found in the final part of my tutorial here. Yes, encoders would definitely improve the robot’s balancing performance. There are some other tutorials online where they describe how to integrate encoders into the PID code.
Simon

4. 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.

5. 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.

1. Hi Vivek! Calibrating the PID values definitely is the hardest part of the project. If you really are having no luck with the system, I would suggest checking that the sensor is working properly, and that the time between each loop is short (less than 10ms) and is relatively regular.

2. The sensor is working alright and the loop looks fine too. My motors don’t respond according to the balance; they don’t switch on at all even after I get the ‘Ready to Balance’ on the monitor. How do I debug with the monitor? I am unable to set values via the serial monitor.

What changes should I be making to the code?

PS I am using the MPU6050 along with an interrupt on an Arduino Mega.

3. I assume you’re using the balancing program I posted a while back? If it doesn’t say “Initializing” after you see “Ready to Balance!”, and you are holding the robot vertical and still, then there is something wrong with the sensor. Here are a number of pointers that might help:

• Have you updated the Gyro & Accel offsets in the setup function with your own values?
• Is the sensor module facing the right direction? Check this by putting the line Serial.println(ypr[1] * 180/M_PI); just before the PID function is called in the main loop.
• My code does not use interrupts, as I used a Galileo Gen2 board instead of an Arduino Mega. All the same, this code should still work for you.
• To send a new P-value for the PID controller over serial, type: “P” and then a new number between 1-50. The same can be done for the “I” and the “D” values.
6. 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?

7. Hi!!
Thanks

1. Hi Sarina!
I am just finishing off the final part of the self-balancing robot tutorial, so I hopefully will get that posted by the end of the week. I’ve also been working on my own 3D printer this summer, and I intend to write some posts about that soon.