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

#define PIN 7
MPU6050 mpu;

int ax, ay, az;
int gx, gy, gz;

long tiempo_prev;
float dt;
float ang_x, ang_y;
float ang_x_prev, ang_y_prev;


void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  if(mpu.testConnection()){
    Serial.println("Se conecto correctamente");
  } else {
    Serial.println("Error al conectar MPU");
  }

}

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

  // Calcular el ángulo de inclinación en el eje Y
  float angle_y = atan2(-ax, sqrt(ay * ay + az * az)) * (180.0 / 3.14);

  // Imprimir el ángulo en grados
  Serial.print("Ángulo en Y: ");
  Serial.println(angle_y);
  delay(100);
}

void updateFiltered()
{
  dt = (millis() - tiempo_prev) / 1000.0;
  tiempo_prev = millis();

  //Calcular los ángulos con acelerometro
  float accel_ang_x = atan(ay / sqrt(pow(ax, 2) + pow(az, 2)))*(180.0 / 3.14);
  float accel_ang_y = atan(-ax / sqrt(pow(ay, 2) + pow(az, 2)))*(180.0 / 3.14);

  //Calcular angulo de rotación con giroscopio y filtro complementario
  ang_x = 0.98*(ang_x_prev + (gx / 131)*dt) + 0.02*accel_ang_x;
  ang_y = 0.98*(ang_y_prev + (gy / 131)*dt) + 0.02*accel_ang_y;

  ang_x_prev = ang_x;
  ang_y_prev = ang_y;
}