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

const int PinIN1 = 7;
const int PinIN2 = 6;

MPU6050 mpu;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  
  if (mpu.testConnection()) {
    Serial.println("MPU6050 conectado correctamente.");
  } else {
    Serial.println("MPU6050 no se pudo conectar.");
  }
  pinMode(PinIN1, OUTPUT);
  pinMode(PinIN2, OUTPUT);
}

void loop() {
  // Variables para almacenar los valores del acelerómetro y giroscopio
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  
  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;

  // Mapear los valores del giroscopio a un rango de 0 a 255
  int mappedGx = map(gx, -32768, 32767, 0, 255);
  int mappedGy = map(gy, -32768, 32767, 0, 255);
  int mappedGz = map(gz, -32768, 32767, 0, 255);

  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(" mappedGx="); Serial.print(mappedGx);
  Serial.print(" gy="); Serial.print(gy);
  Serial.print(" mappedGy="); Serial.print(mappedGy);
  Serial.print(" gz="); Serial.print(gz);
  Serial.print(" mappedGz="); Serial.println(mappedGz);
  
  delay(100);

  // Condiciones para el control del motor basado en las lecturas mapeadas
  if (mappedGx > 128) { // Si gx mapeado es mayor a 128, gira el motor en sentido horario
    MotorHorario();
  } 
  else if (mappedGx < 126) { // Si gx mapeado es menor a 128, gira el motor en sentido antihorario
    MotorAntihorario();
  } 
  else {
    MotorStop(); // Si gx mapeado está cerca de 128, detén el motor
  }
}

void MotorHorario() {
  digitalWrite(PinIN1, HIGH);
  digitalWrite(PinIN2, LOW);
}

void MotorAntihorario() {
  digitalWrite(PinIN1, LOW);
  digitalWrite(PinIN2, HIGH);
}

void MotorStop() {
  digitalWrite(PinIN1, LOW);
  digitalWrite(PinIN2, LOW);
}