How to Program a Robot for Obstacle Avoidance: Step-by-Step Guide

Obstacle avoidance is one of the most fundamental and satisfying autonomous robot behaviors to program. Learning to program a robot for obstacle avoidance teaches essential concepts, including sensor reading, decision-making logic, and motor control, that apply to all autonomous robotics projects.

This comprehensive guide walks through programming obstacle avoidance from basic concepts to working code, explains the logic behind different approaches, provides complete Arduino examples you can use immediately, and helps you troubleshoot common problems.

Understanding Obstacle Avoidance Logic

Before writing code, understanding the logic behind obstacle avoidance helps you create effective programs.

The Basic Algorithm

Obstacle avoidance follows a simple repeating pattern:

1. Measure distance to obstacles ahead

2. Check if the distance is below the safe threshold

3. If an obstacle is detected, execute an avoidance maneuver

4. If the path is clear, continue forward

5. Repeat continuously

This sense-check-act cycle runs many times per second, creating responsive autonomous navigation. The continuous repetition means the robot constantly adapts to changing environments.

Key Components Needed

Programming obstacle avoidance requires several hardware components working together.

The distance sensor measures proximity to obstacles. Ultrasonic sensors like the HC-SR04 are the most common, providing reliable distance measurements from 2cm to 400cm. These cost $2 to $5.

The microcontroller executes your obstacle avoidance program. Arduino Uno handles this perfectly and costs $10 to $25.

Motor driver controls motor direction and speed. L298N motor drivers cost $3 to $10 and control two motors independently.

Motors and wheels create robot movement. DC gear motors with attached wheels cost $5 to $15 per pair.

A power supply provides energy to electronics and motors. Battery packs with 6V to 9V work well for educational robots.

The chassis holds components in position. Simple two-wheel platforms with caster supports cost $10 to $30.

Think Robotics offers complete obstacle-avoidance robot kits with all components selected for compatibility so that you can focus on programming rather than hardware selection.

Setting Up the Hardware

Proper wiring creates the foundation for successful programming.

Ultrasonic Sensor Connections

HC-SR04 ultrasonic sensor connects to Arduino with four wires:

• VCC → Arduino 5V

• GND → Arduino GND

• TRIG → Arduino digital pin 9 (or your choice)

• ECHO → Arduino digital pin 10 (or your choice)

The TRIG pin triggers measurements, and the ECHO pin outputs a pulse width proportional to the distance.

Motor Driver Connections

The L298N motor driver requires multiple connections:

Power connections:

• 12V input → Battery positive (or 5V to 9V depending on motors)

• GND → Battery negative

• 5V output → Can power Arduino 5V pin (if using 7V+ battery)

Motor connections:

• OUT1 and OUT2 → Left motor

• OUT3 and OUT4 → Right motor

Arduino control connections:

• IN1 → Arduino digital pin 5 (left motor direction)

• IN2 → Arduino digital pin 6 (left motor direction)

• IN3 → Arduino digital pin 7 (right motor direction)

• IN4 → Arduino digital pin 8 (right motor direction)

• ENA → Arduino digital pin 3 (left motor speed, PWM pin)

• ENB → Arduino digital pin 11 (right motor speed, PWM pin)

Some motor drivers have jumpers on ENA and ENB for full-speed operation without PWM control. Remove jumpers for speed control.

Complete Wiring Checklist

Before programming, verify all connections:

• Sensor VCC and GND connected to power

• Sensor TRIG and ECHO connected to Arduino digital pins

• Motor driver powered from a battery

• Both motors are connected to the driver outputs

• All six control signals from the Arduino to the motor driver

• Common ground between Arduino and motor driver

Incorrect wiring causes unpredictable behavior. Double-check everything before uploading code.

Basic Obstacle Avoidance Code

Let's build the program step by step, starting with simple implementations and adding sophistication.

Step 1: Reading the Distance Sensor

First, verify the sensor works correctly:

const int trigPin = 9;

const int echoPin = 10;

void setup() {

  Serial.begin(9600);

  pinMode(trigPin, OUTPUT);

  pinMode(echoPin, INPUT);

}

void loop() {

  float distance = getDistance();

  

  Serial.print("Distance: ");

  Serial.print(distance);

  Serial.println(" cm");

  

  delay(500);

}

float getDistance() {

  // Trigger measurement

  digitalWrite(trigPin, LOW);

  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  

  // Read echo pulse

  long duration = pulseIn(echoPin, HIGH);

  

  // Calculate distance

  float distance = duration * 0.0343 / 2;

  

  return distance;

}

Upload this code and open the Serial Monitor. You should see distance readings updating every 500 milliseconds. Hold objects at various distances to verify accuracy.

Step 2: Testing Motor Control

Before combining sensors and motors, verify motor control works:

const int motorLeftA = 5;

const int motorLeftB = 6;

const int motorRightA = 7;

const int motorRightB = 8;

const int motorLeftSpeed = 3;

const int motorRightSpeed = 11;

void setup() {

  pinMode(motorLeftA, OUTPUT);

  pinMode(motorLeftB, OUTPUT);

  pinMode(motorRightA, OUTPUT);

  pinMode(motorRightB, OUTPUT);

  pinMode(motorLeftSpeed, OUTPUT);

  pinMode(motorRightSpeed, OUTPUT);

}

void loop() {

  // Test forward

  driveForward();

  delay(2000);

  

  // Test stop

  stopMotors();

  delay(1000);

  

  // Test reverse

  driveBackward();

  delay(2000);

  

  // Test turn

  stopMotors();

  delay(1000);

  turnRight();

  delay(1000);

  

  stopMotors();

  delay(2000);

}

void driveForward() {

  digitalWrite(motorLeftA, HIGH);

  digitalWrite(motorLeftB, LOW);

  digitalWrite(motorRightA, HIGH);

  digitalWrite(motorRightB, LOW);

  analogWrite(motorLeftSpeed, 200);

  analogWrite(motorRightSpeed, 200);

}

void driveBackward() {

  digitalWrite(motorLeftA, LOW);

  digitalWrite(motorLeftB, HIGH);

  digitalWrite(motorRightA, LOW);

  digitalWrite(motorRightB, HIGH);

  analogWrite(motorLeftSpeed, 200);

  analogWrite(motorRightSpeed, 200);

}

void turnRight() {

  digitalWrite(motorLeftA, HIGH);

  digitalWrite(motorLeftB, LOW);

  digitalWrite(motorRightA, LOW);

  digitalWrite(motorRightB, HIGH);

  analogWrite(motorLeftSpeed, 200);

  analogWrite(motorRightSpeed, 200);

}

void stopMotors() {

  digitalWrite(motorLeftA, LOW);

  digitalWrite(motorLeftB, LOW);

  digitalWrite(motorRightA, LOW);

  digitalWrite(motorRightB, LOW);

}

This code tests forward, backward, and turning motions. Verify all movements work correctly. If a motor spins backward when it should go forward, swap the two wires for that motor.

Think Robotics provides pre-tested motor-control libraries, simplifying the setup process for educational projects.

Step 3: Complete Obstacle Avoidance Program

Now combine sensor reading and motor control into functional obstacle avoidance:

// Pin definitions

const int trigPin = 9;

const int echoPin = 10;

const int motorLeftA = 5;

const int motorLeftB = 6;

const int motorRightA = 7;

const int motorRightB = 8;

const int motorLeftSpeed = 3;

const int motorRightSpeed = 11;

// Constants

const int safeDistance = 25;  // Stop if obstacle within 25cm

const int motorSpeed = 180;    // Speed (0-255)

void setup() {

  // Sensor pins

  pinMode(trigPin, OUTPUT);

  pinMode(echoPin, INPUT);

  

  // Motor pins

  pinMode(motorLeftA, OUTPUT);

  pinMode(motorLeftB, OUTPUT);

  pinMode(motorRightA, OUTPUT);

  pinMode(motorRightB, OUTPUT);

  pinMode(motorLeftSpeed, OUTPUT);

  pinMode(motorRightSpeed, OUTPUT);

  

  // Optional: Serial for debugging

  Serial.begin(9600);

}

void loop() {

  float distance = getDistance();

  

  // Debug output

  Serial.print("Distance: ");

  Serial.println(distance);

  

  if (distance < safeDistance && distance > 0) {

    // Obstacle detected - avoid it

    avoidObstacle();

  } else {

    // Clear path - drive forward

    driveForward();

  }

  

  delay(50);  // Small delay between readings

}

float getDistance() {

  digitalWrite(trigPin, LOW);

  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  

  long duration = pulseIn(echoPin, HIGH, 30000);  // Timeout after 30ms

  

  if (duration == 0) {

    return 999;  // No echo received

  }

  

  float distance = duration * 0.0343 / 2;

  return distance;

}

void driveForward() {

  digitalWrite(motorLeftA, HIGH);

  digitalWrite(motorLeftB, LOW);

  digitalWrite(motorRightA, HIGH);

  digitalWrite(motorRightB, LOW);

  analogWrite(motorLeftSpeed, motorSpeed);

  analogWrite(motorRightSpeed, motorSpeed);

}

void driveBackward() {

  digitalWrite(motorLeftA, LOW);

  digitalWrite(motorLeftB, HIGH);

  digitalWrite(motorRightA, LOW);

  digitalWrite(motorRightB, HIGH);

  analogWrite(motorLeftSpeed, motorSpeed);

  analogWrite(motorRightSpeed, motorSpeed);

}

void turnRight() {

  digitalWrite(motorLeftA, HIGH);

  digitalWrite(motorLeftB, LOW);

  digitalWrite(motorRightA, LOW);

  digitalWrite(motorRightB, HIGH);

  analogWrite(motorLeftSpeed, motorSpeed);

  analogWrite(motorRightSpeed, motorSpeed);

}

void stopMotors() {

  digitalWrite(motorLeftA, LOW);

  digitalWrite(motorLeftB, LOW);

  digitalWrite(motorRightA, LOW);

  digitalWrite(motorRightB, LOW);

  analogWrite(motorLeftSpeed, 0);

  analogWrite(motorRightSpeed, 0);

}

void avoidObstacle() {

  // Stop

  stopMotors();

  delay(300);

  

  // Back up

  driveBackward();

  delay(500);

  

  // Stop

  stopMotors();

  delay(300);

  

  // Turn right

  turnRight();

  delay(600);

  

  // Stop before resuming forward

  stopMotors();

  delay(200);

}

This complete program creates functional obstacle avoidance. Upload it, place the robot on the floor, and watch it navigate autonomously, avoiding obstacles.

Improving the Basic Program

The basic program works, but several improvements create more sophisticated behavior.

Adding Variable Turn Angles

Instead of always turning right, check both directions and turn toward the clearer path:

void avoidObstacle() {

  stopMotors();

  delay(300);

  

  driveBackward();

  delay(500);

  

  stopMotors();

  delay(300);

  

  // Check left

  turnLeft();

  delay(300);

  float leftDistance = getDistance();

  

  // Return to center

  turnRight();

  delay(300);

  

  // Check right

  turnRight();

  delay(300);

  float rightDistance = getDistance();

  

  // Return to center

  turnLeft();

  delay(300);

  

  // Turn toward a clearer direction

  if (leftDistance > rightDistance) {

    turnLeft();

    delay(600);

  } else {

    turnRight();

    delay(600);

  }

  

  stopMotors();

  delay(200);

}

void turnLeft() {

  digitalWrite(motorLeftA, LOW);

  digitalWrite(motorLeftB, HIGH);

  digitalWrite(motorRightA, HIGH);

  digitalWrite(motorRightB, LOW);

  analogWrite(motorLeftSpeed, motorSpeed);

  analogWrite(motorRightSpeed, motorSpeed);

}

This smarter avoidance checks both directions and chooses the path with more clearance.

Implementing Speed Control

Adjust speed based on distance, slowing as obstacles approach:

void loop() {

  float distance = getDistance();

  

  if (distance < 15 && distance > 0) {

    // Very close - avoid

    avoidObstacle();

  } else if (distance < 30 && distance > 0) {

    // Moderately close - slow down

    driveForwardSlow();

  } else {

    // Clear - full speed

    driveForward();

  }

  

  delay(50);

}

void driveForwardSlow() {

  digitalWrite(motorLeftA, HIGH);

  digitalWrite(motorLeftB, LOW);

  digitalWrite(motorRightA, HIGH);

  digitalWrite(motorRightB, LOW);

  analogWrite(motorLeftSpeed, motorSpeed / 2);

  analogWrite(motorRightSpeed, motorSpeed / 2);

}

This creates smoother behavior, slowing gradually rather than suddenly stopping.

Adding Multiple Sensors

Using three sensors (left, center, right) provides better spatial awareness:

const int trigPinLeft = 9;

const int echoPinLeft = 10;

const int trigPinCenter = 11;

const int echoPinCenter = 12;

const int trigPinRight = A0;

const int echoPinRight = A1;

void loop() {

  float leftDist = getDistanceLeft();

  float centerDist = getDistanceCenter();

  float rightDist = getDistanceRight();

  

  if (centerDist < 20) {

    // Obstacle ahead

    if (leftDist > rightDist) {

      turnLeft();

    } else {

      turnRight();

    }

  } else if (leftDist < 15) {

    // Obstacle on left

    turnRight();

    delay(200);

  } else if (rightDist < 15) {

    // Obstacle on right

    turnLeft();

    delay(200);

  } else {

    // Clear path

    driveForward();

  }

  

  delay(50);

}

Multiple sensors enable more nuanced navigation and better obstacle detection.

Think Robotics provides multi-sensor robot kits with mounting brackets and example code for advanced obstacle avoidance implementations.

Common Programming Problems and Solutions

Understanding typical issues helps you debug effectively.

Robot Ignores Obstacles

If the robot drives into obstacles without responding, check sensor readings in the Serial Monitor. Ensure distance values change when objects move closer. Verify the safeDistance threshold is appropriate. Check that the avoidance function actually executes by adding Serial.println() debugging statements.

Erratic or Inconsistent Behavior

Random behavior often indicates electrical noise, loose connections, or power supply issues. Verify all connections are secure. Ensure the battery voltage is adequate (minimum 6V for most setups). Add minor delays in the main loop to prevent overly rapid sensor readings. Check that the motor driver is grounded to the Arduino.

Robot Gets Stuck in Corners

Robots with only forward-facing sensors can trap themselves in corners. Solutions include backing up further during avoidance, implementing the directional-checking code shown earlier, or adding side sensors to detect wall proximity.

Motors Run at Different Speeds

Motor speed differences cause curved driving instead of straight paths. Adjust motor speed values independently to compensate. For example, if the robot curves right, increase the left motor speed or decrease the right motor speed until it drives straight.

const int motorSpeedLeft = 180;

const int motorSpeedRight = 175;  // Adjusted for straight driving

void driveForward() {

  digitalWrite(motorLeftA, HIGH);

  digitalWrite(motorLeftB, LOW);

  digitalWrite(motorRightA, HIGH);

  digitalWrite(motorRightB, LOW);

  analogWrite(motorLeftSpeed, motorSpeedLeft);

  analogWrite(motorRightSpeed, motorSpeedRight);

}

Sensor Gives Incorrect Readings

Ultrasonic sensors struggle with soft materials, angled surfaces, or transparent objects. Test with flat, hard surfaces like walls or cardboard boxes first. Add a sensor timeout to the pulseIn() function to prevent infinite waits. Consider multiple sensors providing redundant measurements.

Testing Your Obstacle Avoidance Program

Systematic testing ensures reliable autonomous operation.

Desktop Testing

Before floor testing, verify sensor and motor functions:

1. Upload code and open Serial Monitor

2. Wave your hand in front of the sensor, observing the distance changes

3. Verify motor responses at different distances

4. Check that motors spin in the correct directions

5. Confirm that the avoidance maneuver executes completely

Controlled Floor Testing

Start with simple obstacles in known locations:

1. Place the robot facing the wall 1 meter away

2. Verify robot approaches, detects the wall, and avoids

3. Add obstacles at various angles, testing the detection range

4. Test corner situations, ensuring escape behaviors work

5. Gradually increase obstacle density

Real-World Testing

After controlled tests succeed, try realistic environments:

1. Test in rooms with furniture obstacles

2. Verify performance on different floor surfaces

3. Check behavior under varied lighting conditions

4. Test battery life during extended autonomous operation

5. Note any situations causing problems for future improvements

Advanced Obstacle Avoidance Concepts

Once basic avoidance works reliably, several advanced concepts enhance performance.

State Machine Implementation

State machines organize complex behaviors into discrete states with defined transitions, creating more predictable and debuggable programs.

PID Control for Smooth Motion

PID (Proportional-Integral-Derivative) control creates smooth speed adjustments based on obstacle proximity rather than binary fast/slow decisions.

Mapping and Memory

Advanced robots remember previously encountered obstacles, building internal maps that enable more efficient navigation and avoid repeated exploration of blocked areas.

Sensor Fusion

Combining multiple sensor types (ultrasonic, infrared, and encoders) provides a more complete understanding of the environment than relying on a single sensor.

These advanced topics build on the fundamental obstacle-avoidance programming covered here and represent natural next steps as your skills progress.

Conclusion

Programming a robot for obstacle avoidance requires reading distance sensors, implementing decision logic to check whether obstacles are too close, and controlling motors to avoid detected obstacles by backing up and performing turning maneuvers. The continuous sense-check-act loop creates responsive autonomous navigation.

Starting with basic implementation testing sensors and motors separately, then combining them into complete obstacle-avoidance programs, teaches essential robotics programming concepts. Improvements like directional checking, variable speeds, and multiple sensors create increasingly sophisticated autonomous behavior.

The obstacle avoidance program you develop provides a foundation for all autonomous robotics projects. The same sensor readings, decision-making, and motor-control patterns apply whether programming line followers, maze solvers, or complex navigation systems.

Upload the code, test systematically, iterate improvements, and watch your robot navigate autonomously. Each obstacle successfully avoided represents another step in your robotics journey.