Lesson 2: Tilt Angle from a Baseline

/*
  DIY KIT - ESP32 Fall-Detection Learning Prototype
  Lesson 2 of 5: Tilt Angle from a Baseline

  WIRING (unplug the USB cable before you wire)
    Keep the OLED and button from Lesson 1.
    GY-521 (MPU-6050)  VCC -> 3V3   GND -> GND   SDA -> GPIO21   SCL -> GPIO22
    The OLED and the GY-521 share the same two I2C wires (SDA and SCL).
    Leave XDA, XCL, AD0 and INT on the GY-521 unconnected.

  WHAT SHOULD HAPPEN
    Keep the sensor still for 3 seconds while it learns "upright". Then the
    OLED shows the angle you have tilted it from that position. Past 55
    degrees it says TILTED. Press the button to learn a new "upright".
    Serial Monitor (115200) or Serial Plotter shows angle_deg.
*/

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

const int SDA_PIN = 21;
const int SCL_PIN = 22;
const int BUTTON_PIN = 26;
const float TILT_DEG = 55.0;          // the "lying down" angle used in Lesson 5
const int CALIBRATION_SAMPLES = 150;  // 150 x 20 ms = 3 seconds
const unsigned long SAMPLE_MS = 20;
const unsigned long REPORT_MS = 250;

Adafruit_SSD1306 display(128, 64, &Wire, -1);
bool displayOk = false;
int mpuAddress = 0;                   // 0x68 or 0x69, found at start
float baseX = 0, baseY = 0, baseZ = 1;  // the "upright" direction of gravity
unsigned long lastSample = 0, lastReport = 0;
bool lastButton = HIGH;

bool answers(int address) {
  Wire.beginTransmission(address);
  return Wire.endTransmission() == 0;
}

// Read the three accelerometer axes, in g. 1 g = the pull of gravity.
bool readAccel(float &x, float &y, float &z) {
  Wire.beginTransmission(mpuAddress);
  Wire.write(0x3B);                   // the first acceleration register
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom(mpuAddress, 6) != 6) return false;
  int16_t rx = (int16_t)((Wire.read() << 8) | Wire.read());
  int16_t ry = (int16_t)((Wire.read() << 8) | Wire.read());
  int16_t rz = (int16_t)((Wire.read() << 8) | Wire.read());
  x = rx / 16384.0;                   // +-2 g range: 16384 counts per g
  y = ry / 16384.0;
  z = rz / 16384.0;
  return true;
}

void show(const char *line1, int angle) {
  if (!displayOk) return;
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println("TILT ANGLE LAB");
  display.println(line1);
  if (angle >= 0) {
    display.setTextSize(3);
    display.setCursor(0, 22);
    display.print(angle);
    display.setTextSize(2);
    display.print(" deg");
  }
  display.setTextSize(1);
  display.setCursor(0, 56);
  display.print("limit ");
  display.print((int)TILT_DEG);
  display.print("  btn=reset");      // "limit 55  btn=reset": 21 characters fit on a line
  display.display();
}

// Average 150 readings while the sensor is still: that is "upright".
bool calibrate() {
  show("KEEP STILL: 3 s", -1);
  Serial.println("Calibrating - keep the sensor still");
  float sx = 0, sy = 0, sz = 0;
  int good = 0;
  for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
    float x, y, z;
    if (readAccel(x, y, z)) {
      sx += x; sy += y; sz += z;
      good++;
    }
    delay(SAMPLE_MS);
  }
  float length = sqrt(sx * sx + sy * sy + sz * sz);
  if (good == 0 || length < 0.001) return false;
  baseX = sx / length;                // keep only the direction
  baseY = sy / length;
  baseZ = sz / length;
  Serial.println("Calibrated");
  return true;
}

void setup() {
  Serial.begin(115200);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  Wire.begin(SDA_PIN, SCL_PIN);
  displayOk = display.begin(SSD1306_SWITCHCAPVCC, 0x3C) || display.begin(SSD1306_SWITCHCAPVCC, 0x3D);
  if (!displayOk) Serial.println("OLED not found - see Lesson 1");

  if (answers(0x68)) mpuAddress = 0x68;
  else if (answers(0x69)) mpuAddress = 0x69;
  if (mpuAddress == 0) {
    Serial.println("GY-521 not found - unplug USB and check VCC 3V3, GND, SDA GPIO21, SCL GPIO22");
    show("GY-521 NOT FOUND", -1);
    while (true) delay(1000);         // stop here
  }
  Serial.print("GY-521 found at 0x");
  Serial.println(mpuAddress, HEX);
  Wire.beginTransmission(mpuAddress); // wake the sensor up (it starts asleep)
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  delay(50);
  if (!calibrate()) Serial.println("Calibration failed - is the sensor wired?");
}

void loop() {
  bool button = digitalRead(BUTTON_PIN);
  if (button == LOW && lastButton == HIGH) calibrate();   // press: learn a new "upright"
  lastButton = button;

  if (millis() - lastSample < SAMPLE_MS) return;
  lastSample = millis();
  float x, y, z;
  if (!readAccel(x, y, z)) return;
  float length = sqrt(x * x + y * y + z * z);
  if (length < 0.001) return;

  // The angle between "upright" and gravity now: the dot product of the two directions.
  float dot = (x * baseX + y * baseY + z * baseZ) / length;
  float angle = acos(constrain(dot, -1.0, 1.0)) * 180.0 / PI;

  if (millis() - lastReport >= REPORT_MS) {
    lastReport = millis();
    Serial.print("angle_deg:");
    Serial.print(angle, 1);
    Serial.print(",limit:");
    Serial.println(TILT_DEG, 0);
    show(angle >= TILT_DEG ? "TILTED past the limit" : "upright", (int)angle);
  }
}