Lesson 5: Fall-Detection Learning Prototype

/*
  DIY KIT - ESP32 Fall-Detection Learning Prototype
  Lesson 5 of 5: Fall-Detection Learning Prototype (the main build)

  WIRING: everything from Lessons 1-4.
    OLED and GY-521: VCC -> 3V3, GND -> GND, SDA -> GPIO21, SCL -> GPIO22
    Button: GPIO26 and GND        Buzzer: I/O -> GPIO27, VCC -> 3V3, GND -> GND

  WHAT SHOULD HAPPEN
    KEEP STILL AND LEVEL for 3 seconds (it learns "upright"), then MONITORING.
    A jolt above 2.5 g or a drop below 0.45 g, followed by staying tipped more
    than 55 degrees for 1.5 seconds, starts a 20-second countdown with a beep
    every second. Press the button to cancel. If nobody does, LOCAL ALERT:
    the buzzer beeps until you press the button. Nothing is sent anywhere.
    Serial Monitor at 115200 baud.

    Set FALL_DIAGNOSTIC to true to start the countdown with the button
    instead of a movement - handy for testing the screen and buzzer.

  An educational bench prototype. NOT a medical, wheelchair, emergency or
  life-safety device. Test only on a padded, unoccupied platform.
*/

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

#ifndef FALL_DIAGNOSTIC
#define FALL_DIAGNOSTIC false
#endif

constexpr uint8_t SDA_PIN = 21;
constexpr uint8_t SCL_PIN = 22;
constexpr uint8_t BUTTON_PIN = 26;
constexpr uint8_t BUZZER_PIN = 27;  // Lesson 4
constexpr uint8_t LED_PIN = 2;  // Common ESP32 Dev Module onboard LED; verify board.
// Most buzzer modules beep when the pin is HIGH. If yours beeps in the gaps
// or all the time, change false to true (see Lesson 4).
constexpr bool BUZZER_ACTIVE_LOW = false;
constexpr uint32_t SAMPLE_MS = 20;
constexpr uint32_t DEBOUNCE_MS = 35;
constexpr uint32_t CANDIDATE_MS = 5000;
constexpr uint32_t TILT_HOLD_MS = 1500;
constexpr uint32_t COUNTDOWN_MS = 20000;
constexpr uint32_t DISPLAY_MS = 250;
constexpr float IMPACT_G = 2.5f;
constexpr float FREEFALL_G = 0.45f;
constexpr float TILT_DEG = 55.0f;

Adafruit_SSD1306 display(128, 64, &Wire, -1, 100000, 100000);

enum class State { Monitoring, Candidate, Countdown, Alert, Fault };
State state = State::Fault;
uint8_t mpuAddress = 0, oledAddress = 0;
float baseX = 0, baseY = 0, baseZ = 1;
uint32_t stateSince = 0, tiltSince = 0, lastSample = 0, lastDisplay = 0;
uint32_t events = 0;
uint8_t readFailures = 0;
bool rawButton = HIGH, stableButton = HIGH, buttonArmed = false;
uint32_t rawSince = 0;

void buzzer(bool on) {
  if (BUZZER_ACTIVE_LOW) digitalWrite(BUZZER_PIN, on ? LOW : HIGH);
  else                   digitalWrite(BUZZER_PIN, on ? HIGH : LOW);
}

bool elapsed(uint32_t now, uint32_t since, uint32_t interval) {
  return uint32_t(now - since) >= interval;
}

bool i2cAck(uint8_t address) {
  Wire.beginTransmission(address);
  return Wire.endTransmission() == 0;
}

bool writeRegister(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(mpuAddress);
  Wire.write(reg);
  Wire.write(value);
  return Wire.endTransmission() == 0;
}

bool readAccel(float &x, float &y, float &z) {
  Wire.beginTransmission(mpuAddress);
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom(int(mpuAddress), 6) != 6) return false;
  int16_t rx = int16_t((uint16_t(Wire.read()) << 8) | Wire.read());
  int16_t ry = int16_t((uint16_t(Wire.read()) << 8) | Wire.read());
  int16_t rz = int16_t((uint16_t(Wire.read()) << 8) | Wire.read());
  x = rx / 4096.0f;
  y = ry / 4096.0f;
  z = rz / 4096.0f;
  return true;
}

void show(const __FlashStringHelper *line1, const String &line2 = "") {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println(F("FALL-DETECTION LAB"));
  display.println(F("TRAINING ONLY"));
  display.println();
  display.println(line1);
  display.println(line2);
  display.display();
}

void fault(const __FlashStringHelper *reason) {
  state = State::Fault;
  buzzer(false);
  digitalWrite(LED_PIN, HIGH);
  Serial.print(F("FAULT: "));
  Serial.println(reason);
  if (oledAddress && i2cAck(oledAddress)) show(F("FAULT - POWER OFF"), String(reason));
}

bool initialiseDevices() {
  bool oled3c = i2cAck(0x3C), oled3d = i2cAck(0x3D);
  if (oled3c == oled3d) return false;
  oledAddress = oled3c ? 0x3C : 0x3D;
  if (!display.begin(SSD1306_SWITCHCAPVCC, oledAddress, true, false)) return false;

  bool mpu68 = i2cAck(0x68), mpu69 = i2cAck(0x69);
  if (mpu68 == mpu69) return false;
  mpuAddress = mpu68 ? 0x68 : 0x69;
  if (!writeRegister(0x6B, 0x01)) return false;
  delay(50);
  if (!writeRegister(0x1C, 0x10)) return false;
  if (!writeRegister(0x1A, 0x04)) return false;
  Serial.printf("OLED=0x%02X MPU=0x%02X\n", oledAddress, mpuAddress);
  return true;
}

bool calibrateBaseline() {
  show(F("KEEP STILL AND LEVEL"), F("Calibrating 3 seconds"));
  float sx = 0, sy = 0, sz = 0;
  constexpr int samples = 150;
  for (int i = 0; i < samples; ++i) {
    float x, y, z;
    if (!readAccel(x, y, z)) return false;
    float mag = sqrtf(x * x + y * y + z * z);
    if (mag < 0.75f || mag > 1.25f) return false;
    sx += x; sy += y; sz += z;
    delay(20);
  }
  float mag = sqrtf(sx * sx + sy * sy + sz * sz);
  if (mag < 0.001f) return false;
  baseX = sx / mag; baseY = sy / mag; baseZ = sz / mag;
  return true;
}

float angleFromBaseline(float x, float y, float z, float mag) {
  if (mag < 0.001f) return 180.0f;
  float dot = (x * baseX + y * baseY + z * baseZ) / mag;
  return acosf(constrain(dot, -1.0f, 1.0f)) * 180.0f / PI;
}

bool pressedEdge(uint32_t now) {
  bool reading = digitalRead(BUTTON_PIN);
  if (reading != rawButton) {
    rawButton = reading;
    rawSince = now;
  }
  bool edge = false;
  if (reading != stableButton && elapsed(now, rawSince, DEBOUNCE_MS)) {
    stableButton = reading;
    edge = stableButton == LOW;
    Serial.println(edge ? F("Button pressed") : F("Button released"));
  }
  if (stableButton == HIGH && rawButton == HIGH && elapsed(now, rawSince, DEBOUNCE_MS))
    buttonArmed = true;
  if (edge && buttonArmed) {
    buttonArmed = false;
    return true;
  }
  return false;
}

void enterCountdown(uint32_t now, const __FlashStringHelper *source) {
  state = State::Countdown;
  stateSince = now;
  ++events;
  Serial.print(F("Countdown started: "));
  Serial.println(source);
  show(F("EVENT? 20 SECONDS"), F("Button = cancel"));
}

void setup() {
  Serial.begin(115200);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);
  pinMode(BUZZER_PIN, OUTPUT);
  buzzer(false);
  rawButton = stableButton = digitalRead(BUTTON_PIN);
  rawSince = millis();
  if (!Wire.begin(SDA_PIN, SCL_PIN, 100000)) {
    fault(F("I2C start failed"));
    return;
  }
  Wire.setTimeOut(20);
  if (!initialiseDevices()) {
    fault(F("Need one OLED and one MPU"));
    return;
  }
  if (!calibrateBaseline()) {
    fault(F("Calibration failed"));
    return;
  }
  state = State::Monitoring;
  stateSince = millis();
  show(F("MONITORING"), FALL_DIAGNOSTIC ? F("Button starts test") : F("Local demo only"));
  Serial.println(F("Local demo only: no Wi-Fi, no messages, no emergency call."));
}

void loop() {
  uint32_t now = millis();
  bool button = pressedEdge(now);
  if (state == State::Fault) return;

  if (button && state == State::Countdown) {
    state = State::Monitoring;
    stateSince = now;
    digitalWrite(LED_PIN, LOW);
    buzzer(false);
    show(F("CANCELLED"), F("Monitoring resumed"));
    Serial.println(F("Countdown cancelled"));
    return;
  }
  if (button && state == State::Alert) {
    state = State::Monitoring;
    stateSince = now;
    digitalWrite(LED_PIN, LOW);
    buzzer(false);
    show(F("ACKNOWLEDGED"), F("Monitoring resumed"));
    Serial.println(F("Local alert acknowledged"));
    return;
  }
  if (button && FALL_DIAGNOSTIC && state == State::Monitoring) {
    enterCountdown(now, F("diagnostic button"));
    return;
  }

  if (state == State::Countdown) {
    uint32_t used = now - stateSince;
    if (used >= COUNTDOWN_MS) {
      state = State::Alert;
      stateSince = now;
      buzzer(false);
      show(F("LOCAL ALERT"), F("Press to acknowledge"));
      Serial.println(F("LOCAL ALERT ONLY - no message was sent"));
      return;
    }
    digitalWrite(LED_PIN, ((now / 300) & 1) ? HIGH : LOW);
    buzzer((used % 1000) < 80);  // a short beep at the start of every second
    if (elapsed(now, lastDisplay, DISPLAY_MS)) {
      lastDisplay = now;
      uint32_t seconds = (COUNTDOWN_MS - used + 999) / 1000;
      show(F("POSSIBLE EVENT"), String(seconds) + F(" s - button cancels"));
    }
    return;
  }
  if (state == State::Alert) {
    digitalWrite(LED_PIN, ((now / 150) & 1) ? HIGH : LOW);
    buzzer(((now - stateSince) % 500) < 150);  // 150 ms on, 350 ms off - as in Lesson 4
    return;
  }
  if (!elapsed(now, lastSample, SAMPLE_MS)) return;
  lastSample = now;

  float x, y, z;
  if (!readAccel(x, y, z)) {
    if (++readFailures >= 5) fault(F("MPU read failed"));
    return;
  }
  readFailures = 0;
  float mag = sqrtf(x * x + y * y + z * z);
  float tilt = angleFromBaseline(x, y, z, mag);

  if (state == State::Monitoring) {
    if (mag > IMPACT_G || mag < FREEFALL_G) {
      state = State::Candidate;
      stateSince = now;
      tiltSince = 0;
      Serial.printf("Candidate: %.2f g, %.1f deg\n", mag, tilt);
    } else if (elapsed(now, lastDisplay, 1000)) {
      lastDisplay = now;
      show(F("MONITORING"), String(mag, 2) + F(" g  events ") + events);
    }
    return;
  }

  if (state == State::Candidate) {
    if (elapsed(now, stateSince, CANDIDATE_MS)) {
      state = State::Monitoring;
      tiltSince = 0;
      show(F("NO EVENT"), F("Monitoring resumed"));
      return;
    }
    if (tilt >= TILT_DEG) {
      if (tiltSince == 0) tiltSince = now;
      if (elapsed(now, tiltSince, TILT_HOLD_MS)) enterCountdown(now, F("motion rule"));
    } else {
      tiltSince = 0;
    }
  }
}