• Post Comments:12 Comments

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.

Diagram of PID controller being used on a self-balancing robot.

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;

	// 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
	float 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 constrained 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 the function is run 100 times 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);

/****** 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 found to tune the controller for your specific 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:

Calibrating your PID Controller

  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!

Please leave a comment below if you have any questions or suggestions.

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

Oldest Most Voted
Inline Feedbacks
View all comments