// Rescue Rover: sense, decide, act, and log each reading for the debug record
// STOP button D2 to GND (INPUT_PULLUP); HC-SR04 Trig D9, Echo D10
// TB6612FNG: STBY D3~; left PWMA D5~, AIN1 D4, AIN2 D7; right PWMB D6~, BIN1 D12, BIN2 D11
// A reading every 60 ms, timed with millis()
// Serial Monitor at 9600 baud: one line per distance reading, in cm
const int stopPin = 2;                        // STOP button to GND: LOW = pressed
const int trigPin = 9;
const int echoPin = 10;
const int stbyPin = 3;
const int pwmAPin = 5;                        // left motor (A)
const int ain1Pin = 4;
const int ain2Pin = 7;
const int pwmBPin = 6;                        // right motor (B)
const int bin1Pin = 12;
const int bin2Pin = 11;

const int DRIVE = 0;                          // the rover's states
const int BRAKE = 1;
const int TURN = 2;
const int STOPPED = 3;
int state = DRIVE;                            // what the rover is doing now

const unsigned long readEvery = 60;           // ms between distance readings
const int stopCm = 20;
const unsigned long brakeMs = 300;
const unsigned long turnMs = 600;
unsigned long lastRead = 0;                   // millis() at the last reading
unsigned long stateStart = 0;                 // millis() when this state began

void leftMotor(int in1, int in2, int speed) {
  digitalWrite(ain1Pin, in1);
  digitalWrite(ain2Pin, in2);
  analogWrite(pwmAPin, speed);
}

void rightMotor(int in1, int in2, int speed) {
  digitalWrite(bin1Pin, in1);
  digitalWrite(bin2Pin, in2);
  analogWrite(pwmBPin, speed);
}

unsigned long readEcho() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);                // a 10 µs pulse starts a reading
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  return pulseIn(echoPin, HIGH, 30000);       // µs; 0 if no echo within 30 ms
}

void setup() {
  pinMode(stopPin, INPUT_PULLUP);             // HIGH until pressed
  Serial.begin(9600);                         // the Serial Monitor must match
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(stbyPin, OUTPUT);
  pinMode(ain1Pin, OUTPUT);
  pinMode(ain2Pin, OUTPUT);
  pinMode(bin1Pin, OUTPUT);
  pinMode(bin2Pin, OUTPUT);
  digitalWrite(stbyPin, HIGH);                // wake the driver up
}

void loop() {
  unsigned long now = millis();               // ms since the program started
  bool close = false;
  if (now - lastRead >= readEvery) {          // safe when millis() rolls over
    lastRead = now;
    unsigned long duration = readEcho();      // waits in pulseIn() for the echo
    float distance = duration * 0.0343 / 2;
    close = duration > 0 && distance <= stopCm; // 0 is no reading, never close
    Serial.println(distance);                 // cm, for the debug record
  }
  if (digitalRead(stopPin) == LOW) state = STOPPED; // STOP wins: checked on every pass, first
  switch (state) {                            // decide: run the case for the state
    case DRIVE:
      leftMotor(HIGH, LOW, 128);              // act: both forward
      rightMotor(HIGH, LOW, 128);
      if (close) {
        state = BRAKE;
        stateStart = now;
      }
      break;
    case BRAKE:
      leftMotor(HIGH, HIGH, 0);               // short brake
      rightMotor(HIGH, HIGH, 0);
      if (now - stateStart >= brakeMs) {
        state = TURN;
        stateStart = now;
      }
      break;
    case TURN:
      leftMotor(LOW, HIGH, 110);              // spin left
      rightMotor(HIGH, LOW, 110);
      if (now - stateStart >= turnMs) state = DRIVE;
      break;
    case STOPPED:
      leftMotor(HIGH, HIGH, 0);               // stays here until RESET
      rightMotor(HIGH, HIGH, 0);
      break;
  }
}

Press ▶ Run.