I’ll try to explain the algorithm I’ve decided to try for balancing my robot. As I understand it, this type of algorithm is called a PID controller. An extremely simplistic self balancing robot algorithm would do the following:

If the robot is tilting forwards, move the wheels in the forward direction.

Else, if the robot is tilting backwards, move the wheels in the backward direction.

Else do nothing.

Repeat.

Now, the PID controller is a little more advanced – being named “proportional-integral-derivative” controller. The PID controller takes into account the amount of tilt and applies a proportionally large amount of force on the wheels. The algorithm takes three arguments; present error, sum of accumulated errors and expected future error.

Proportional – present error

The present error is the amount of tilt that the robot has. It is supposed to have a tilt of 0°. If it is tiled by 10°, the error is -10.

Integral – sum of accumulated errors

The sum of errors accumulated. For the self balancing robot, this value is zero.

Derivative – expected future error

The error we expect to have on the next iteration of this algorithm. The current tilt may be 10°, and we may be expecting it to become 11° on the next iteration. The algorithm takes this into account when it decides how much force to apply on the wheels.

PD controller algorithm

Since we’re not taking the integral into account, our algorithm is actually called a PD-controller algorithm. It is a loop where we take a measure, and compare it with what we want. We then apply an operation then repeat.

We need to take care, so that the responses our robot does is not too strong. This is called overshooting. However, if the response is too weak, the robot will spend too much time before it is positioned correctly – and this leads to it moving too much on the floor. Ideally, we want the robot to move as little as possible on the floor, while at the same time not oscillating due to overshooting.

I want the PD controller algorithm to have the following interface:

float pd(float currentError, float nextError);

A very simple implementation of this function could probably be

return -(currentError * 0.5 + nextError * 0.5);

Basically, it returns the negative average of the two arguments. This will work, but it has a bunch of problems. The most obvious one is the fact that if the controller is currently at -10°, and the rate of change makes us expect it to be at +10° on the next iteration – the pd() function will return 0. It should actually try to prevent overshoot. A more advanced function is required. What about:

return -(currentError * 0.25 + nextError * 0.75);

This function emphasizes the next error more than it emphasizes the current error. Applying it to the previous example would yield a result of -2.5 – so this should actually work.

It turns out this is actually how the PD controller algorithm works. You tune the emphasis on the current error, and the next error. A PID controller algorithm is quite identical:

return -(currentError * 0.3 + accumulatedError * 0.2 + derivative * 0.5);

The variables in the previous example are called tuning parameters and their names is usually Kp, Ki and Kd. A good generic PID-controller function looks like this:

float pid(error, integral, derivative, Kp, Ki, Kd) { return -(error * Kp + integral * Ki + derivative * Kd); }

In the control loop for frobot, I’ll use it like this:

void loop() { float expectedTilt = 0; float currentTilt = getCurrentTilt(); // From accelerometer float currentTiltRate = getCurrentTiltRate(); // From gyro float thrust = pid(currentTilt - expectedTilt, 0, currentTilt + currentTiltRate, 0.25, 0, 0.75); // Apply thrust to each motor }

I’m looking forward to trying this untested code. You should look at it as pseudo code at the moment. 🙂