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

MPU6050 mpu;
Servo servo;

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

  // Testa conexão com MPU6050
  if (mpu.testConnection()) {
    Serial.println("MPU6050 conectado com sucesso!");
  } else {
    Serial.println("Falha ao conectar ao MPU6050.");
    while (1); // Para o código caso falhe
  }

  // Configuração do servo
  servo.attach(9); // Servo no pino 9
  servo.write(90); // Inicia o servo no meio (90 graus)
}

void loop() {
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Calcula inclinação com base nos eixos
  float anguloX = atan2(ay, az) * 180 / PI;
  
  // Ajusta o servo de acordo com o ângulo
  int anguloServo = map(anguloX, -90, 90, 0, 180); // Mapeia de -90 a 90 graus para o servo
  anguloServo = constrain(anguloServo, 0, 180); // Limita para 0-180 graus
  servo.write(anguloServo);

  // Exibe dados no console
  Serial.print("Aceleração X: "); Serial.println(ax);
  Serial.print("Inclinação X (graus): "); Serial.println(anguloX);
  Serial.println("---------");
  
  delay(500); // Aguarda 500ms
}