Lesson 3: Motion Data Lab

/*
  DIY KIT - ESP32 Fall-Detection Learning Prototype
  Lesson 3 of 5: Motion Data Lab

  WIRING: the same as Lesson 2 (OLED and GY-521 on GPIO21/22, button on GPIO26).

  WHAT SHOULD HAPPEN
    Open Tools > Serial Plotter at 115200 baud. The total_g line sits at about
    1.0 while the sensor is still. Lift and drop the test platform gently onto
    foam: total_g dips towards 0 while it falls and spikes when it lands.
    The OLED counts every IMPACT (above 2.5 g) and FREE FALL (below 0.45 g).
    Press the button to set the counts back to 0.
    Use a padded, unoccupied test platform - never a person.
*/

#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 IMPACT_G = 2.5;           // the same limits as Lesson 5
const float FREEFALL_G = 0.45;
const unsigned long SAMPLE_MS = 20;   // 50 readings a second
const unsigned long REPORT_MS = 100;  // 10 plotter points a second

Adafruit_SSD1306 display(128, 64, &Wire, -1);
bool displayOk = false;
int mpuAddress = 0;
int impacts = 0, freefalls = 0;
bool wasImpact = false, wasFreefall = false;
float peakG = 0, lowestG = 10;
unsigned long lastSample = 0, lastReport = 0;
bool lastButton = HIGH;

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

bool writeRegister(int reg, int 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(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 / 4096.0;                    // +-8 g range: 4096 counts per g
  y = ry / 4096.0;
  z = rz / 4096.0;
  return true;
}

void showCounts(float g) {
  if (!displayOk) return;
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println("MOTION DATA LAB");
  display.setTextSize(2);
  display.setCursor(0, 12);
  display.print(g, 2);
  display.println(" g");
  display.setTextSize(1);
  display.setCursor(0, 34);
  display.print("impacts   "); display.println(impacts);
  display.print("free falls "); display.println(freefalls);
  display.print("peak "); display.print(peakG, 1); display.print("  low "); display.println(lowestG, 2);
  display.display();
}

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 (answers(0x68)) mpuAddress = 0x68;
  else if (answers(0x69)) mpuAddress = 0x69;
  if (mpuAddress == 0 || !writeRegister(0x6B, 0x00) || !writeRegister(0x1C, 0x10)) {   // wake up, +-8 g
    Serial.println("GY-521 not found - see Lesson 2");
    while (true) delay(1000);
  }
  Serial.println("Lesson 3: open the Serial Plotter");
}

void loop() {
  bool button = digitalRead(BUTTON_PIN);
  if (button == LOW && lastButton == HIGH) {   // press: reset the counts
    impacts = 0; freefalls = 0; peakG = 0; lowestG = 10;
  }
  lastButton = button;

  if (millis() - lastSample < SAMPLE_MS) return;
  lastSample = millis();
  float x, y, z;
  if (!readAccel(x, y, z)) return;
  float g = sqrt(x * x + y * y + z * z);   // total g-force, whichever way it points
  if (g > peakG) peakG = g;
  if (g < lowestG) lowestG = g;

  // Count each crossing once, not every reading above the line.
  bool impact = g > IMPACT_G;
  if (impact && !wasImpact) impacts++;
  wasImpact = impact;
  bool freefall = g < FREEFALL_G;
  if (freefall && !wasFreefall) freefalls++;
  wasFreefall = freefall;

  if (millis() - lastReport >= REPORT_MS) {
    lastReport = millis();
    Serial.print("total_g:");
    Serial.print(g, 2);
    Serial.print(",impact_line:");
    Serial.print(IMPACT_G, 2);
    Serial.print(",freefall_line:");
    Serial.println(FREEFALL_G, 2);
    showCounts(g);
  }
}