Tilt meter on an Arduino Uno

// Tilt meter: an MPU6050 over I2C, three lamps and a serial readout.
//
//   A5 -> SCL   A4 -> SDA   the GY-521 board carries its own pull-ups
//   pin 7   the red lamp:   tipped left
//   pin 8   the green lamp: flat
//   pin 9   the blue lamp:  tipped right
//
// Drag the board on the canvas to tip it. Three things worth knowing, and all
// three are in the first twenty lines of every driver ever written for this
// chip.
//
// # It powers up asleep
//
// PWR_MGMT_1 reads 0x40 after reset, the SLEEP bit is set, and until it is
// cleared every measurement register reads zero. A sketch that forgets gets a
// perfectly steady nothing and no error, which is worse than a crash. The
// driver's begin() writes 0x00 there, and this sketch prints whether that
// worked.
//
// # WHO_AM_I is 0x68 whatever the address is
//
// The register holds bits 6:1 of the address and AD0 is bit 0, so a board
// strapped to 0x69 still identifies as 0x68. Comparing WHO_AM_I against the
// address a driver is using is wrong on exactly the board somebody had to
// strap, and this prints both so the difference is visible.
//
// # An accelerometer at rest measures gravity
//
// That is the whole trick. Flat, all of gravity is on Z and the reading is
// (0, 0, +1 g). Tip it and gravity moves onto X and Y, and the angle is the
// arctangent of the two. There is no floating point on this board, so the
// driver's tiltDegrees() does the arctangent from a table, good to about a
// degree, which is far better than a real MPU6050 is before it is calibrated.
//
// The gyroscope reads how *fast* the board is being moved rather than where it
// is, so it sits at zero until you drag it and falls back the moment you stop.

#include "mokxi_i2c.h"
#include "mokxi_mpu6050.h"

const int SCL_PIN = A5;
const int SDA_PIN = A4;
const int LAMP_LEFT = 7;
const int LAMP_FLAT = 8;
const int LAMP_RIGHT = 9;

// Past this much roll, one of the outer lamps takes over.
const int FLAT_DEGREES = 12;

SoftI2c bus(SCL_PIN, SDA_PIN);
Mpu6050 imu(bus);

unsigned long printedAt = 0;

void setup() {
  pinMode(LAMP_LEFT, OUTPUT);
  pinMode(LAMP_FLAT, OUTPUT);
  pinMode(LAMP_RIGHT, OUTPUT);
  Serial.begin(115200);
  Serial.println("Mokxi Uno: MPU6050 tilt meter");
  bus.begin();

  Serial.print("WHO_AM_I: 0x");
  Serial.println((long)imu.whoAmI(), HEX);
  if (!imu.begin()) {
    Serial.println("no answer at 0x68; check the wiring");
    return;
  }
  Serial.println("awake (SLEEP cleared)");
  // The most sensitive ranges, which is what a tilt meter wants.
  imu.setRanges(0, 0);
}

void loop() {
  Mpu6050Reading r;
  if (!imu.read(r)) {
    Serial.println("read failed");
    delay(500);
    return;
  }

  // Thousandths of a g. Flat on the bench is (0, 0, 1000).
  long ax = imu.milliG(r.ax);
  long ay = imu.milliG(r.ay);
  long az = imu.milliG(r.az);
  int roll = Mpu6050::tiltDegrees(ay, az);
  int pitch = Mpu6050::tiltDegrees(-ax, az);

  digitalWrite(LAMP_LEFT, roll < -FLAT_DEGREES ? HIGH : LOW);
  digitalWrite(LAMP_RIGHT, roll > FLAT_DEGREES ? HIGH : LOW);
  digitalWrite(LAMP_FLAT, (roll >= -FLAT_DEGREES && roll <= FLAT_DEGREES) ? HIGH : LOW);

  if (millis() - printedAt >= 500) {
    printedAt = millis();
    Serial.print("a ");
    Serial.print(ax);
    Serial.print(",");
    Serial.print(ay);
    Serial.print(",");
    Serial.print(az);
    Serial.print(" mg  roll ");
    Serial.print(roll);
    Serial.print(" pitch ");
    Serial.print(pitch);
    Serial.print("  gyro ");
    Serial.print(imu.degreesPerSecond(r.gx));
    Serial.print(",");
    Serial.print(imu.degreesPerSecond(r.gy));
    Serial.print(" deg/s  chip ");
    Serial.print(Mpu6050::temperatureTenths(r.temperature) / 10);
    Serial.println(" C");
  }
  delay(40);
}