const float g = 9.81;
#include<Wire.h>
#include<LiquidCrystal_I2C.h>
#include<MPU6050.h>
#define TRIGGER_PIN 13  // Pin connected to the trigger
#define ECHO_PIN 12     // Pin connected to the echo
#define MAX_DISTANCE 2000 // Maximum distance to measure in cm
MPU6050 mpu;

LiquidCrystal_I2C lcd(0x27, 20, 4);
static float yawAngle = 0.0;  
unsigned long lastTime = 0;
void setup() {
 Wire.begin();
  mpu.initialize();
  lcd.begin(20, 4);
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Initializing...");

  pinMode(TRIGGER_PIN, OUTPUT );
  pinMode(ECHO_PIN, INPUT );
  if (mpu.testConnection()) {
    lcd.setCursor(0, 1);
    lcd.print("Gyroscope Connected");
    } 
    else {
      lcd.setCursor(0, 1);
      lcd.print("Gyroscope Error");
      }
      lastTime = millis();
      delay(1000);
      lcd.clear();
 
  }
void loop() {
  long pulseT;
  float verticalHeight;
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);
  pulseT = pulseIn(ECHO_PIN, HIGH);
  float distance= ((pulseT*0.034)/2);
  int16_t ax, ay, az; 
  mpu.getmotion6tation(&ax, &ay, &az);
 yawAngle = atan2(ay, sqrt(ax * ax + az * az)) * 180.0 / PI;
 verticalHeight = distance * cos(yawAngle * PI / 180.0);
 float verlocity= sqrt(2*g*verticalHeight);
 lcd.print(verticalHeight);
 lcd.clear();
 lcd.setCursor(0, 0);
 lcd.print("Height: ");
 lcd.print(verticalHeight, 2);
 lcd.print(" cm");
 lcd.setCursor(0, 1);
 lcd.print(" Verlocity: ");
 lcd.print(verlocity, 2);
 lcd.print("m/s");
 lcd.setCursor(0,2);
 lcd.print(yawAngle, 2);
    
 delay(1000);
}