//Вычисление угла по Акселерометру
//https://robotclass.ru/tutorials/arduino-accelerometer-mpu6050/
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>

#define TO_DEG 57.29577951308232087679815481410517033f
#define T_OUT 20

MPU6050 accel;
Servo servo1;
Servo servo2;

float angle_ax, angle_ay;
long int t_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);
  accel.initialize(); // первичная настройка датчика
  Serial.println(accel.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
}

void loop() {
  long int t = millis();
  if ( t_next < t ) {
    int16_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw;
    float ay, gx, ax;

    t_next = t + T_OUT;
    accel.getMotion6(&ax_raw, &ay_raw, &az_raw, &gx_raw, &gy_raw, &gz_raw);

    // сырые данные акселерометра нужно преобразовать в единицы гравитации
    // при базовых настройках 1G = 4096
    ay = ay_raw / 4096.0;
    ax = ax_raw / 4096.0;
    // на случай, если на акселерометр действуют посторонние силы, которые могут
    // увеличить ускорение датчика свыше 1G, установим границы от -1G до +1G
    ay = clamp(ay, -1.0, 1.0);
    ax = clamp(ax, -1.0, 1.0);
    // функция acos возвращает угол в радианах, так что преобразуем
    // его в градусы при помощи коэффициента TO_DEG

    //Поворот вокруг оси X
    if ( ay >= 0) {
      angle_ax = 180 - TO_DEG * acos(ay);
    } else {
      angle_ax = TO_DEG * acos(-ay) - 180;
    }
    Serial.print("X ");
    Serial.print(angle_ax); // вывод в порт угла поворота вокруг оси X
    servo1.write(angle_ax);

    //Поворот вокруг оси Y
    if ( ax >= 0) {
      angle_ay = 180 - TO_DEG * acos(ax);
    } else {
      angle_ay = TO_DEG * acos(-ax) - 180;
    }
    Serial.print("  Y ");
    Serial.println(angle_ay); // вывод в порт угла поворота вокруг оси Y
    servo2.write(angle_ay);
  }
  delay(50);
}