#include "MPU6050.h" // подключение библиотеки
MPU6050 mpu1(0x68);
MPU6050 mpu2(0x69);

int16_t ax1, ay1, az1; // результаты измерения
int16_t gx1, gy1, gz1;

int16_t ax2, ay2, az2; // результаты измерения
int16_t gx2, gy2, gz2;

const int pinA0 = A0;
int pinA0State = HIGH;
// Подключаем библиотеку для работы с LCD
#include <LiquidCrystal.h>

// Инициализируем объект-экран, передаём использованные
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

const int windowSize = 10;
int indexX = windowSize - 1;
int indexY = windowSize - 1;
int indexZ = windowSize - 1;
int filterArrX[windowSize];
int filterArrY[windowSize];
int filterArrZ[windowSize];
int totalX = 0;
int totalY = 0;
int totalZ = 0;

int filter(const int& value, int& total, int& index, int* filterArr)
{
  total -= filterArr[index];
  filterArr[index] = value;
  total += filterArr[index];
  int average = total / windowSize;
  --index;
  index = (index < 0) ? windowSize - 1 : index;
  return average;
}


void setup()
{
  pinMode(pinA0, OUTPUT);
  Wire.begin();
  mpu1.initialize();
  mpu2.initialize();
  lcd.begin(16, 2);
  lcd.clear();
  digitalWrite(pinA0, pinA0State);
  Serial.begin(9600); // для отладки
}

void loop()
{
  mpu1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1); //четние всех 6-и осей
  mpu2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2); //четние всех 6-и осей

  int gyrX1, gyrY1, gyrZ1, gyrX2, gyrY2, gyrZ2;

    gyrX1 = filter(round((float)gx1 / 32768 * 250), totalX, indexX, filterArrX);
    gyrY1 = filter(round((float)gy1 / 32768 * 250), totalY, indexY, filterArrY);
    gyrZ1 = filter(round((float)gz1 / 32768 * 250), totalZ, indexZ, filterArrZ);

    gyrX2 = filter(round((float)gx2 / 32768 * 250), totalX, indexX, filterArrX);
    gyrY2 = filter(round((float)gy2 / 32768 * 250), totalY, indexY, filterArrY);
    gyrZ2 = filter(round((float)gz2 / 32768 * 250), totalZ, indexZ, filterArrZ);



  int dgyrX = abs(gyrX1 - gyrX2);
  int dgyrY = abs(gyrY1 - gyrY2);
  int dgyrZ = abs(gyrZ1 - gyrZ2);
  Serial.println(gyrX1);
  Serial.println(gyrX2);

  lcd.setCursor(0, 0);
  lcd.print("dgyrX");
  lcd.setCursor(6, 0);
  lcd.print("dyrY ");
  lcd.setCursor(12, 0);
  lcd.print("dyrZ");
  lcd.setCursor(0, 1);
  lcd.print(dgyrX);
  lcd.setCursor(6, 1);
  lcd.print(dgyrY);
  lcd.setCursor(12, 1);
  lcd.print(dgyrZ);
  Serial.println();
  delay(100);
  lcd.clear();
}