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

LiquidCrystal lcd(12, 11, 10, 9, 8, 7);
MPU6050 mpu1(0x68);
MPU6050 mpu2(0x69);

void setup() {
  mpu1.initialize();
  mpu2.initialize();
  lcd.begin(16, 2);
}


void loop() {
  int16_t ax1, ay1, az1;
  mpu1.getAcceleration(&ax1, &ay1, &az1);
  int16_t ax2, ay2, az2;
  mpu2.getAcceleration(&ax2, &ay2, &az2);

  float _ax1 = static_cast<float>(ax1) / 32768 * 2;
  float _ax2 = static_cast<float>(ax2) / 32768 * 2;
  float _ay1 = static_cast<float>(ay1) / 32768 * 2;
  float _ay2 = static_cast<float>(ay2) / 32768 * 2;
  float _az1 = static_cast<float>(az1) / 32768 * 2;
  float _az2 = static_cast<float>(az2) / 32768 * 2;

  float diffX = abs(_ax1 - _ax2);
  float diffY = abs(_ay1 - _ay2);
  float diffZ = abs(_az1 - _az2);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("X:"); lcd.print(diffX);
  lcd.setCursor(8, 0);
  lcd.print("Y:"); lcd.print(diffY);
  lcd.setCursor(0, 1);
  lcd.print("Z:"); lcd.print(diffZ);

  delay(500);

}