Obstacle avoidance with HC-SR04 and Arduino Nano

obstacleAvoider.ino

//Obstacle avoiding robot with HC-SR04 and Arduino Nano
#include <Servo.h>

Servo myservo;
int dAngle_deg = 2;
const int trigPin = 8;
const int echoPin = 4;
const int buzzerPin = 3;
const int motorLeftDir = 2;  //motorLeft: m1
const int motorRightDir = 7; //motorRight: m2
const int motorLeftEn = 5;
const int motorRightEn = 6;
int speed = 80;

void setup() {
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(motorLeftDir, OUTPUT);
  pinMode(motorRightDir, OUTPUT);
  pinMode(motorLeftEn, OUTPUT);
  pinMode(motorRightEn, OUTPUT);
  Serial.begin(9600);
}

void stop() {
  digitalWrite(motorLeftDir, LOW);
  analogWrite(motorLeftEn, 0);
  digitalWrite(motorRightDir, LOW);
  analogWrite(motorRightEn, 0);
}

void moveForward() {
  digitalWrite(motorLeftDir, HIGH);
  analogWrite(motorLeftEn, speed);
  digitalWrite(motorRightDir, HIGH);
  analogWrite(motorRightEn, speed);
}

void moveBackward() {
  digitalWrite(motorLeftDir, LOW);
  analogWrite(motorLeftEn, speed);
  digitalWrite(motorRightDir, LOW);
  analogWrite(motorRightEn, speed);
}

void turnRight() {
  digitalWrite(motorLeftDir, HIGH);
  analogWrite(motorLeftEn, speed);
  digitalWrite(motorRightDir, LOW);
  analogWrite(motorRightEn, speed);
}

void turnLeft() {
  digitalWrite(motorLeftDir, LOW);
  analogWrite(motorLeftEn, speed);
  digitalWrite(motorRightDir, HIGH);
  analogWrite(motorRightEn, speed);
}

void playSiren() {
  const int NOTE_C4 = 262;
  const int NOTE_F4 = 349;
  const int duration_ms = 500;
  tone(buzzerPin, NOTE_C4, duration_ms);
  delay(duration_ms*1.10);
  tone(buzzerPin, NOTE_F4, duration_ms);
  delay(duration_ms*1.10);
}

bool obstacleAt(int angle_deg) {
  myservo.write(angle_deg);
  delay(15); //wait for servo turn to finish
  delay(50-15); //wait at least 50ms between pings, otherwise results might be mixed with previous ping. See https://www.makerguides.com/hc-sr04-arduino-tutorial/
  digitalWrite(trigPin, LOW); //to make sure that the trigPin is clear
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW); //after this, 8 cycle sonic burst will be sent out  
  long duration_us = pulseIn(echoPin, HIGH, 2000); //2000us ~ 10cm. If you don't use timeout, it might take up to 1s for pulseIn to return.
  int distanceRaw_cm = 0.034*duration_us/2;
  int distance_cm = constrain(distanceRaw_cm, 0, 30); //to get rid of noisy readings
  return (duration_us > 0 && distance_cm < 10); //duration_us == 0 --> no sensor reading, i.e. no obstacle near sensor
}

bool noObstacleAtInterval(int startAngle_deg, int endAngle_deg) {
  int obstacleCounter = 0;
  for(int angle_deg = startAngle_deg; angle_deg < endAngle_deg; angle_deg += dAngle_deg) {
    if(obstacleAt(angle_deg)) obstacleCounter++;
    if(obstacleCounter >=3) return false;
  }
  return obstacleCounter < 3;
}

bool frontIsClear() {
  return noObstacleAtInterval(70, 110);
}

bool leftIsClear() {
  return noObstacleAtInterval(120, 170);
}

bool rightIsClear() {
  return noObstacleAtInterval(10, 60);
}

void loop() {
  stop();
  if(frontIsClear()) {
    moveForward();
  } else if (leftIsClear()) {
    turnLeft();
  } else if (rightIsClear()) {
    turnRight();
  } else { //robot is surrounded on all three sides by obstacles
    playSiren();
  }
  delay(500); //wait for robot motion to finish
}