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

MPU6050 mpu;
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);

void setup() {
  lcd.begin(16, 2);
  lcd.print("MPU6050 Demo");
  delay(1000);
  lcd.clear();

  Wire.begin();
  mpu.initialize();
}

void loop() {
  // Read raw accelerometer data
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Calculate angle deviation in X direction
  float angleX = atan2(ay, az) * 180 / M_PI;

  // Determine the direction of train wheel turning
  String direction;
  if (angleX > 0) {
    direction = "Left";
  } else {
    direction = "Right";
  }

  // Display the result on LCD screen
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Angle deviation:");
  lcd.setCursor(0, 1);
  lcd.print(angleX, 2);
  lcd.print(" degrees");
  lcd.setCursor(11, 1);
  lcd.print(direction);

  delay(100);
}