#include <Wire.h>
#include <MPU6050.h>
const int PinIN1 = 7;
const int PinIN2 = 6;

MPU6050 mpu;
//scl a5
//sda a4
void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  
  // Verifica si el MPU6050 está conectado correctamente
  if (mpu.testConnection()) {
    Serial.println("MPU6050 conectado correctamente.");
  } else {
    Serial.println("MPU6050 no se pudo conectar.");
  }
  pinMode(PinIN1, OUTPUT);
  pinMode(PinIN2, OUTPUT);

}

/*
OSCILACIONES 0 del giroscopio:
-20 < gx < 20
-10 < gy < 40
-30 < gz < 30
*/
void loop() {
  // Variables para almacenar los valores del acelerómetro y giroscopio
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  
  // Lee los valores del acelerómetro y giroscopio
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  // Factor de escala para ±2g
  float factor_escala = 16384.0;

  // Convertir las lecturas a g
  float ax_g = ax / factor_escala;
  float ay_g = ay / factor_escala;
  float az_g = az / factor_escala;

  // Convertir de g a m/s^2
  float ax_ms2 = ax_g * 9.81;
  float ay_ms2 = ay_g * 9.81;
  float az_ms2 = az_g * 9.81;

  Serial.print("ax="); Serial.print(ax_ms2);
  Serial.print(" ay="); Serial.print(ay_ms2);
  Serial.print(" az="); Serial.print(az_ms2);
  Serial.print("| gx="); Serial.print(gx);
  Serial.print(" gy="); Serial.print(gy);
  Serial.print(" gz="); Serial.println(gz);
  
  delay(1000);
  //condiciones para que el motor se apague, gire para un lado u otro
  // Condiciones para el control del motor basado en las lecturas del giroscopio
  if (gx > 20) {
    MotorHorario(); // Si gx es mayor a 20, gira el motor en sentido horario
  } 
  else if (gx < -20) {
    MotorAntihorario(); // Si gx es menor a -20, gira el motor en sentido antihorario
  } 
  else {
    MotorStop(); // Si gx está entre -20 y 20, detén el motor
  }
}

void MotorHorario()
{
  digitalWrite (PinIN1, HIGH);
  digitalWrite (PinIN2, LOW);
}
//función para girar el motor en sentido antihorario
void MotorAntihorario()
{
  digitalWrite (PinIN1, LOW);
  digitalWrite (PinIN2, HIGH);
}

//función para apagar el motor
void MotorStop()
{
  digitalWrite (PinIN1, LOW);
  digitalWrite (PinIN2, LOW);
}