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


const int MPU_ADDR_1 = 0x68;
const int MPU_ADDR_2 = 0x69;

LiquidCrystal_I2C lcd(0x27, 16, 2); 

const float scaleFactor = 9.81 / 16384.0; 

void setup() {
  lcd.begin(16, 2);
  lcd.backlight();
  lcd.print("Initializing...");

  Wire.begin();

  
  Wire.beginTransmission(MPU_ADDR_1);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  
  Wire.beginTransmission(MPU_ADDR_2);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  lcd.clear();
  lcd.print("MPU6050 Ready");
  delay(1000);
  lcd.clear();
}

void loop() {
  int16_t ax1, ay1, az1;
  int16_t ax2, ay2, az2;

  readMPU6050(MPU_ADDR_1, ax1, ay1, az1);

  readMPU6050(MPU_ADDR_2, ax2, ay2, az2);

  float ax1_mps2 = ax1 * scaleFactor;
  float ay1_mps2 = ay1 * scaleFactor;
  float az1_mps2 = az1 * scaleFactor;

  float ax2_mps2 = ax2 * scaleFactor;
  float ay2_mps2 = ay2 * scaleFactor;
  float az2_mps2 = az2 * scaleFactor;

  
  float diffX = abs(ax1_mps2 - ax2_mps2);
  float diffY = abs(ay1_mps2 - ay2_mps2);
  float diffZ = abs(az1_mps2 - az2_mps2);

  lcd.setCursor(0, 0);
  lcd.print("dX:");
  lcd.print(diffX, 2); 
  lcd.print(" dY:");
  lcd.print(diffY, 2);

  lcd.setCursor(0, 1);
  lcd.print("dZ:");
  lcd.print(diffZ, 2);

  delay(500); 
}

void readMPU6050(int address, int16_t &ax, int16_t &ay, int16_t &az) {
  Wire.beginTransmission(address);
  Wire.write(0x3B); 
  Wire.endTransmission(false);
  Wire.requestFrom(address, 6, true);

  ax = Wire.read() << 8 | Wire.read();
  ay = Wire.read() << 8 | Wire.read();
  az = Wire.read() << 8 | Wire.read();
}