#include <Wire.h>
#include <Servo.h>
#include <MPU6050.h>

Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
MPU6050 mpu;

int16_t ax, ay, az, gx, gy, gz;
float roll, pitch;

void setup() {
  Wire.begin();
  mpu.initialize();

  myservo1.attach(8);  // Anexa o servo ao pino 8
  myservo2.attach(9);  // Anexa o servo ao pino 9
  myservo3.attach(10); // Anexa o servo ao pino 10
  myservo4.attach(11); // Anexa o servo ao pino 11
  Serial.begin(9600);
}

void loop() {
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Cálculos de ângulo usando acelerômetro (roll e pitch)
  roll = atan2(gy, gz) * RAD_TO_DEG; // Calcula o ângulo de roll com base no acelerômetro
  pitch = atan2(gx, sqrt(gy * gy + gz * gz)) * RAD_TO_DEG; // Calcula o ângulo de pitch com base no acelerômetro

  Serial.println(roll);
  Serial.println(pitch);
  // Use os ângulos calculados para controlar os servos das grid fins
  int servoAngle1 = map(roll, 0, 1023, 0, 180); // Mapeia o ângulo de roll para o servo 1
  int servoAngle2 = map(roll, 0, 1023, 0, 180); // Mapeia o ângulo de roll para o servo 2
  int servoAngle3 = map(pitch, 0, 1023, 0, 180); // Mapeia o ângulo de pitch para o servo 3
  int servoAngle4 = map(pitch, 0, 1023, 0, 180); // Mapeia o ângulo de pitch para o servo 4

  myservo1.write(servoAngle1); // Move o servo 1 para a posição calculada
  myservo2.write(servoAngle2); // Move o servo 2 para a posição calculada
  myservo3.write(servoAngle3); // Move o servo 3 para a posição calculada
  myservo4.write(servoAngle4); // Move o servo 4 para a posição calculada

  delay(15);  // Espera de 15 ms para evitar atualizações muito rápidas
}