Steer correction with HC-SR04: Single sensor, stop and scan

If speed is not an issue, and you want to look your robot car like it can actually see and think, than this method is surely a very satisfying one.

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

Basics

Another, more common method with the HC-SR04, is driving until an object in front is detected, stop the car, scan left and right, and decide with way to turn to.

This method has proven to work well in mazes (with high walls), although it only works best if the car can make perfect (as possible) 90° turns, otherwise it will end up bouncing off the walls, since it has only a 15° angle of view.

The biggest disadvantage of this method is that it quite time consuming. Each time an obstacle is detected, the following procedure is started

  • Stop driving
  • Turn sensor left
  • Scan distance en remember measurement 1
  • Turn sensor right
  • Scan distance en remember measurement 2
  • Turn sensor back to center
  • Figure out the largest distance (most space to move)
  • Turn car towwrds largest free space
  • continue driving

However, it is quite cute to see the robot almost behave like it is “looking for something”.

Build up

For the build up, the same chassis is used as the multi scanner vehicle.
(see Automatic steer correction with HC-SR04)

The HC-SR04 ultrasonic sensor is mounted on a Pi Camera pan/tilt servo mount by just using a rubber band. Since only the panning direction (X axis) is used, any servo on which you can mount the HC-SR04 can do the trick (using a piece of cardboard will do fine).

HC-SR04 servo mounted scanner
HC-SR04 servo mounted scanner

The motor controller will be the Cytron Make Drive, but just as last time, it can be replaced with the L298N.

Electrical

For controlling the servo, pin 8 is used.
For the HC-SR04 pin 2 for echo (interrupt pin) and pin 4 for trigger are used.

Servo and HC-SR04 connection to Arduino Uno

HC-SR04 scanner
HC-SR04 scanner

Motor control

A little adaptation is required for the motor controller. Originally pins 5 and 6 are used for forward control (PWM = 980Hz), and pins 10 and 11 for backwards driving (PWM = 490Hz). Pin 3 was use for the encoder (interrupt pin) but won’t be used for this version.

When using the servo library, pins 9 and 10 cannot be used with PWM anymore (unless you are using the Arduino mega). So pin 10 wil be replaced with pin 3 in order to make the control work properly.

Schematic

HC-SR04 scanner schematic
HC-SR04 scanner schematic

The schematic for the HC-SR04 servo mounted scanner on an Arduino Uno can be downloaded below.

Arduino Uno code

As usual, the Arduino Uno was used, but the code can simple be adapted for other boards as well. Just as in the previous method, don’t forget the change the port register in the interrupt call ISR_ECHO()

/****************************************************************
      Configuration
****************************************************************/
// HC-SR04 - sensors
#define tSENS 1            // 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
const int pinTriggers[tSENS] = {4};    // HC-SR04: trigger pin of each sensor

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

// Maker Drive - Motor control
/*
    On boards other than the Mega, use of the library disables analogWrite()
    (PWM) functionality on pins 9 and 10, whether or not there is a Servo on those pins
    --> So use 3 and 11 in stead !
*/
#define pinDriveM1A 5      // Cytron Maker Drive: Pin for M1A (PWM pin) - 980Hz
#define pinDriveM1B 3      // 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

// Servo
#include <Servo.h>      // Include servo library
#define pinServo   8    // Define servo pin
#define servoCenter 1500        // µs for servo center
#define servoLeft  1500 + 900   // µs for servo left limit
#define servoRight 1500 - 900   // µs for servo right limit

/*****   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,
  stopping,
  stopped,
  scanning,
  hard_left,
  hard_right,
  reversing
};
ENUM_robotState robotState = booting;

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

// Create servo
Servo servoScan;

/****************************************************************
      SETUP
****************************************************************/
void setup()
{
  Serial.begin(115200);
  while (!Serial) {
    ; // Wait for Serial
  }
  Serial.println("--- Serial monitor started ---");
  
  // 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)
  pinMode(pinServo, OUTPUT);     // servo for scanner

  // INIT servo
  servoScan.attach(pinServo);

  // 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(20);    // Detecting objects
    lastSensorMillis = millis();
  }

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

/****************************************************************
      Robot brain
****************************************************************/
void robotBrain() {
  // When is an object too close, when distance < 
  float brakeDistance = 50; // in cm ; brake, stop and act

  // Make code easier to read
  float distance = distances[0];
  // Define here, causes bugs when defined in switch
  float distanceLeft;
  float distanceRight;

  // 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 = driving;
        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 (distance < brakeDistance) {
        robotState = sensing_block;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Sensing object = keep driving until sensing object exceeds timers
    case sensing_block:
      makerDrive(B1010);
      if (distance > 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
      if (distance > brakeDistance) {
        // Obstacle cleared -> resume driving
        robotState = driving;
        stateStartTime = millis();  // Remember when switching states started
      }
      else {
        // Still obstructed -> scan
        robotState = scanning;
        stateStartTime = millis();  // Remember when switching states started
      }
      break;

    // Scanning
    case scanning:
      // The scan cycle will block the programm flow
      // 1) Scan left side
      servoScan.writeMicroseconds(servoLeft);
      delay(1000);
      distances[0] = 0;
      checkHCSR04(40);
      delay(1);
      distanceLeft = distances[0];
      updateDisplay();
      // 2) Scan right side
      servoScan.writeMicroseconds(servoRight);
      delay(1000);
      distances[0] = 0;
      checkHCSR04(40);
      delay(1);
      distanceRight = distances[0];
      updateDisplay();
      // 3) Center
      servoScan.writeMicroseconds(servoCenter);
      // 4) Pick an action
      if (distanceLeft > distanceRight) {
        // More free space on the left
        robotState = hard_left;
        stateStartTime = millis();  // Remember when switching states started
      }
      else if (distanceLeft < distanceRight) {
        // More free space on the right
        robotState = hard_right;
        stateStartTime = millis();  // Remember when switching states started
      }
      else {
        // Equal distance
        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;

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

  }  // end switch

  // Refresh the display
  updateDisplay();
}

/****************************************************************
      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 = timeout * 1000;
        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%
  */
  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
    case B1111:   // Coast
      // Full speed
      analogWrite(pinDriveM1A, fullSpeed * M1A);
      analogWrite(pinDriveM1B, fullSpeed * M1B);
      analogWrite(pinDriveM2A, fullSpeed * M2A);
      analogWrite(pinDriveM2B, fullSpeed * M2B);
      break;
    case B0101:   // Backward
    case B1001:   // Hard Left
    case B0110:   // Hard Right
      // Reduced speed
      analogWrite(pinDriveM1A, halfSpeed * M1A);
      analogWrite(pinDriveM1B, halfSpeed * M1B);
      analogWrite(pinDriveM2A, halfSpeed * M2A);
      analogWrite(pinDriveM2B, halfSpeed * 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 = "Distance (cm) - Action: ";
  for (int s=0; s<(tSENS); s++) {
    displayText += String(distances[s]);
    if (s < (tSENS - 1)) { displayText += "-"; }
  }
  displayText += " - ";
  // 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 == sensing_block) { displayText += "detecting object"; }
  else if (robotState == reversing) { displayText += "reversing"; }
  else if (robotState == booting) { displayText += "booting"; }
  else if (robotState == scanning) { displayText += "scanning"; }
  else if (robotState == hard_left) { displayText += "hard left"; }
  else if (robotState == hard_right) { displayText += "hard right"; }

  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

The “stop and scan” method may be the least practical, but admitted, it is the most fun one to look at the robot car behaving like it is actually looking around and making decisions (which it actually is).


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