#include <Stepper.h>
#include <Servo.h>

// Definimos los pasos por vuelta para los motores 28BYJ-48
const int pasosPorVuelta = 2048; 

// Configuración de pines según tu estructura de la ULA
Stepper motorIzq(pasosPorVuelta, 2, 4, 3, 5);
Stepper motorDer(pasosPorVuelta, 6, 8, 7, 9);
Servo marcador;

const int trigPin = 12;
const int echoPin = 13;

void setup() {
  marcador.attach(10);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  // Velocidad inicial para los motores paso a paso
  motorIzq.setSpeed(10); 
  motorDer.setSpeed(10);
}

void loop() {
  long duracion, distancia;
  
  // Lanzar pulso ultrasónico
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  duracion = pulseIn(echoPin, HIGH);
  distancia = (duracion / 2) / 29.1;

  if (distancia > 15 || distancia == 0) {
    marcador.write(90); // Marcador abajo (dibujando)
    motorIzq.step(10);
    motorDer.step(10);
  } else {
    marcador.write(0);  // Sube marcador antes de chocar
    delay(500);
    // Giro para esquivar
    motorIzq.step(-500); 
    motorDer.step(500);
  }
}