Wall-following maze robot with three ultrasonic sensors (Arduino Nano)

// Wall-following maze robot — Arduino Nano + 3× HC-SR04 + TB6612FNG + 2 N20 gear motors
// Sonars (Trig/Echo): left D2/D3, front D7/D8, right A1/A2, all at 5V.
// TB6612FNG: PWMA D5, AIN1 D9, AIN2 D10 (left), PWMB D6, BIN1 D11, BIN2 D12 (right), STBY D4,
// VCC 5V, VM from the 2S Li-ion pack. It keeps a wall on its LEFT at a set distance (the
// "left-hand rule"): turn left into every opening, go straight if you can, turn right if blocked.
// That solves any maze whose walls are all connected — most school and fest mazes.
constexpr uint8_t L_TRIG = 2, L_ECHO = 3, F_TRIG = 7, F_ECHO = 8, R_TRIG = A1, R_ECHO = A2;
constexpr uint8_t PWMA = 5, AIN1 = 9, AIN2 = 10, PWMB = 6, BIN1 = 11, BIN2 = 12, STBY = 4;
constexpr int WALL_CM = 8;         // distance to keep from the left wall
constexpr int FRONT_STOP_CM = 9;   // wall ahead closer than this → turn right
constexpr int OPENING_CM = 25;     // left reading beyond this = an opening on the left
constexpr int BASE = 150;          // cruising PWM (0–255)
constexpr float KP = 9;            // steering gain: PWM per cm of error
constexpr int TURN_MS = 330;       // time for a 90° pivot at TURN_PWM — tune on your floor
constexpr int TURN_PWM = 170;

void motor(uint8_t pwm, uint8_t a, uint8_t b, int speed) {
  digitalWrite(a, speed > 0 ? HIGH : LOW);
  digitalWrite(b, speed < 0 ? HIGH : LOW);
  analogWrite(pwm, constrain(abs(speed), 0, 255));
}
void drive(int left, int right) {
  motor(PWMA, AIN1, AIN2, left);
  motor(PWMB, BIN1, BIN2, right);
}

// Distance in cm; 100 when nothing echoes within ~1 m (an open side).
int sonar(uint8_t trig, uint8_t echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  unsigned long us = pulseIn(echo, HIGH, 6000UL);
  delay(8);  // let the echo die out before the next sonar fires (no crosstalk)
  return us ? us / 58 : 100;
}

void pivot(int dir) {  // dir −1 = left, +1 = right, 90° by time
  drive(0, 0);
  delay(60);
  drive(dir * TURN_PWM, -dir * TURN_PWM);
  delay(TURN_MS);
  drive(0, 0);
  delay(60);
}

void setup() {
  Serial.begin(115200);
  const uint8_t outs[] = {L_TRIG, F_TRIG, R_TRIG, PWMA, AIN1, AIN2, PWMB, BIN1, BIN2, STBY};
  for (uint8_t p : outs) pinMode(p, OUTPUT);
  pinMode(L_ECHO, INPUT);
  pinMode(F_ECHO, INPUT);
  pinMode(R_ECHO, INPUT);
  digitalWrite(STBY, HIGH);  // driver awake
  drive(0, 0);
  Serial.println(F("Maze robot: starting in 3 s"));
  delay(3000);
}

void loop() {
  int left = sonar(L_TRIG, L_ECHO), front = sonar(F_TRIG, F_ECHO), right = sonar(R_TRIG, R_ECHO);
  Serial.print(F("L "));
  Serial.print(left);
  Serial.print(F(" F "));
  Serial.print(front);
  Serial.print(F(" R "));
  Serial.print(right);

  if (left > OPENING_CM) {
    // Opening on the left: roll forward so the body clears the corner, then turn into it.
    Serial.println(F(" -> left"));
    drive(BASE, BASE);
    delay(200);
    pivot(-1);
    drive(BASE, BASE);
    delay(250);
  } else if (front < FRONT_STOP_CM) {
    Serial.println(right > OPENING_CM ? F(" -> right") : F(" -> dead end, turn round"));
    pivot(1);
    if (right <= OPENING_CM) pivot(1);  // blocked on three sides: U-turn
  } else {
    // Corridor: steer to hold WALL_CM from the left wall.
    int error = left - WALL_CM;  // positive = drifting away from the wall → steer left
    int steer = constrain((int)(KP * error), -80, 80);
    drive(BASE - steer, BASE + steer);
    Serial.println(F(" -> follow"));
  }
}

// Wall-following maze robot — Arduino Nano + 3× HC-SR04 + TB6612FNG + 2 N20 gear motors
// Sonars (Trig/Echo): left D2/D3, front D7/D8, right A1/A2, all at 5V.
// TB6612FNG: PWMA D5, AIN1 D9, AIN2 D10 (left), PWMB D6, BIN1 D11, BIN2 D12 (right), STBY D4,
// VCC 5V, VM from the 2S Li-ion pack. It keeps a wall on its LEFT at a set distance (the
// "left-hand rule"): turn left into every opening, go straight if you can, turn right if blocked.
// That solves any maze whose walls are all connected — most school and fest mazes.
constexpr uint8_t L_TRIG = 2, L_ECHO = 3, F_TRIG = 7, F_ECHO = 8, R_TRIG = A1, R_ECHO = A2;
constexpr uint8_t PWMA = 5, AIN1 = 9, AIN2 = 10, PWMB = 6, BIN1 = 11, BIN2 = 12, STBY = 4;
constexpr int WALL_CM = 8;         // distance to keep from the left wall
constexpr int FRONT_STOP_CM = 9;   // wall ahead closer than this → turn right
constexpr int OPENING_CM = 25;     // left reading beyond this = an opening on the left
constexpr int BASE = 150;          // cruising PWM (0–255)
constexpr float KP = 9;            // steering gain: PWM per cm of error
constexpr int TURN_MS = 330;       // time for a 90° pivot at TURN_PWM — tune on your floor
constexpr int TURN_PWM = 170;

void motor(uint8_t pwm, uint8_t a, uint8_t b, int speed) {
  digitalWrite(a, speed > 0 ? HIGH : LOW);
  digitalWrite(b, speed < 0 ? HIGH : LOW);
  analogWrite(pwm, constrain(abs(speed), 0, 255));
}
void drive(int left, int right) {
  motor(PWMA, AIN1, AIN2, left);
  motor(PWMB, BIN1, BIN2, right);
}

// Distance in cm; 100 when nothing echoes within ~1 m (an open side).
int sonar(uint8_t trig, uint8_t echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  unsigned long us = pulseIn(echo, HIGH, 6000UL);
  delay(8);  // let the echo die out before the next sonar fires (no crosstalk)
  return us ? us / 58 : 100;
}

void pivot(int dir) {  // dir −1 = left, +1 = right, 90° by time
  drive(0, 0);
  delay(60);
  drive(dir * TURN_PWM, -dir * TURN_PWM);
  delay(TURN_MS);
  drive(0, 0);
  delay(60);
}

void setup() {
  Serial.begin(115200);
  const uint8_t outs[] = {L_TRIG, F_TRIG, R_TRIG, PWMA, AIN1, AIN2, PWMB, BIN1, BIN2, STBY};
  for (uint8_t p : outs) pinMode(p, OUTPUT);
  pinMode(L_ECHO, INPUT);
  pinMode(F_ECHO, INPUT);
  pinMode(R_ECHO, INPUT);
  digitalWrite(STBY, HIGH);  // driver awake
  drive(0, 0);
  Serial.println(F("Maze robot: starting in 3 s"));
  delay(3000);
}

void loop() {
  int left = sonar(L_TRIG, L_ECHO), front = sonar(F_TRIG, F_ECHO), right = sonar(R_TRIG, R_ECHO);
  Serial.print(F("L "));
  Serial.print(left);
  Serial.print(F(" F "));
  Serial.print(front);
  Serial.print(F(" R "));
  Serial.print(right);

  if (left > OPENING_CM) {
    // Opening on the left: roll forward so the body clears the corner, then turn into it.
    Serial.println(F(" -> left"));
    drive(BASE, BASE);
    delay(200);
    pivot(-1);
    drive(BASE, BASE);
    delay(250);
  } else if (front < FRONT_STOP_CM) {
    Serial.println(right > OPENING_CM ? F(" -> right") : F(" -> dead end, turn round"));
    pivot(1);
    if (right <= OPENING_CM) pivot(1);  // blocked on three sides: U-turn
  } else {
    // Corridor: steer to hold WALL_CM from the left wall.
    int error = left - WALL_CM;  // positive = drifting away from the wall → steer left
    int steer = constrain((int)(KP * error), -80, 80);
    drive(BASE - steer, BASE + steer);
    Serial.println(F(" -> follow"));
  }
}