Práctica 2: Frenado Inteligente

// --- BIBLIOTECAS ---
#include <Servo.h>
#include <Stepper.h>

// --- CONFIGURACIÓN MOTOR PASO A PASO (28BYJ-48) ---
const int pasosPorVuelta = 2048; 
Stepper motorPasos(pasosPorVuelta, 8, 10, 9, 11); // Nota el orden de pines: 8, 10, 9, 11 es clave para ULN2003

// --- CONFIGURACIÓN SERVO ---
Servo servoRadar;
int posServo = 0;
int direccionServo = 1; // 1 para subir, -1 para bajar
unsigned long ultimoTiempoServo = 0;

// --- CONFIGURACIÓN SENSORES ULTRASÓNICOS ---
const int trig1 = 2, echo1 = 3; // Sensor Izquierdo
const int trig2 = 4, echo2 = 5; // Sensor Derecho
unsigned long ultimoTiempoSensor = 0;
int distanciaSeguridad = 0;

void setup() {
  Serial.begin(9600);
  
  // Iniciar Servo
  servoRadar.attach(6);
  
  // Iniciar Pines Sensores
  pinMode(trig1, OUTPUT); pinMode(echo1, INPUT);
  pinMode(trig2, OUTPUT); pinMode(echo2, INPUT);
  
  // Velocidad inicial base del stepper (RPM)
  motorPasos.setSpeed(10);
}

void loop() {
  unsigned long tiempoActual = millis();

  // --- TAREA 1: LEER SENSORES (Cada 100ms) ---
  // No leemos en cada ciclo para no saturar el procesador con esperas de eco
  if (tiempoActual - ultimoTiempoSensor > 100) {
    ultimoTiempoSensor = tiempoActual;
    
    int d1 = obtenerDistancia(trig1, echo1);
    int d2 = obtenerDistancia(trig2, echo2);
    
    // Tomamos la distancia MENOR (el objeto más cercano es el peligro)
    distanciaSeguridad = min(d1, d2);
    
    // Depuración
    Serial.print("Min Dist: ");
    Serial.println(distanciaSeguridad);
  }

  // --- TAREA 2: CONTROLAR VELOCIDAD MOTOR (Lógica de Seguridad) ---
  if (distanciaSeguridad <= 20) {
    // ZONA ROJA: Parada de Emergencia
    // No movemos el motor (saltamos el paso)
  } 
  else if (distanciaSeguridad <= 50) {
    // ZONA AMARILLA: Velocidad Lenta
    motorPasos.setSpeed(5); // 5 RPM
    motorPasos.step(5);     // Avanzar un poco
  } 
  else {
    // ZONA VERDE: Velocidad Normal/Rápida
    motorPasos.setSpeed(15); // 15 RPM (Límite seguro 28BYJ-48 cargado)
    motorPasos.step(10);     // Avanzar más pasos
  }

  // --- TAREA 3: BARRIDO DE SERVO (Radar Continuo) ---
  // Actualizar posición cada 15ms sin usar delay() bloqueante
  if (tiempoActual - ultimoTiempoServo > 15) {
    ultimoTiempoServo = tiempoActual;
    
    posServo += direccionServo;
    servoRadar.write(posServo);
    
    // Cambiar dirección al llegar a los extremos
    if (posServo >= 180 || posServo <= 0) {
      direccionServo = -direccionServo;
    }
  }
}

// Función auxiliar para limpiar y leer sensor
int obtenerDistancia(int trig, int echo) {
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  
  long duracion = pulseIn(echo, HIGH, 30000); // Timeout de 30ms para no bloquear si no hay eco
  if (duracion == 0) return 999; // Si no hay lectura, asumimos lejos
  return duracion * 0.034 / 2;
}