3.3 Iteracion y convergencia de sistemas de ecuaciones

 // Definir pines para los sensores ultrasónicos

int triggerPin1 = 2;

int echoPin1 = 3;

int triggerPin2 = 4;

int echoPin2 = 5;

int triggerPin3 = 6;

int echoPin3 = 7;

int triggerPin4 = 8;

int echoPin4 = 9;

// Definir pines para los motores del carrito

int motorIzquierdoA = 10;

int motorIzquierdoB = 11;

int motorDerechoA = 12;

int motorDerechoB = 13;

void setup() {

  // Configurar pines como entrada o salida

  pinMode(triggerPin1, OUTPUT);

  pinMode(echoPin1, INPUT);

  pinMode(triggerPin2, OUTPUT);

  pinMode(echoPin2, INPUT);

  pinMode(triggerPin3, OUTPUT);

  pinMode(echoPin3, INPUT);

  pinMode(triggerPin4, OUTPUT);

  pinMode(echoPin4, INPUT);

  pinMode(motorIzquierdoA, OUTPUT);

  pinMode(motorIzquierdoB, OUTPUT);

  pinMode(motorDerechoA, OUTPUT);

  pinMode(motorDerechoB, OUTPUT);

}

void loop() {

  // Lógica de esquivar obstáculos

  if (distanciaSensor(triggerPin1, echoPin1) < 20 || distanciaSensor(triggerPin2, echoPin2) < 20 ||

      distanciaSensor(triggerPin3, echoPin3) < 20 || distanciaSensor(triggerPin4, echoPin4) < 20) {

    // Si un objeto está cerca, gira

    girar();

  } else {

    // Si no hay obstáculos, avanza

    avanzar();

  }

}

// Función para medir la distancia con un sensor ultrasónico

int distanciaSensor(int triggerPin, int echoPin) {

  digitalWrite(triggerPin, LOW);

  delayMicroseconds(2);

  digitalWrite(triggerPin, HIGH);

  delayMicroseconds(10);

  digitalWrite(triggerPin, LOW);

  return pulseIn(echoPin, HIGH) * 0.034 / 2;

}

// Funciones de control de motores

void avanzar() {

  digitalWrite(motorIzquierdoA, HIGH);

  digitalWrite(motorIzquierdoB, LOW);

  digitalWrite(motorDerechoA, HIGH);

  digitalWrite(motorDerechoB, LOW);

}

void girar() {

  digitalWrite(motorIzquierdoA, LOW);

  digitalWrite(motorIzquierdoB, HIGH);

  digitalWrite(motorDerechoA, HIGH);

  digitalWrite(motorDerechoB, LOW);

}