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).
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
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
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 Registers – attachInterrupt() – 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