//Комплементарный фильтр
//https://robotclass.ru/articles/complementary-filter/
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>

#define TO_DEG 57.29577951308232087679815481410517033f
#define T_OUT 20 // каждый 20 миллисекунд будем проводить вычисления 
#define P_OUT 50 // каждый 50 миллисекунд будем выводить данные
#define FK 0.1 // коэффициент комплементарного фильтра

MPU6050 accelgyro;
Servo servo1;
Servo servo2;

float angle_ax, angle_gx, angle_cpl, angle_ay, angle_gy;
int dt = 0;
long int t_next, p_next;

// функция, которая не даёт значению выйти за пределы
float clamp(float v, float minv, float maxv) {
  if ( v > maxv )
    return maxv;
  else if ( v < minv )
    return minv;
  return v;
}

void setup() {
  Serial.begin(9600);
  servo1.attach(11);
  servo2.attach(10);
  // инициализация MPU6050
  accelgyro.initialize();
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
}

void loop() {
  long int t = millis();
  // каждые T_TO миллисекунд выполняем рассчет угла наклона
  if ( t_next < t ) {
    int16_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw;
    float ay, gx, ax, gy;

    t_next = t + T_OUT;
    // получаем сырые данные от датчиков в MPU6050
    accelgyro.getMotion6(&ax_raw, &ay_raw, &az_raw, &gx_raw, &gy_raw, &gz_raw);

    // преобразуем сырые данные гироскопа в град/сек
    gx = gx_raw / 16.4;
    gy = gy_raw / 16.4;
    // преобразуем сырые данные акселерометра в G
    ay = ay_raw / 4096.0;
    ay = clamp(ay, -1.0, 1.0);
    ax = ax_raw / 4096.0;
    ax = clamp(ax, -1.0, 1.0);

    // вычисляем угол наклона по акселерометру
    angle_ax = 90 - TO_DEG * acos(ay);
    angle_ay = 90 - TO_DEG * acos(ax);
    // вычисляем угол наклона по гироскопу
    angle_gx = angle_gx + gx * T_OUT / 1000.0;
    angle_gy = angle_gy + gy * T_OUT / 1000.0;
    // корректируем значение угла с помощью акселерометра
    angle_gx = angle_gx * (1 - FK) + angle_ax * FK;
    angle_gy = angle_gy * (1 - FK) + angle_ay * FK;
  }

  t = millis();
  // каждые P_OUT миллисекунд выводим результат в COM порт
  if ( p_next < t ) {
    p_next = t + P_OUT;
    Serial.print("X ");
    Serial.print(angle_gx);
    servo1.write( map(angle_gx, -90, 90, 0, 180) );

    Serial.print("  Y ");
    Serial.println(angle_gy);
    servo2.write( map(angle_gy, -90, 90, 0, 180) );
  }
  delay(50); //Нужно закоментировать. Это фишка для отладки в эмуляторе WOKWI
}