#include "MPU6050.h" // подключение библиотеки
MPU6050 mpu; //датчик
int16_t ax, ay, az; // результаты измерения
int16_t gx, gy, gz;

// Подключаем библиотеку для работы с 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()
{
  Wire.begin();
  mpu.initialize();
  lcd.begin(16, 2);
  lcd.clear();
  Serial.begin(9600); // для отладки
}
void loop()
{
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //четние всех 6-и осей

  Serial.print((float)gx / 32768 * 250); Serial.print('\t');
  Serial.print((float)gy / 32768 * 250); Serial.print('\t');
  Serial.println((float)gz / 32768 * 250);

  int gyrX = filter(round((float)gx / 32768 * 250), totalX, indexX, filterArrX);
  int gyrY = filter(round((float)gy / 32768 * 250), totalY, indexY, filterArrY);
  int gyrZ = filter(round((float)gz / 32768 * 250), totalZ, indexZ, filterArrZ);

  lcd.setCursor(0, 0);
  lcd.print("gyrX");
  lcd.setCursor(6, 0);
  lcd.print("gyrY ");
  lcd.setCursor(12, 0);
  lcd.print("gyrZ");
  lcd.setCursor(0, 1);
  lcd.print(gyrX);
  lcd.setCursor(6, 1);
  lcd.print(gyrY);
  lcd.setCursor(12, 1);
  lcd.print(gyrZ);
  Serial.println();
  delay(100);
  lcd.clear();
}