Cpp
#include <Wire.h>

#define TRIG_PIN 2
#define ECHO_PIN 3
#define MOTOR_LEFT_PIN 4
#define MOTOR_RIGHT_PIN 5
#define DISTANCE_THRESHOLD 20  // Cambiar dirección si el obstáculo está a menos de 20 cm

long duration;
int distance;

void setup() {
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
    pinMode(MOTOR_LEFT_PIN, OUTPUT);
    pinMode(MOTOR_RIGHT_PIN, OUTPUT);
    Serial.begin(9600);
}

void loop() {
    // Enviar una señal de 10us a Trig
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    
    // Leer la duración del eco
    duration = pulseIn(ECHO_PIN, HIGH);
    
    // Calcular la distancia
    distance = (duration / 2) / 29.1;
    
    if (distance < DISTANCE_THRESHOLD) {
        // Cambiar dirección si hay un obstáculo cercano
        digitalWrite(MOTOR_LEFT_PIN, LOW);  // Motor izquierdo hacia atrás o detenerse
        digitalWrite(MOTOR_RIGHT_PIN, HIGH); // Motor derecho sigue adelante, gira a la derecha
    } else {
        // Avanzar si no hay obstáculo
        digitalWrite(MOTOR_LEFT_PIN, HIGH);
        digitalWrite(MOTOR_RIGHT_PIN, HIGH);
    }
    
    Serial.print("Distancia: ");
    Serial.println(distance);
    delay(100);
}

• Pins:
  • TRIG_PIN se conecta al pin de disparo del HC-SR04.
  • ECHO_PIN se conecta al pin de eco del HC-SR04.
  • MOTOR_LEFT_PIN y MOTOR_RIGHT_PIN controlan los motores del robot.