#include <LiquidCrystal_I2C.h>

#include "Wire.h"
const int MPU_addr_1 = 0x68; // адрес датчика
const int MPU_addr_2 = 0x69; // адрес датчика

int16_t data_1[7];
int16_t data_2[7];

float data_calc[6];

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

void getData(int16_t *data, int MPU_addr){
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  for (byte i = 0; i < 7; i++) {
    data[i] = Wire.read() << 8 | Wire.read();
  }  
}

void calcData(){
  int index = 0;
  float d1;
  float d2;
  for(int i = 0; i < 7; i++){
    if(i < 3){
      d1 = (float)data_1[i] / 32768 * 2;
      d2 = (float)data_2[i] / 32768 * 2;
    }    
    else if(i == 3) {
      continue;
    }
    else if(i > 3){
      d1 = (float)data_1[i] / 32768 * 250;
      d2 = (float)data_2[i] / 32768 * 250;
    }

    if(data_1[i] >= data_2[i]){
      data_calc[index] = d1 - d2;
    }
    else{
      data_calc[index] = d2 - d1;
    }

    index++;
  }
}

void setup()
{
  Wire.begin();
  Serial.begin(9600);
  
  Wire.beginTransmission(MPU_addr_1);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU_addr_2);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("    ACCEL  |  ROTAT");
  delay(1000);
}

void loop()
{  
  const char axelsLetter[3] = {'X', 'Y', 'Z'};

  getData(data_1, MPU_addr_1);
  getData(data_2, MPU_addr_2);

  calcData();
  
  for(int i = 1; i <= 3; i++){
    lcd.setCursor(0, i);
    lcd.print(axelsLetter[i - 1]); lcd.print(":  ");
    lcd.print(data_calc[i - 1]);
    lcd.print("   |  ");
    lcd.print(data_calc[i + 2]);
  }

  delay(200);

}