// DEFINICIÓN DEL PROGRAMA:
// -Se utiliza el controlador L298N para manejar los dos motores en avance
// -Los tres sensores miden la distancia frente a cada uno
// -Si algunno de los sensores detecta un obstáculo (distancia menor a 20cm), 
// los motores se detienen. De lo contrario, los motores siguen avanzando

// Definir pines del L298N para el control de los motores
const int IN1 = 8;   // Motor 1
const int IN2 = 9;   // Motor 1
const int ENA = 10;  // Velocidad Motor 1

const int IN3 = 11;  // Motor 2
const int IN4 = 12;  // Motor 2
const int ENB = 13;  // Velocidad Motor 2

// Definir pines de los sensores ultrasónicos
const int trigPin1 = 2;
const int echoPin1 = 3;
const int trigPin2 = 4;
const int echoPin2 = 5;
const int trigPin3 = 6;
const int echoPin3 = 7;

// Función para medir la distancia con los sensores ultrasónicos
long medirDistancia(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duracion = pulseIn(echoPin, HIGH);
  long distancia = duracion * 0.034 / 2;  // Convertir tiempo en distancia (cm)
  return distancia;
}

void setup() {
  // Configurar pines de los motores como salida
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENA, OUTPUT);
  
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENB, OUTPUT);

  // Configurar pines de los sensores ultrasónicos
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  
  Serial.begin(9600);
}

void loop() {
  long distancia1 = medirDistancia(trigPin1, echoPin1);
  long distancia2 = medirDistancia(trigPin2, echoPin2);
  long distancia3 = medirDistancia(trigPin3, echoPin3);

  Serial.print("Distancia Sensor 1: ");
  Serial.println(distancia1);
  Serial.print("Distancia Sensor 2: ");
  Serial.println(distancia2);
  Serial.print("Distancia Sensor 3: ");
  Serial.println(distancia3);

  // Control de motores según las distancias
  if (distancia1 < 20 || distancia2 < 20 || distancia3 < 20) {
    // Si algún sensor detecta un obstáculo cercano, detener los motores
    detenerMotores();
  } else {
    // Si no hay obstáculos, avanzar
    moverAdelante();
  }
  
  delay(500);  // Pequeña pausa para estabilizar lecturas
}

void moverAdelante() {
  // Motor 1 adelante
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(ENA, 255);  // Velocidad máxima

  // Motor 2 adelante
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENB, 255);  // Velocidad máxima
}

void detenerMotores() {
  // Detener motor 1
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);

  // Detener motor 2
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}