DIY jump sensor

/* ------ JUMP SENSOR ------ Mario Wang

A DIY jump sensor that measures the height of every single jump with a small margin of error.

As a volleyball player, I can say that athletes who jump a lot are interested in knowing how high
they jump and how many jumps they do in a session.

HOW IT WORKS
The sensor is based on a GY-6500 (MPU6500) accelerometer. In a flat, static position it reads
about 1 g on the Z axis, which corresponds to 9.81 m/s^2. During the flight phase the reading
drops close to 0, because the accelerometer only measures g while the sensor is in contact with
the ground: in free fall there is no contact force to measure. On landing, the impact generates
a peak above 2 g.
I mentioned the Z axis for simplicity, but the program actually uses the magnitude
M = sqrt(gX^2 + gY^2 + gZ^2). This makes the measurement independent of the sensor orientation,
which matters because the device can be tilted while worn.

FORMULA
h = (1/8) * g * t^2, derived from the equations of motion v = v0 - g*t and s = s0 + v0*t - (1/2)*g*t^2,
where t is the total flight time between take-off and landing.

HARDWARE
ESP32 (chosen over Arduino for its smaller size), GY-6500 accelerometer, SSD1306 OLED display,
breadboard and MB-V2 power supply module.

VALIDATION
Tested against 240 fps slow-motion video: 0.56 s (video) vs 0.53 s (sensor), and
0.48 s (video) vs 0.47 s (sensor). Error margin: 2-5%.

KNOWN LIMITATION
The height cannot be measured by simply throwing the device in the air: during the flight the
sensor may rotate, which creates false landing triggers.

I'm 17, I play volleyball in an elite club and I'm passionate about tech.
Suggestions are welcome - @sono_marione
*/




#include "FastIMU.h"
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <math.h>

#define IMU_ADDRESS 0x68    
#define PERFORM_CALIBRATION // Sensor calibration 
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


// Module GY-6500
MPU6500 IMU;               

// Pin I2C ESP32
#define SDA_PIN 21
#define SCL_PIN 22

calData calib = { 0 };  
AccelData accelData;    
GyroData gyroData;

// JUMP specific
  double h, M, timetot;
  unsigned long tpre, tnow;
  const double Tmin = 300; //ms 0,3m min
  const double Tmax = 1100; //Impossible jump! (148cm)
  bool Act = false;
  const double GMIN = 0.5;  
  const double GMAX = 1.5;
  bool pre = false;
  int countS = 0;

  void setup() {
  // Initialization for esp32 i2c pins
  Wire.begin(SDA_PIN, SCL_PIN);
  Wire.setClock(400000); 
  
  Serial.begin(115200);
  while (!Serial) { delay(10); }

  Serial.println("\n--- Initialize GY-6500 (MPU6500) ---");

  // Inizializzazione del chip
  int err = IMU.init(calib, IMU_ADDRESS);
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)){
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }

  display.display(); // inizializza
  delay(200);

  display.clearDisplay(); //delete buffer
  display.setTextSize(1);  
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0); 

  if (err != 0) {
    Serial.print("Error initialization IMU: ");
    Serial.println(err);
    while (true) { delay(1000); }
  }
  
  Serial.println("Sensor GY-6500 reconized!");

#ifdef PERFORM_CALIBRATION
  Serial.println("\n--- CALIBRATION ---");
  Serial.println("Keep the sensor in a flat surface");
  delay(3000);
  
  // Calib Gyro and Accel
  IMU.calibrateAccelGyro(&calib);
  Serial.println("Success!");
  
  // Reboot the sensor with correct calibration data
  IMU.init(calib, IMU_ADDRESS);
  delay(1000);
#endif

  Serial.println("\n Begin ");
  display.clearDisplay();
  display.setCursor(0,0);
  display.print("You can jump!");
  display.display();
}

void loop() {
  IMU.update();
  IMU.getAccel(&accelData);
  M = sqrt(accelData.accelX*accelData.accelX+accelData.accelY*accelData.accelY+accelData.accelZ*accelData.accelZ);

  if(M < GMIN && Act == false){
    Act = true;
  }
  else if(M > GMAX && Act == true){
    Act = false;
    pre = false;
    tnow = millis();
    if(tnow - tpre >= Tmin && tnow - tpre <= Tmax){
      countS++;
      Serial.println("time in s: ");
      timetot = (tnow - tpre) / 1000.0;
      Serial.println(timetot);
      h = 1.0/8.0 * 9.81 * timetot*timetot*100;
      Serial.println("Height in cm: ");
      Serial.println(h);
      Serial.println("jumps: ");
      Serial.println(countS);

      //display
      display.clearDisplay();
      display.setCursor(0,0);
      display.print("time in s: ");
      display.print(timetot);
      display.print("\nheight in cm: ");
      display.print(h);    
      display.print("\njumps: ");    
      display.print(countS);    
      display.display();

    }
  }

  if(Act && pre == false){
    pre = true;
    tpre = millis();
  }


}