MPU6050 servo stabilizer on an Arduino Uno

// MPU6050 servo stabilizer: a two-axis self-leveling gimbal. Tilt the
// MPU6050 and two servos turn the other way to keep a platform level, the
// way a camera gimbal or a model plane's wing leveler does.
//
//   MPU6050  SDA to A4, SCL to A5, VCC to 5V, GND to GND
//   servos   roll on pin 9, pitch on pin 10, from a separate 5 V supply
//            with the grounds joined
//
// TILT FROM GRAVITY. Held still, an accelerometer measures only gravity, so
// the way gravity splits between its three axes tells you the tilt:
//
//     roll  = atan2(ay, az)
//     pitch = atan2(-ax, sqrt(ay*ay + az*az))
//
// That is right on a bench and wrong in a moving model, where every bump
// and turn adds acceleration that is not gravity.
//
// TILT FROM THE GYRO. The gyroscope measures how fast the board is turning,
// in degrees per second. Add that up over time and you also have the tilt,
// smooth and unbothered by bumps, but every tiny error adds up too and the
// angle slowly drifts away.
//
// THE COMPLEMENTARY FILTER uses each for what it is good at: trust the gyro
// from moment to moment, and pull gently toward the accelerometer so the
// drift cannot build up.
//
//     alpha = TAU / (TAU + dt)
//     angle = alpha * (angle + rate * dt) + (1 - alpha) * accelAngle
//
// TAU is the filter's time constant, a quarter of a second here: after a
// tilt the angle covers about two thirds of the way to the accelerometer's
// answer in TAU, and nearly all of it in four TAU. Working alpha out from
// dt on every pass keeps that true however long a pass takes. Reading the
// sensor and the floating-point math take time too, so a pass is longer
// than the 10 ms delay at the end of loop().
//
// THE SERVOS hold the platform level by turning the same amount the other
// way: 90 degrees is level, so a roll of +20 sends the roll servo to 70.

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

Adafruit_MPU6050 mpu;
Servo rollServo, pitchServo;

const float TAU = 0.25;  // the filter's time constant, in seconds
const float DEGREES = 180.0 / M_PI;

// If a servo turns the same way as the tilt on your build, set its entry
// to -1 so it turns the other way.
const int ROLL_DIRECTION = -1;
const int PITCH_DIRECTION = -1;

float roll = 0, pitch = 0;  // the filtered tilt, in degrees
unsigned long lastMicros = 0;
unsigned long lastPrint = 0;

void setup() {
  Serial.begin(115200);
  if (!mpu.begin()) {
    Serial.println("no MPU6050 at 0x68: check SDA, SCL and VCC");
    while (true) delay(10);
  }
  rollServo.attach(9);
  pitchServo.attach(10);
  rollServo.write(90);
  pitchServo.write(90);

  // Start the filter from the accelerometer, so it does not have to swing
  // up from zero at power on.
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  roll = atan2(a.acceleration.y, a.acceleration.z) * DEGREES;
  pitch = atan2(-a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * DEGREES;
  lastMicros = micros();
  Serial.println("stabilizer: drag the MPU6050 to tilt it");
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // How long since the last pass, in seconds.
  unsigned long now = micros();
  float dt = (now - lastMicros) / 1000000.0;
  lastMicros = now;

  // Tilt from gravity alone.
  float ax = a.acceleration.x, ay = a.acceleration.y, az = a.acceleration.z;
  float accelRoll = atan2(ay, az) * DEGREES;
  float accelPitch = atan2(-ax, sqrt(ay * ay + az * az)) * DEGREES;

  // The complementary filter. The gyro reads radians per second.
  float alpha = TAU / (TAU + dt);
  roll = alpha * (roll + g.gyro.x * DEGREES * dt) + (1 - alpha) * accelRoll;
  pitch = alpha * (pitch + g.gyro.y * DEGREES * dt) + (1 - alpha) * accelPitch;

  // Turn each servo against its tilt, held to its travel.
  int rollAngle = constrain(90 + ROLL_DIRECTION * (int)lround(roll), 0, 180);
  int pitchAngle = constrain(90 + PITCH_DIRECTION * (int)lround(pitch), 0, 180);
  rollServo.write(rollAngle);
  pitchServo.write(pitchAngle);

  // Report five times a second.
  if (millis() - lastPrint >= 200) {
    lastPrint = millis();
    Serial.print("roll ");
    Serial.print(roll, 1);
    Serial.print("  pitch ");
    Serial.print(pitch, 1);
    Serial.print("  ->  roll servo ");
    Serial.print(rollAngle);
    Serial.print("  pitch servo ");
    Serial.println(pitchAngle);
  }
  delay(10);
}