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

MPU6050 mpu1(0x68);
MPU6050 mpu2(0x69);
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);
int16_t ax1, ay1, az1, gx1, gy1, gz1;
int16_t ax2, ay2, az2, gx2, gy2, gz2;
int16_t diffAX, diffAY, diffAZ;
int16_t diffGX, diffGY, diffGZ;

int16_t difference(int16_t diff, int16_t a1, int16_t a2){
  diff = a1-a2;
  if(diff < 0){
    diff = diff*(-1);
  }
  return diff;
}

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

void loop(void)
{
 mpu1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);
 mpu2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);
 diffAX = difference(diffAX, ax1, ax2);
 diffAY = difference(diffAY, ay1, ay2);
 diffAZ = difference(diffAZ, az1, az2);
 diffGX = difference(diffGX, gx1, gx2);
 diffGY = difference(diffGY, gy1, gy2);
 diffGZ = difference(diffGZ, gz1, gz2);

 lcd.setCursor(0,0);
 lcd.print((float)diffAX/32768*2);
 lcd.print(" ");
 lcd.print((float)diffAY/32768*2);
 lcd.print(" ");
 lcd.print((float)diffAZ/32768*2);
 lcd.print(" ");

 lcd.setCursor(0,1);
 lcd.print((float)diffGX/32768*250);
 lcd.print(" ");
 lcd.print((float)diffGY/32768*250);
 lcd.print(" ");
 lcd.print((float)diffGZ/32768*250);
 lcd.print(" ");
}