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