Automatic steer correction with HC-SR04

Making a driving vehicle avoiding obstacles using HC-SR04 sensors, turns out to be quite challenging. But despite the shortcomings of this hardware, it is achievable in a certain environment.

Autonomous vehicles, HC-SR04 sensors

Step 1: Detecting objects with HC-SR04
Step 2: Multiple HC-SR04 with Arduino using interrupts
Step 2b: Multiple HC-SR04 with Arduino using only 1 interrupt pin
Step 3: Choosing the right vehicle
Step 4: Automatic steer correction with HC-SR04
Step 5a: Vehicle accessories – SSD1306 status display
Step 5b: Vehicle accessories – Rotary encoder illuminated button
Step 6: Autonomous vehicles, putting it all together
Alternative 1: Steer correction with HC-SR04: Single sensor, stop and scan
Alternative 2: Steer correction with HC-SR04: Servo controlled car

Downside of the HC-SR04

Due to the concept of the HC-SR04 (ultrasonic waves), and the narrow measuring angle (15°), there are a few issues in detecting objects

  • It is very difficult to scan small or narrow objects like table legs and chairs
  • Round object are sometimes hard to detect as well
  • Facing a corner (for example of a cabinet) at a 45° angle, also appears to be hard to detect

The graphic representation below if exaggerated, but shows the complications of detecting an angled object using ultrasonic sensors.

45° angled corner is hard to detect
45° angled corner is hard to detect

Another issue with the HC-SR04, is that it doesn’t measure very stable. Looking at the serial monitor, the measured distances seem to be quite accurate, but every few measurements, a erroneous reading appears. To fix (or at least reduce) this issue, there are some alternatives.

Alternative sensors

The HC-SR04 is one of the cheapest ultrasonic sensors on the market, having its minor issues when it comes to accuracy and stability, especially the latter. But there are some alternatives, that perform slightly better, and still are affordable. They were not tested for this project, but doing some research, there are two very good candidates:

  • HY-SRF05
    This sensor is claimed to be slightly more accurate than the HC-SR04
  • US-015
    Within the “affordable” price range, the US-015 is said to be the way to go, outperforming both the HC-SR04 and HY-SRF05

Unfortunately, these sensors are a little bit harder to find, but certainly not impossible, than the HC-SR04.

HY-SRF05 and US-015
HY-SRF05 and US-015

Concept

Test space

The HC-SR04 performs best on walls, so this project is mainly tested in rooms with only large cabinets present, and no round or small objects, that could be missed by the ultrasonic sensor.

Logic

The intelligence of the vehicle is relatively simple. It will constantly measure the distance using three HC-SR04 sensors. One will look straight in front, the other 2 are angled left and right (about 30°). If the left or right sensor detects an object within a certain distance, the vehicle will try to steer away while continue driving. When the distance becomes too small, the vehicle will stop, make a hard turn to the side with the most space, and resumes driving.

Code

Depending on your model, some parameters may need adjustment. This code is written for the Arduino Uno (and alternatives), so don’t forget to check the ISR_ECHO() function for the correct port registers if an alternative board is used. All other parameters are set in the “Configuration” section.

/****************************************************************
      Configuration
****************************************************************/
// HC-SR04 - sensors
#define tSENS 3            // Total number of sensors (3: Left, center, right)
#define pinEcho 2          // HC-SR04: common pin for echo (must be interrupt pin - also check port register in ISR !)
#define soundSpeed 343     // Speed of sound in m/s (343m/s at 20°C with dry air) to calculate distance
//#define pinTriggers[tSENS] {A0,A1,A2}    // HC-SR04: trigger pin of each sensor
const int pinTriggers[tSENS] = {A0,A1,A2};    // HC-SR04: trigger pin of each sensor (0=left, 1=center, 2=right)

// Sensor delay
#define sensorDelay 50     // Check sensors every x ms (keep at least > 5ms)

// Maker Drive - Motor control
#define pinDriveM1A 5      // Cytron Maker Drive: Pin for M1A (PWM pin) - 980Hz
#define pinDriveM1B 10     // Cytron Maker Drive: Pin for M1B (PWM pin) - 490Hz
#define pinDriveM2A 6      // Cytron Maker Drive: Pin for M2A (PWM pin) - 980Hz
#define pinDriveM2B 11     // Cytron Maker Drive: Pin for M2B (PWM pin) - 490Hz
#define fullSpeed   150    // 0-255 PWM signal for highest speed (forward drive, soft turn)
#define halfSpeed   100    // 0-255 PWM signal for reduced speed (hard turn, reverse)

// Time between decisions (keep bigger than sensor delay)
#define robotBrainDelay 100   // Brain will call drive control and display refresh

// Use display ?
bool displayPresent = true;      // SD1306 display present
/*****   END OF CONFIGUTATION    *****/

volatile unsigned long startEcho;     // Place to store start time (interrupt)
volatile unsigned long stopEcho;      // Place to store stop time (interrupt)
float distances[tSENS];               // HC-SR04 last measured distance per sensor (in cm)

// Robot state ENUM
enum ENUM_robotState {
  paused,
  booting,
  driving,
  sensing_block,
  soft_left,
  soft_right,
  hard_left,
  hard_right,
  stopping,
  stopped,
  reversing
};
ENUM_robotState robotState = booting;

// Poll timers
unsigned long lastSensorMillis;
unsigned long lastRobotBrainMillis;
unsigned long stateStartTime;

/****************************************************************
      SETUP
****************************************************************/
void setup()
{
  Serial.begin(115200);
  while (!Serial) {
    ; // Wait for Serial
  }
  Serial.println("--- Serial monitor started ---");
  updateDisplay();
  
  // Set pins as input
  pinMode(pinEcho, INPUT);	       // Echo pin

  // Set pins as output
  for (int s = 0; s < tSENS; s++) {
    pinMode(pinTriggers[s], OUTPUT);   // Set trigger pin x as OUTPUT
  }
  pinMode(pinDriveM1A, OUTPUT);  // Motor 1A (PWM)
  pinMode(pinDriveM1B, OUTPUT);  // Motor 1B (PWM)
  pinMode(pinDriveM2A, OUTPUT);  // Motor 2A (PWM)
  pinMode(pinDriveM2B, OUTPUT);  // Motor 2B (PWM)

  // Set interrupts
  attachInterrupt(0, ISR_ECHO, CHANGE );    // Set interrupt call for echo pin

  // Set timers
  lastSensorMillis = millis();
  lastRobotBrainMillis = millis();
  
  // Set robot state to booting
  robotState = booting;
  stateStartTime = millis();  // Remember when booting state started
}

/****************************************************************
      LOOP
****************************************************************/
void loop()
{
  // Read sensors
  if (millis() - lastSensorMillis >= sensorDelay)
  {
    checkHCSR04(15);    // Detecting objects (L/C/R)
    lastSensorMillis = millis();
  }

  // Make logic decision
  if (millis() - lastRobotBrainMillis >= robotBrainDelay)
  {
    robotBrain(); // Making decisions
    lastRobotBrainMillis = millis();
  }
}

/****************************************************************
      Robot brain
****************************************************************/
void robotBrain() {
  // 
  float steerDistance = 50; // in cm ; steer away but keep driving
  float brakeDistance = 20; // in cm ; brake, stop and act

  // Make code easier to read
  float distanceLeft = distances[0];
  float distanceCenter = distances[1];
  float distanceRight = distances[2];

  // Check current state and make decision
  switch (robotState) {
    // If booting, apply brakes and stay in this state for a while
	  // so the battery can be properly attached
    case booting:
      makerDrive(B0000);
      if (stateStartTime + 2000 < millis()) {
        robotState = paused;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Paused state activated: brake and wait
    case paused:
      makerDrive(B0000);
      if (stateStartTime + 2000 < millis()) {
       // Option encoder button used = FALSE -> auto resume after x milliseconds
        robotState = stopped;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;
      

    // Driving = move forward untill object detected
    case driving:
      makerDrive(B1010);
      // Distance < brake distance on any sensor
      if (not(checkDistance(brakeDistance))) {
        robotState = sensing_block;
        stateStartTime = millis();  // Remember when switching states started
      }

      // Distance > brake distance but steer correction required
      if (distanceLeft < steerDistance || distanceRight < steerDistance) {
        if (distanceLeft < distanceRight) {
          // correction to right
          robotState = soft_right;
          stateStartTime = millis();  // Remember when switching states started
        }
        else if(distanceRight < distanceLeft) {
          // correction to left
          robotState = soft_left;
          stateStartTime = millis();  // Remember when switching states started
        }
        // else just keep heading straight
      }
      break;

	  // Sensing object = keep driving until sensing object exceeds timers
	  case sensing_block:
  	  makerDrive(B1010);
  	  if (checkDistance(brakeDistance)) {
  		  robotState = driving;
  		  stateStartTime = millis();  // Remember when switching states started
  	  }
  	  else if (stateStartTime + 150 < millis()) {
  		  robotState = stopping;
  		  stateStartTime = millis();  // Remember when switching states started
  	  }
  	  break;

    // Coast stop and assume time it takes to standstill
    case stopping:
      makerDrive(B1111);
      if (stateStartTime + 500 < millis()) {
        robotState = stopped;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Stopped = apply brakes and wait for road to be cleared
    case stopped:
      makerDrive(B0000);  // Brakes on
      // Obstacle cleared -> resume driving
      if (checkDistance(brakeDistance)) {
        robotState = driving;
      }
      // Wait a small amount of time, then make decision
      if (stateStartTime + 500 < millis()) {
        if (distances[0] > distances[2]) {
          // More space on the left, turn left
          robotState = hard_left;
        }
        else if (distances[0] < distances[2]) {
          // more space on the right, turn right
          robotState = hard_right;
        }
        else {
          // Equal distance? What's the chance, drive reverse
          robotState = reversing;
        }
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Turning hard left
    case hard_left:
      makerDrive(B0110);
      // Turning left = depending on speed, so adjust for 90° turn or use encoders
      if (stateStartTime + 500 < millis()) {
        robotState = stopping;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Turning hard right
    case hard_right:
      makerDrive(B1001);
      // Turning right = depending on speed, so adjust for 90° turn or use encoders
      if (stateStartTime + 500 < millis()) {
        robotState = stopping;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Reversing should only take a little time
    case reversing:
      makerDrive(B0101);
      if (stateStartTime + 500 < millis()) {
        robotState = stopping;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Steering soft left (continue driving)
    case soft_left:
      makerDrive(B1110);  // coast left motor
      if (distanceRight > steerDistance) {
        robotState = driving;
        stateStartTime = millis();  // Remember when switching states started
      }
      // If object become to close -> brake
      if (not(checkDistance(brakeDistance))) {
        robotState = stopping;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Steering soft right (continue driving)
    case soft_right:
      makerDrive(B1011); // coast right motor
      if (distanceLeft > steerDistance) {
        robotState = driving;
        stateStartTime = millis();  // Remember when switching states started
      }
      // If object become to close -> brake
      if (not(checkDistance(brakeDistance))) {
        robotState = stopping;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Something went wrong ?
    default:
      robotState = booting;
      stateStartTime = millis();  // Remember when switching states started
      break;

  }  // end switch

  // Refresh the display
  updateDisplay();
}

/****************************************************************
      All sensors detect free route ?
****************************************************************/
bool checkDistance(int brakeDistance) {
  // Check if any of the sensors detects an object that is too close
  bool distOK = true;  
  for (int s=0; s<tSENS; s++) {
    if (distances[s] < brakeDistance) {
      distOK = false;
    }
  }
  return distOK;
}

/****************************************************************
      Handle HC-SR04 ensors
****************************************************************/
void checkHCSR04(unsigned long timeout)
{
  unsigned long startWait;

  // Loop trough all sensors
  for (int s = 0; s < tSENS; s++)
  {
    startEcho = 0;  // Reset start time
    stopEcho = 0;   // Reset stop time

    // Initiate next trigger
    // digitalWrite(triggerPins[s], LOW);  // rest of loop already takes > 2µs
    // delayMicroseconds(2);
    digitalWrite(pinTriggers[s], HIGH);    // HIGH pulse for at least 10µs
    delayMicroseconds(10);
    digitalWrite(pinTriggers[s], LOW);    // Set LOW again

    startWait = millis();   // remember starting to wait for the echo signal
    while (startEcho == 0 or stopEcho == 0) {
      // Keep looping unill start and stoptime is not 0 OR a timeout occurs
      if (startWait + timeout < millis())
      {
        // Timeout (object out of range for sensor)
        startEcho = 0;
        stopEcho = 20000;
        break;  // exit the while loop
      }
    }
    // Store latest measurement
    noInterrupts();   // cli()
    distances[s] = (stopEcho - startEcho) * (float)soundSpeed / 1000.0 / 10.0 / 2.0;   // in cm
    interrupts();   // sei();
  }
}

/****************************************************************
      Maker Drive
****************************************************************/
void makerDrive(byte controlByte) {
  /*
      controlByte     |
      M1A M1B M2A M2B | A B
       0   0   0   0  | 0 0 = Brake
       1   0   1   0  | 1 0 = Forward
       0   1   0   1  | 0 1 = Backward
       1   1   1   1  | 1 1 = Coast

      controlSpeed
      0: 0%
      127: 50%
      255: 100%
  */
  byte highSpeed;
  byte lowSpeed;

  // Handle speed values
  highSpeed = fullSpeed;
  lowSpeed = halfSpeed;
    
  bool M1A = bool(controlByte >> 3 & B1);  // Mask bit for M1A
  bool M1B = bool(controlByte >> 2 & B1);  // Mask bit for M1B
  bool M2A = bool(controlByte >> 1 & B1);  // Mask bit for M2A
  bool M2B = bool(controlByte >> 0 & B1);  // Mask bit for M2B

  switch (controlByte) {
    case B0000:   // Brake
    case B1010:   // Forward
      // Full speed
      analogWrite(pinDriveM1A, highSpeed * M1A);
      analogWrite(pinDriveM1B, highSpeed * M1B);
      analogWrite(pinDriveM2A, highSpeed * M2A);
      analogWrite(pinDriveM2B, highSpeed * M2B);
      break;
    case B1111:   // Coast
      digitalWrite(pinDriveM1A, 1);
      digitalWrite(pinDriveM1B, 1);
      digitalWrite(pinDriveM2A, 1);
      digitalWrite(pinDriveM2B, 1);
      break;
    case B0101:   // Backward
    case B1001:   // Hard Right (L forward ; R reverse)
    case B0110:   // Hard Left  (R forward ; L reverse)
      // Reduced speed
      analogWrite(pinDriveM1A, lowSpeed * M1A);
      analogWrite(pinDriveM1B, lowSpeed * M1B);
      analogWrite(pinDriveM2A, lowSpeed * M2A);
      analogWrite(pinDriveM2B, lowSpeed * M2B);
      break;
    case B1011:   // Soft Right (L fast ; R slow)
      // Mixed speed
      analogWrite(pinDriveM1A, highSpeed * M1A);
      analogWrite(pinDriveM1B, highSpeed * M1B);
      analogWrite(pinDriveM2A, lowSpeed * 1);  // Overwrite coast
      analogWrite(pinDriveM2B, lowSpeed * 0);  // Overwrite coast
      break;
    case B1110:   // Soft Left (L slow ; R fast)
      // Mixed speed
      analogWrite(pinDriveM1A, lowSpeed * 1);  // Overwrite coast
      analogWrite(pinDriveM1B, lowSpeed * 0);  // Overwrite coast
      analogWrite(pinDriveM2A, highSpeed * M2A);
      analogWrite(pinDriveM2B, highSpeed * M2B);
      break;
    default:
      // Unexpected value ==> brake
      digitalWrite(pinDriveM1A, LOW);
      digitalWrite(pinDriveM1B, LOW);
      digitalWrite(pinDriveM2A, LOW);
      digitalWrite(pinDriveM2B, LOW);
      break;
  } // END SWITCH
}

/****************************************************************
      Display update
****************************************************************/
void updateDisplay() {
  // Build display text
  String displayText;
  // Display distance measured by the front sensor
  displayText = "Distances L/C/R (cm):\n";
  for (int s=0; s<(tSENS); s++) {
    displayText += String(distances[s]);
    if (s < (tSENS - 1)) { displayText += "-"; }
  }
  displayText += "\n";
  displayText += "Action:";
  // Displat current state of  the robot
  if (robotState == stopped) { displayText += "stopped"; }
  else if (robotState == stopping) { displayText += "stopping"; }
  else if (robotState == driving) { displayText += "driving (F)"; }
  else if (robotState == reversing) { displayText += "reversing"; }
  else if (robotState == booting) { displayText += "booting"; }
  else if (robotState == paused) { displayText += "paused"; }
  else if (robotState == hard_left) { displayText += "hard left"; }
  else if (robotState == hard_right) { displayText += "hard right"; }
  else if (robotState == soft_left) { displayText += "soft left"; }
  else if (robotState == soft_right) { displayText += "soft right"; }
  displayText += "\n";
  
  Serial.println(displayText);        // Show on serial monitor instead
}

/****************************************************************
      INTERRUPT handling (keep as fast(=short) as possible)
****************************************************************/
// Echo pin
void ISR_ECHO()
{
  // bool pinRead = digitalRead(pinEcho);  // Use faster port registers for HC-SR04 instead

  // For ATmega328(P) chips, INT0 as at pin PD2
  bool pinRead = bool(PIND >> 2 & B0001);  // Read state of interrupt pin

  if (pinRead)  {
    // State = HIGH -> Remember start time
    startEcho = micros();
  }
  else {
    // State is LOW -> Remember stop time
    stopEcho = micros();
  }
}
//END

Conclusion

With this method in this specific testing environment, collisions can be reduced to a very minimum, but there are a few minor issues left.

When the vehicle drives in a U-shaped corner, it will stop, measure left and right distances and turns to the side where the distance is largest (more space). However, that space may still be too close, so the vehicle measures again, and now the opposite side is further away. As you may guess, it will get stuck in a turning left to right loop. Off-course, with some additional “intelligence” coding, this can be solved.

Another issue is when driving to the corner of a cabinet at a 45° angle. because of the concept of the ultrasonic sensor, the sound pulse will bounce away and cannot be detected. The vehicle will hit the corner, and continues to try driving forward. Writing code to avoid this, may be a bit harder.


References
Arduino Boards: Arduino Uno R3 – Arduino Nano V3 – Arduino Micro
Arduino Software: Arduino IDE
Arduino Playground: Library NewPing
Arduino forum: Arduino Micro Interrupts 6
Arduino Reference: Port RegistersattachInterrupt()interrupt()noInterrrupt()pulseIn()pulseInLong()Servo library
Random Nerd Tutorials: Complete Guide for Ultrasonic Sensor HC-SR04 with Arduin
Circuit Digest: Arduino Interrupts Tutorial
Electro Noobs: Arduino register control, timers and interruptions
Wikipedia: Speed of sound – Echolocation