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: