P5: ROBOT EXPLORADOR

// P5: Robot Explorador - EigenRobotics

// --- Pines Motor Izquierdo ---
const int ENA = 5;   // PWM velocidad
const int IN1 = 4;   // Direccion
const int IN2 = 3;   // Direccion

// --- Pines Motor Derecho ---
const int ENB = 6;   // PWM velocidad
const int IN3 = 8;   // Direccion
const int IN4 = 7;   // Direccion

// --- Pines Sensor ---
const int TRIG = 10;
const int ECHO = 9;

// --- Configuracion ---
const int VELOCIDAD = 150;      // 0-255
const int DISTANCIA_MIN = 20;   // cm
const int TIEMPO_GIRO = 400;    // ms

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

  // Configurar pines de sensor
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  Serial.begin(9600);
  Serial.println("Robot Explorador listo!");

  delay(2000);  // Esperar antes de arrancar
}

void loop() {
  int distancia = medirDistancia();

  Serial.print("Distancia: ");
  Serial.print(distancia);
  Serial.println(" cm");

  if (distancia > DISTANCIA_MIN || distancia == 0) {
    avanzar();
  } else {
    parar();
    delay(200);
    girarDerecha();
    delay(TIEMPO_GIRO);
    parar();
    delay(100);
  }

  delay(50);
}

// ========== FUNCIONES DE MOVIMIENTO ==========

void avanzar() {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(ENA, VELOCIDAD);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENB, VELOCIDAD);
}

void retroceder() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  analogWrite(ENA, VELOCIDAD);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  analogWrite(ENB, VELOCIDAD);
}

void girarDerecha() {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(ENA, VELOCIDAD);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  analogWrite(ENB, VELOCIDAD);
}

void girarIzquierda() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  analogWrite(ENA, VELOCIDAD);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENB, VELOCIDAD);
}

void parar() {
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

// ========== FUNCION DE SENSOR ==========

int medirDistancia() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  long duracion = pulseIn(ECHO, HIGH, 30000);

  if (duracion == 0) return 0;

  return duracion / 58;
}