#define DIR_PINA 2
#define STEP_PINA 3
#define DIR_PINB 4
#define STEP_PINB 5
#define TRIGA 6
#define ECHA 7
#define TRIGB 8
#define  ECHB 9

void setup() {
  pinMode(STEP_PINA, OUTPUT);
  pinMode(DIR_PINA, OUTPUT);
  digitalWrite(STEP_PINA, LOW);
  pinMode(STEP_PINB, OUTPUT);
  pinMode(DIR_PINB, OUTPUT);
  digitalWrite(STEP_PINB, LOW);
  pinMode(TRIGA, OUTPUT);
  pinMode(TRIGB, OUTPUT);
  pinMode(ECHA, INPUT);
  pinMode(ECHB, INPUT);
  Serial1.begin(115200);
}

void loop() {
  int distancia1 = obtenerDistancia(TRIGA, ECHA);
  int distancia2 = obtenerDistancia(TRIGB, ECHB);

  // Realizar acciones basadas en las distancias
  if (distancia1 > 20 && distancia2 > 20) {
    avanzar();
  } else if (distancia1 <= 20) {
   
    girarDerecha();
  } else if (distancia2 <= 20) {
   
    girarIzquierda();
  }
  Serial1.print("Distancia1: ");
  Serial1.print(distancia1);
  Serial1.println(" cm"); 
   
  Serial1.print("Distancia2: ");
  Serial1.print(distancia2);
  Serial1.println(" cm");


  delay(100);
}

// Función para obtener la distancia 
int obtenerDistancia(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  int duracion = pulseIn(echoPin, HIGH);
  int distancia = duracion * 0.034 / 2;

  return distancia;
}

void avanzar() {
  digitalWrite(DIR_PINA, HIGH);
  digitalWrite(DIR_PINB, HIGH);
  digitalWrite(STEP_PINA, HIGH);
  digitalWrite(STEP_PINB, HIGH);
  delay(5);  
  digitalWrite(STEP_PINA, LOW);
  digitalWrite(STEP_PINB, LOW);
  delay(5);  
}


void girarIzquierda() {
  digitalWrite(DIR_PINA, HIGH);
  digitalWrite(DIR_PINB, LOW);
  digitalWrite(STEP_PINA, HIGH);
  digitalWrite(STEP_PINB, HIGH);
  delay(5);  
  digitalWrite(STEP_PINA, LOW);
  digitalWrite(STEP_PINB, LOW);
  delay(5);  
}


void girarDerecha() {
  digitalWrite(DIR_PINA, LOW);
  digitalWrite(DIR_PINB, HIGH);
  digitalWrite(STEP_PINA, HIGH);
  digitalWrite(STEP_PINB, HIGH);
  delay(5);  
  digitalWrite(STEP_PINA, LOW);
  digitalWrite(STEP_PINB, LOW);
  delay(5);  
}