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

MPU6050 mpu;

// Constantes para la integración
const float dT = 0.02;     // Tiempo de muestreo (segundos)
const float alpha = 0.9;   // Factor de suavizado

// Variables para la integración
float roll = 0, pitch = 0, yaw = 0;   // Ángulos de Euler
float roll_acc = 0, pitch_acc = 0;   // Ángulos obtenidos de la aceleración
float roll_gyro = 0, pitch_gyro = 0, yaw_gyro = 0;   // Velocidades angulares
float roll_last = 0, pitch_last = 0, yaw_last = 0;   // Valores previos de velocidad angular
int16_t ax, ay, az, gx, gy, gz;   // Declaración de las variables para la lectura del giroscopio
void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
}

void loop() {
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);   // Lectura de datos del giroscopio

  // Cálculo de las velocidades angulares
  roll_gyro = gx / 131.0;
  pitch_gyro = gy / 131.0;
  yaw_gyro = gz / 131.0;

  // Integración de la velocidad angular
  roll += (roll_gyro + roll_last) * dT / 2.0;
  pitch += (pitch_gyro + pitch_last) * dT / 2.0;
  yaw += (yaw_gyro + yaw_last) * dT / 2.0;

  // Suavizado de los ángulos de Euler
  roll = alpha * roll + (1 - alpha) * roll_acc;
  pitch = alpha * pitch + (1 - alpha) * pitch_acc;

  // Obtención de los ángulos a partir de la aceleración
  roll_acc = atan2(ay, az) * 180.0 / PI;
  pitch_acc = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / PI;

  // Impresión de los valores de los ángulos de Euler
  Serial.print("Roll: ");
  Serial.print(roll);
  Serial.print(", Pitch: ");
  Serial.print(pitch);
  Serial.print(", Yaw: ");
  Serial.println(yaw);
  Serial.print("gx: ");
  Serial.print(gx);
  Serial.print(", gy: ");
  Serial.print(gy);
  Serial.print(", gz: ");
  Serial.println(gz);

  // Actualización de los valores previos de velocidad angular
  roll_last = roll_gyro;
  pitch_last = pitch_gyro;
  yaw_last = yaw_gyro;

  delay(dT * 1000);
}