Steer correction with HC-SR04: Servo controlled car

As an alternative to the 3 wheel robot, an RC controlled car with multiple HC-SR04 sensors, is the next best thing to try out.

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

Building multiple sensors on the vehicle and letting it drive without stopping by continuously scanning and correcting the steer, seems like the most interesting method. However, it is also the most likely to miscalculate and collide with an obstacle.

The biggest advantage of this method is that the car doesn’t need to stop first, but just continues to drive while trying to avoid the obstacle, resulting in a smooth driving experience.

Another advantage is the proportional steering, which allows for subtle adjustments (object still far away) or hard steering (object close). It also has the advantage of being able to drive between walls.

The front sensor also triggers an emergency brake when an object suddenly appears within a certain range.

Driving servo controlled car

Using a servo controlled car does have a few issues; since the ultrasonic sensor doesn’t seem to perform very good measurements while moving.

Looking at these measurements, it appears that the sensors regularly have a bad measurement. There are two ways to handle this:

  1. Calculate average of last 5 measurements to smooth out the error
  2. Write some intelligence to filter out unexpected readings

Neither methods are perfect, but the latter, if well written, is the most preferable one, since it won’t respond to a bad measurement at all, while averaging may cause to not detecting a close object.

Buildup of the servo controlled car

The picture below shows a (not ideal) way of accomplishing a multiple sensor method on a very small vehicle.

RC car with 3 active scanners
RC car with 3 active scanners

Code for servo controlled car

The code below is originally written for the Arduino Uno, but can very simply be adjusted for other boards.
– Configuration in first part of the code
– Port register for interrupt pin in function ISR_ECHO()

Detailed explanations of the code can be found in the comments.
Fiddling with the parameters maxError and maxErrorFront in function autoDrive(), can improve accuracy, depending on the terrain.

// Update 11/09/2019

#include <Servo.h>
Servo servoSteer;

// Servo configuration
int leftServoSteer = 1100;  // µs for servo left
int rightServoSteer = 1900; // µs for servo right
int midServoSteer = 1500;   // µs for servo in centre
int pinServo = 8;           // Servo attached on pin 8

#define tSENS 4            // Total number of sensors
#define echoPin 2          // Pin to collect echo signal --> This must be any interrupt pin
// #define triggerPins[tSENS] {3,4};   // Trigger pin for each sensor
#define pingDelay 50      // How many milliseconds between each measurement (ping) ; best to keep > 5ms
#define soundSpeed 343.0  // Speed of sound in m/s (343m/s at 20°C with dry air)

volatile unsigned long startEcho;     // Place to store start time (interrupt)
volatile unsigned long stopEcho;      // Place to store stop time (interrupt)
float distancesCm[tSENS];             // Distances in cm
float histDistances[tSENS][5];        // Keep 5 measurements of each sensor

// Place pin numbers of the sensors' triggers here
int triggerPins[tSENS] = {4, 5, 6, 7};      // Trigger pin for each sensor

// Poll timer
unsigned long lastPingMillis;

/****************************************************************
      SETUP
****************************************************************/
void setup()
{
  Serial.begin(115200);
  while (!Serial) {
    ; // Wait for Serial
  }
  Serial.println("--- Serial monitor started ---");

  // Set all trigger pins as OUTPUTS
  for (int i = 0; i < tSENS; i++) {
    pinMode(triggerPins[i], OUTPUT);   // Set trigger pin x as OUTPUT
  }

  // Set echo pin
  pinMode(echoPin, INPUT);                  // Set echo pin as INPUT (do not use pullup when using diodes !)
  attachInterrupt(0, ISR_ECHO, CHANGE );    // Set interrupt call for echo pin

  // Set servo control
  servoSteer.attach(pinServo);
  servoSteer.writeMicroseconds(1500);

  lastPingMillis = millis();
}

/****************************************************************
      LOOP
****************************************************************/
void loop()
{
  // Ping every x ms
  if (millis() - lastPingMillis >= pingDelay)
  {
    doMeasurement();  // Call measure procedure
    // Serial.println(String(distancesCm[0]) + " - " + String(distancesCm[1]) + " - " + String(distancesCm[2])  + " - " + String(distancesCm[3]));
    autoDrive();
    lastPingMillis = millis();
  }
}


/****************************************************************
      Auto drive
****************************************************************/
void autoDrive()
{
  bool bUseAverageValues = true;
  float distFront = 0;
  float distLeft = 0;
  float distRight = 0;
  float distBack = 0;

  if (bUseAverageValues)
  {
    for (int i = 0; i < 5; i++)
    {
      distFront += histDistances[0][i];
      distLeft += histDistances[1][i];
      distRight += histDistances[2][i];
      distBack += histDistances[3][i];
    }
    distFront /= 5;
    distLeft /= 5;
    distRight /= 5;
    distBack /= 5;
  }
  else
  {
    // Which sensor is which ?
    distFront = distancesCm[0];
    distLeft = distancesCm[1];
    distRight = distancesCm[2];
    distBack = distancesCm[3];
  }

  // Everything closer than <maxError>cm needs action
  // Error is 0..100% (for distances <maxError>..5 cm)
  int maxError = 80;
  int maxErrorFront = 120;
  float errLeft = constrain(map(maxError - distLeft, 0, (maxError - 5), 0, 100), 0, 100);
  float errRight = constrain(map(maxError - distRight, 0, (maxError - 5), 0, 100), 0, 100);
  float errFront = constrain(map(maxErrorFront - distFront, 0, (maxErrorFront - 5), 0, 100), 0, 100);
  int steerCorrection = 0;

  // Find obstructions
  if (errLeft > 0 && errRight == 0)
  {
    // Correction to the right
    steerCorrection = map(errLeft, 0, 100, midServoSteer, rightServoSteer);
  }
  else if (errRight > 0 && errLeft == 0)
  {
    // Correction to the left
    steerCorrection = map(errRight, 0, 100, midServoSteer, leftServoSteer);
  }
  else if (errRight > 0 && errLeft > 0)
  {
    // Correct DIFF
    steerCorrection = map((errRight - errLeft), -100, 100, rightServoSteer, leftServoSteer);
  }
  else
  {
    // No cerrection required - steer in centre
    steerCorrection = midServoSteer;
  }

  // If front comes to close ... 
  if (errFront > 0)
  {
    // BRAKE
    Serial.println("!!! BRAKE !!!");
  }

  servoSteer.writeMicroseconds(steerCorrection);
}


/****************************************************************
      Retrieve measurement and set next trigger
****************************************************************/
void doMeasurement()
{
  // First read will be bad (or 0)
  unsigned long startWait;
  // unsigned long timeout = 50; // 50ms is the max amount of time we'll wait (23ms = 4m = out of range)
  unsigned long timeout = 13; // 13ms is about 200cm (2m)

  // For each sensor
  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(triggerPins[s], HIGH);    // HIGH pulse for at least 10µs
    delayMicroseconds(10);
    digitalWrite(triggerPins[s], LOW);    // Set LOW again

    startWait = millis();   // remember time we started 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())
      {
        // This will result in a distance related to timeout
        startEcho = 0;
        stopEcho = 200.0 * 2.0 * 10.0 * 1000.0 / (float)soundSpeed;
        break;  // exit the while loop
      }
    }

    // Store latest measurement
    noInterrupts();   // cli()
    distancesCm[s] = (stopEcho - startEcho) * (float)soundSpeed / 1000.0 / 10.0 / 2.0;   // in cm
    interrupts();   // sei();

    // Add measuement to list
    addMeasurement(distancesCm[s], s);
  }
}

/****************************************************************
      Add measurement to history
****************************************************************/
void addMeasurement(float distance, int sensor)
{
  for (int i = 4; i >= 1 ; i--)
  {
    // Shift 1 position, push pos. 5 off
    histDistances[sensor][i] = histDistances[sensor][i - 1];
  }
  // Add latest measurement to position 0
  histDistances[sensor][0] = distance;
}

/****************************************************************
      INTERRUPT handling (keep as fast(=short) as possible)
****************************************************************/
void ISR_ECHO()
{
  // For ATmega328(P) chips, INT0 as at pin PD2
  byte pinRead = 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

A tiny bit harder than the 3 wheel robot, since it cannot make hard turn when standing still, it still performs quite OK. Once it gets stuck in a U-shaped corner, driving backwards is the only option. Therefore, a sensor for detecting object in the back, is definitely a must.


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