#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>

Adafruit_MPU6050 mpu;
LiquidCrystal_I2C lcd(0x27,16 ,2);
const int echo = 10;
const int trig = 9;

float g = 9.81;



void setup() {

  lcd.init();
  lcd.backlight();

  
  Serial.begin(3600);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);

mpu.begin();
}
void loop() {

  digitalWrite(trig, LOW);
  delayMicroseconds(10);
  digitalWrite(echo, HIGH);
  delayMicroseconds (10);


  long duration = pulseIn(echo , HIGH);
  float InitialH =( duration / 2.00 * 0.034);

  sensors_event_t event;

  mpu.getGyroSensor()->getEvent(&event);
  float Yaw = (event.gyro.x);
  float Pitch = (event.gyro.z);

   float ObtainH = (InitialH * cos(Yaw));
   float CalcH = (ObtainH * cos(Pitch));

   float velocity = sqrt(2 * g * CalcH);

   lcd.print(velocity);


   lcd.clear();
   lcd.setCursor(0,0);
   delay(1000);
}