#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 servos[3];
byte servo_pins[3] = {11, 10, 9}; 

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

bool active = true;

// функция, которая не даёт значению выйти за пределы 
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);
    // инициализация MPU6050
    accelgyro.initialize();
    for(byte i = 0; i < 3; i++) {
      servos[i].attach(servo_pins[i]);
      servos[i].write(90);
    }
    pinMode(4, INPUT);
    active = (digitalRead(4) == LOW) ? false : true;
    attachInterrupt(0, attach1, RISING);
    attachInterrupt(1, attach2, RISING);
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
}

void attach1() {
  active = false;
}

void attach2() {
  active = true;
}

void loop() {
  if (active) {
    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, az,gz;

        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;
        gz = gz_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);
        az = az_raw / 4096.0;
        az = clamp(az, -1.0, 1.0);

        // вычисляем угол наклона по акселерометру
        angle_ax = 90 - TO_DEG*acos(ay);
        angle_ay = 90 - TO_DEG*acos(ax);
        angle_az = 90 - TO_DEG*acos((az + ay)/2);
        // вычисляем угол наклона по гироскопу
        angle_gx = angle_gx + gx * T_OUT/1000.0;
        angle_gy = angle_gy + gy * T_OUT/1000.0;
        angle_gz = angle_gz + gz * T_OUT/1000.0;
        // корректируем значение угла с помощью акселерометра
        angle_gx = angle_gx*(1-FK) + angle_ax*FK;
        angle_gy = angle_gy*(1-FK) + angle_ay*FK;
        angle_gz = angle_gz*(1-FK) + angle_az*FK;
        servos[0].write(90+angle_gx);
        servos[1].write(90+angle_gy);
        servos[2].write(90+angle_gz);
    }
  } else {
    for(byte i = 0; i < 3; i++) {
      servos[i].write(90);
    }
  }
}