Build a self-levelling platform that keeps steady like a drone

A drone has no wings to keep it steady. If you switched off its computer, it would flip over in less than a second. What keeps it level is its flight controller: a small board with a motion sensor that measures the drone's angle hundreds of times a second and changes the speed of the four motors to correct every wobble. The heart of that controller is a short piece of maths called a PID controller. Camera gimbals, self-balancing scooters, rockets and robot arms use the same idea. In this capstone you build a one-axis version: an MPU-6050 sits on a small platform on top of a servo. Tilt the base, and the servo tilts the platform back until it is level again. You can change the controller's settings while it runs and see what each one does.

What you'll learn

• How a closed control loop works: measure, compare, correct, repeat
• What the P, I and D parts of a PID controller each do, and how to tune them
• How to combine the gyroscope and the accelerometer into one steady angle
• How a drone's flight controller uses the same loop on three axes and four motors

What you need

• UNO R3 board and USB cable (UNO R3 Starter Kit)
• GY-521 MPU-6050 module (45-in-1 Sensor Kit)
• Servo motor (UNO R3 Starter Kit)
• A small platform: a 10 cm piece of ruler, an ice-lolly stick or stiff cardboard
• Strong double-sided tape or hot glue
• Long female-to-male jumper wires for the sensor
• Breadboard and jumper wires (UNO R3 Starter Kit)

Build the platform

1. Fix the servo to a small box or a thick book with tape, its shaft pointing sideways (horizontal).
2. Press the long arm (horn) onto the servo shaft. Glue or tape the platform across the arm, so the servo can tilt it up and down like a see-saw.
3. Stick the MPU-6050 flat on the platform, with the chip facing up and its X arrow pointing along the servo shaft. The sensor must move with the platform, not with the base.
4. Run long jumper wires from the sensor to the breadboard so the platform can move freely.

Wiring

Build it step by step

Plug in all the parts first, then add the wires in this order. In each picture the new wires are bright and the ones from earlier steps are faded. Click a picture to see it larger.

The GY-521 module has its own 3.3 V regulator, so it runs from the UNO's 5V. If you have never used the sensor before, start with the MPU-6050 lesson; the servo lesson explains the servo.

The sketch

#include <Wire.h>
#include <Servo.h>

const int MPU = 0x68, SERVO_PIN = 9;
int direction = 1;                    // type "flip" if the platform runs away instead of levelling
float kp = 1.2, ki = 1.5, kd = 0.05;  // the three PID settings: change them live in the Serial Monitor

Servo servo;
float angle = 0, bias = 0, integral = 0;
unsigned long lastMicros, lastPrint = 0;

void readSensor(float& ay, float& az, float& gx) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);                   // first register: acceleration X
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14);
  int16_t v[7];
  for (int i = 0; i < 7; i++) {
    v[i] = Wire.read() << 8;
    v[i] |= Wire.read();
  }
  ay = v[1] / 16384.0;                // acceleration in g
  az = v[2] / 16384.0;
  gx = v[4] / 131.0;                  // turning speed around X in degrees per second
}

void printSettings() {
  Serial.print("kp ");
  Serial.print(kp);
  Serial.print("   ki ");
  Serial.print(ki);
  Serial.print("   kd ");
  Serial.print(kd);
  Serial.print("   direction ");
  Serial.println(direction);
}

void setup() {
  Serial.begin(115200);
  Serial.setTimeout(50);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);                   // power register: wake up
  Wire.write(0);
  Wire.endTransmission();
  servo.attach(SERVO_PIN);
  servo.write(90);
  delay(1000);

  float ay, az, gx, sum = 0;          // keep it still: measure the gyro's small error
  for (int i = 0; i < 200; i++) {
    readSensor(ay, az, gx);
    sum += gx;
    delay(3);
  }
  bias = sum / 200;
  readSensor(ay, az, gx);
  angle = atan2(ay, az) * 180 / PI;
  printSettings();
  Serial.println("Type for example: kp 2   ki 0   kd 0.1   flip");
  lastMicros = micros();
}

void loop() {
  // 1. Measure: gyro for fast changes, accelerometer to stop slow drift
  float ay, az, gx;
  readSensor(ay, az, gx);
  unsigned long now = micros();
  float dt = (now - lastMicros) / 1000000.0;
  lastMicros = now;
  float rate = gx - bias;
  float accAngle = atan2(ay, az) * 180 / PI;
  angle = 0.98 * (angle + rate * dt) + 0.02 * accAngle;

  // 2. Compare: how far from level are we?
  float error = 0 - angle;

  // 3. Correct: P + I + D
  integral += error * dt;
  integral = constrain(integral, -25, 25);        // stop the I part from growing forever
  float output = kp * error + ki * integral - kd * rate;
  servo.write(constrain(90 + direction * output, 30, 150));

  // Live tuning from the Serial Monitor
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    float value = cmd.substring(3).toFloat();
    if (cmd.startsWith("kp ")) kp = value;
    else if (cmd.startsWith("ki ")) { ki = value; integral = 0; }
    else if (cmd.startsWith("kd ")) kd = value;
    else if (cmd == "flip") direction = -direction;
    printSettings();
  }

  // For the Serial Plotter: the platform's angle and the servo's correction
  if (millis() - lastPrint >= 50) {
    lastPrint = millis();
    Serial.print("angle:");
    Serial.print(angle, 1);
    Serial.print(",correction:");
    Serial.println(output, 1);
  }
  delay(5);                           // about 150 loops a second
}

Try it

1. Hold the base still and level, and upload the sketch. The first second is used to measure the gyro's error, so don't touch it.
2. Open the Serial Monitor at 115200 baud, choose Newline as the line ending, and slowly tilt the base. The platform should tilt the other way and stay level.
3. If the platform tilts even further instead, it is pushing the wrong way: type flip and press Enter.
4. Now explore one setting at a time. Type ki 0 and kd 0 to keep only P. Tilt the base: the platform corrects, but not all the way. Try kp 4: faster, but it starts to wobble.
5. Back to kp 1.2, then ki 1.5: the last few degrees disappear after a second. Add kd 0.05: tap the platform and it settles without bouncing.
6. Close the Serial Monitor and open the Serial Plotter to watch the angle and the correction while you tilt and tap.

What you should see

How it works

Measure. The gyroscope measures how fast the platform turns, in degrees per second. Adding up those small turns gives the angle, smooth and fast, but every tiny error adds up too, so the angle slowly drifts. The accelerometer feels gravity and knows which way is down, but every bump and shake disturbs it. The complementary filter mixes both: 98 % gyro for the fast changes and 2 % accelerometer to pull the angle slowly back to the truth. Drones use the same trick, or a more advanced version called a Kalman filter.

Compare. The error is how far the platform is from level. If it leans 10° one way, the error is −10°.

Correct. The three parts of PID each look at the error differently:

The derivative of the angle is its turning speed, and that is exactly what the gyro measures, so the D part simply uses rate. Flight controllers do the same.

From one servo to four motors. A drone runs this loop three times at once, for roll (tipping left and right), pitch (nose up and down) and yaw (turning), often 1,000 to 8,000 times a second. It has no servo to tilt, so it changes motor speeds instead: to lift the left side, the left motors spin a little faster and the right ones a little slower. Each corner motor gets the sum of the three corrections plus the throttle. When drone pilots talk about "tuning the PIDs", they are changing the same three numbers you just typed.

Try this next

• Hold a small cup of water on the platform (outside, in case it spills) and walk around with the base. How level can you keep it?
• Change the target from 0 to 15 degrees: float error = 15 - angle;. The platform now holds that angle, like a drone flying forwards with its nose tipped down.
• Add a second servo and use the sensor's Y axis to level a second direction: a two-axis gimbal for a phone camera.
• Count how many loops run each second and see how the platform behaves when you add a delay(50). Why do drones need fast loops?

Troubleshooting

• The platform tips over to one end and stays there: it pushes the wrong way. Type flip.
• It shakes or buzzes all the time: lower kp and kd. Check that the sensor is stuck down firmly; a loose sensor shakes on its own.
• It slowly drifts away from level: keep it completely still during the first second after a reset, while it measures the gyro's error.
• The angle is stuck at 0 and the servo doesn't move: check SDA on A4 and SCL on A5.
• The board restarts when the servo moves: servos draw bursts of current. Use a good USB port or a powered hub, and check that the servo's red wire is on 5V.