#include <Wire.h>
#include <MPU6050.h>
#include <LiquidCrystal.h>

MPU6050 mpu;
LiquidCrystal lcd(12, 11, 5, 4, 3, 2); // Adjust the LCD pin connections as per your setup

void setup() {
  Serial.begin(9600);
  lcd.begin(16, 2);
  
  Wire.begin();
  mpu.initialize();
  

  lcd.print("Motion Capture");
  lcd.setCursor(0, 1);
  lcd.print("Using MPU-6050");
  delay(2000);
  lcd.clear();
}

void loop() {
  // Read accelerometer and gyroscope data
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Calculate roll, pitch, and yaw angles
  float roll = atan2(ay, az)*(180.0 / PI); 
  float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * (180.0 / PI);
  float yaw = atan2(gy, gz) * (180.0 / PI);

  // Print the angles on the LCD display
  lcd.setCursor(0, 0);
  lcd.print("Roll: ");
  lcd.print(roll);
  
  lcd.setCursor(0, 1);
  lcd.print("Pitch: ");
  lcd.print(pitch);

  // Print the angles on the Serial Monitor
  Serial.print("Roll: ");
  Serial.print(roll);
  Serial.print("\tPitch: ");
  Serial.print(pitch);
  Serial.print("\tYaw: ");
  Serial.println(yaw);

  delay(2);  // Adjust the delay as per your requirements
}