//librerias a importar
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

MPU6050 sensor; //ni idea

int ax,ay,az; //acelerometro mas eje
int gx, gy, gz;
long tiempo_prev;
float dt;
float ang_x, ang_y;
float ang_x_prev, ang_y_prev;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Wire.begin();
  sensor.initialize();

  if (sensor.testConnection()) {
      Serial.println("El Sensor se a Iniciado Correctamente");
  }
  else {Serial.println("Error al iniciar el Sensor");}

  tiempo_prev = millis(); // Inicializar tiempo_prev
}

void loop() {
  sensor.getAcceleration(&ax,ay,az);
  sensor.getRotation(&gx, &gy, &gz);
  /* ACELEROMETRO
  //calculo de los angulos de inclinacion:
  float accel_ang_x=atan(ax/sqrt(pow(ay,2) + pow(az,2)))*(180.0/3.14);
  float accel_ang_y=atan(ay/sqrt(pow(ax,2) + pow(az,2)))*(180.0/3.14);
  //mostrar
  Serial.print("Inclinacion en X: ");
  Serial.println(accel_ang_x);
  Serial.print("Inclinacion en Y: ");
  Serial.println(accel_ang_y);
  delay(500);
  */


  //Aceleracion y giroscopio

  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 complemento  
  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;

  //Mostrar los angulos separadas por un [tab]

  Serial.print("Rotacion en X:  ");
  Serial.print(ang_x); 
  Serial.print("tRotacion en Y: ");
  Serial.println(ang_y);

  delay(2000);
}