At long last I’ve gotten around to doing a post on obstacle avoidance! Thanks to everyone for your patience.

When I started writing this post it had been months since I had last thought about obstacle avoidance. I opened my obstacle avoidance sketch for Colin intending to use it for this post unchanged but it looked terrible. It was a kludgy mess of nested if/else statements. So I took a few hours to totally rewrite my code and make it more efficient and readable. Revisiting old programs can be a great opportunity to reevaluate previous work.

Anyway, the basic idea behind obstacle avoidance is pretty simple. Colin, my mobile robot, can sense objects around himself. If he gets too close to an object he turns away and goes off in another direction. Easy, right?

Wiring Diagram
Program Design
Example Sketch
Video

Preliminaries

We’ll be using HC-SR04 ultrasonic sensors for this tutorial. If you’ve never used ultrasonic sensors before you should take a look at my tutorial. This tutorial also uses DC motors. If you’ve never used DC motors with an Arduino before, take a look at my motor control tutorial.

There is one problem to address right away: the configuration of our sensors. HC-SR04 sensors have a roughly 30° field of view, so with just one sensor Colin won’t be able to see anything to his sides.

Others have solved this problem by mounting a sensor on a servo so the sensor rotates and sweeps out a larger field of view. This instructable is a good example of the technique.

For my purposes it was easier to use an array of three sensors, however. With sensors facing forward, left and right Colin gets a pretty good picture of what’s around him. He still has blind spots at +45° and -45° though, so I’m planning on adding two more sensors.

top

Wiring Diagram

Wiring up the sensors and motors is pretty simple. We really just have to combine the wiring diagrams from the motor control tutorial and the ultrasonic sensor tutorial. Wire the components per the diagram below and you’ll be in good shape.

It’s come to my attention that, on some displays, the color of the TRIG and ECHO wires for the left sonar sensors is very similar to the color of the +5V wire. These wires SHOULD NOT connect, however.

top

Program Design

Before we get into actually writing the obstacle avoidance program we have to decide: how should Colin react to sensor inputs?

The simplest thing we could do is have Colin turn 90º to the left whenever one of his sensors sees an obstacle in front of him. But what if there is also an obstacle to his left? He would turn directly into the obstacle on his left while trying to avoid the one in front of him. What if there is an obstacle on Colin’s left or right but no obstacle in front? Clearly there are several possibilities here.

We need to identify the set of situations Colin might encounter that require him to react. Then we need to identify what those situations look like to Colin in terms of sensor inputs. Lastly, we need to specify an action for Colin to take in each situation.

Let’s assume Colin only needs to take action when an obstacle is within a certain distance. We can call this distance, d_rfor reaction distance. When the distance from one or more of Colin’s sensors to the nearest obstacle is less than d_rColin needs to take action to avoid the obstacle(s). The table below breaks down the situations and sensor inputs that require Colin to react and the action Colin could take.

Reaction Matrix
Obstacle Locations	Left Distance	Front Distance	Right Distance	Response
No Obstacles	> d_r	> d_r	> d_r	Drive forward
Front	> d_r	< d_r	> d_r	Turn left 90°
Front and right	> d_r	< d_r	< d_r	Turn left 90°
Front and left	< d_r	< d_r	> d_r	Turn right 90°
Front, left and right	< d_r	< d_r	< d_r	Turn left 180°
Left and right	< d_r	> d_r	< d_r	Turn left 180°
Right	> d_r	> d_r	< d_r	Turn left 45°
Left	< d_r	> d_r	> d_r	Turn right 45°

Notice that our reaction matrix requires Colin to turn by a number of degrees for most of his reactions. But Colin has no way of knowing how far his wheels have turned, which would be required for him to know how many degrees he’s turned. The best we can do for now is set Colin’s motors to run at different speeds for a certain time interval. Through trial and error we can find the speed differential and time interval required to get a specific amount of turning. This is an approximate solution but we can’t do any better until we add encoders to Colin’s motors.

top

Example Sketch

/*
 * Obstacle avoidance example
 * by Andrew Kramer
 * 8/3/2015
 *
 * Uses 3 ultrasonic sensors to determine
 * if nearest obstacle is too close.
 * Outlines 8 possible cases for
 * positions of obstacles.
 */

#include <NewPing.h>

#define RH_PWM 3 // PWM pin for right hand motor
#define RH_DIR1 4 // direction control for right hand motor
                  // BIN1 pin on motor controller
#define RH_DIR2 5 // direction control for right hand motor
                    // BIN2 pin on motor controller
#define LH_PWM 9 // PWM pin for left hand motor
#define LH_DIR1 7 // direction control for right hand motor
                     // AIN1 pin on motor controller
#define LH_DIR2 8 // direction control for left hand motor
                     // AIN2 pin on motor controller
#define DEFAULT_SPEED 25 // default PWM level for the motors
#define TURN_DIST 25 // distance at which the bot will turn
#define MAX_DISTANCE 200 // max range of sonar sensors
#define NUM_SONAR 3 // number of sonar sensors
#define NUM_CASES 8 // number of reaction cases

#define MS_PER_DEGREE 10 // milliseconds per degree of turning

NewPing sonar[NUM_SONAR] = { // array of sonar sensor objects
  NewPing(13, 13, MAX_DISTANCE), // left
  NewPing(12, 12, MAX_DISTANCE), // front
  NewPing(11, 11, MAX_DISTANCE) // right
};

/*  
 *  stores a bool for each sensor (left, front, and right respectively
 *  true if nearest obstacle is within TURN_DIST
 *  true if not
 */
bool sensor[3] = {false, false, false}; 

// stores all possible sensor states
bool reactions[NUM_CASES][NUM_SONAR] = { 
   {false, false, false}, // 0: no obstacles
   {false, true, false},  // 1: obstacle in front
   {false, true, true},   // 2: obstacles front and right
   {true, true, false},   // 3: obstacles front and left
   {true, true, true},    // 4: obstacles front, left, and right
   {true, false, true},   // 5: obstacles left and right
   {false, false, true},  // 6: obstacle to the right
   {true, false, false} }; // 7: obstacle to the left

void setup() {
  for (int pin = 3; pin <= 9; pin++) {
    pinMode(pin, OUTPUT); // set pins 3 through 9 to OUTPUT
  }
  Serial.begin(9600);
}

void loop() {
  updateSensor();
  switch (compareCases()) {    
    case 0: // no obstacles
      straightForward();
      break;
    case 1: // obstacle in front
      turnLeft(90);
      break;
    case 2: // obstacles front and right
      turnLeft(90);
      break;
    case 3: // obstacles front and left
      turnRight(90);
      break;
    case 4: // obstacles front, left, and right
      turnLeft(180);
      break;
    case 5: // obstacles left and right
      turnLeft(180);
      break;
    case 6: // obstacle to the right
      turnLeft(30);
      break;
    case 7: // obstacle to the left
      turnRight(30);
      break;
  }
  delay(100);
}

void updateSensor() {
  for (int i = 0; i < NUM_SONAR; i++) {
    int dist = sonar[i].ping_cm();
    // if sonar returns 0 nearest obstacle is out of range
    if (dist == 0) sensor[i] = false;
    else sensor[i] = dist < TURN_DIST;
  }
}

int compareCases() {
  for (int i = 0; i < NUM_CASES; i++) {
    bool flag = true;
    for (int j = 0; j < NUM_SONAR; j++) {
      if (reactions[i][j] != sensor[j]) flag = false;
    }
    if (flag) return i;
  }
}

void setLeftForward() {
  digitalWrite(LH_DIR1, HIGH);
  digitalWrite(LH_DIR2, LOW);
}

void setRightForward() {
  digitalWrite(RH_DIR1, HIGH);
  digitalWrite(RH_DIR2, LOW);
}

void setBothForward() {
  setLeftForward();
  setRightForward();
}

void setLeftBackward() {
  digitalWrite(LH_DIR1, LOW);
  digitalWrite(LH_DIR2, HIGH);
}



void setRightBackward() {
  digitalWrite(RH_DIR1, LOW);
  digitalWrite(RH_DIR2, HIGH);
}

void setBothBackward() {
  setRightBackward();
  setLeftBackward();
}

void setLeftSpeed(int speed) {
  analogWrite(LH_PWM, speed);
}

void setRightSpeed(int speed) {
  analogWrite(RH_PWM, speed);
}

void setBothSpeeds(int speed) {
  setLeftSpeed(speed);
  setRightSpeed(speed);
}

// sets direction of both motors to forward and sets both speeds
// to default speed
void straightForward() {
  setBothForward();
  setBothSpeeds(DEFAULT_SPEED);
}

void turnLeft(int deg) {
  setBothSpeeds(0);
  delay(100); // delay to allow motors to stop before direction change
  setLeftBackward();
  setBothSpeeds(DEFAULT_SPEED);
  delay(MS_PER_DEGREE * deg); // allow time for the bot to turn
  straightForward(); // resume driving forward at default speed
}

void turnRight(int deg) {
  setBothSpeeds(0);
  delay(100); // delay to allow motors to stop before direction change 
  setRightBackward(); 
  setBothSpeeds(DEFAULT_SPEED); 
  delay(MS_PER_DEGREE * deg); // allow time for the bot to turn
  straightForward(); // resume driving forward at default speed
}

You probably won’t be able to load up the code on your differential drive robot and run it, even if you have it wired properly. Depending on how you have the motors wired, one or both of them might run backward. To fix this you should just swap the values in the DIR1 and DIR2 fields for the problematic motor. Also, you may have to adjust the value of MS_PER_DEGREE to get accurate turning.

You’ll notice most of the code in the above sketch is devoted to simply controlling the two motors: setting motor directions, PWM levels, etc. In fact, very little of the above sketch codes for higher level behaviors like deciding when and how to react to obstacles. This makes the sketch more difficult to read and it will only get worse when we add encoders to the mix.

To fix this I’ve developed a motor control library. Encapsulating the motor control code in separate motor objects allows us to focus on programming high-level behaviors without worrying too much about how we’re controlling the motors. I’ll present my motor control library (and make it available for download) in my next post.

top

Video

Below you can see a video of Colin running the above example sketch.

You’ll notice that Colin always stops before making a turn. This slows him down and makes his behavior appear jerky. New methods could be added to the sketch to allow Colin to turn while still moving forward in some situations. These methods would simply slow down the wheel on the inside of the turn rather than reversing it. This could be useful when Colin approaches an obstacle obliquely. In this case only a minor course correction is required so a small, smooth turn is better than stopping, rotating, and starting forward again.

That’s all for today. Check back in a couple of weeks for posts on my motor control library and a refinement to obstacle avoidance that uses bit math!

top

Bot Blog

building my robot army one sketch at a time

Obstacle Avoidance

Preliminaries

Wiring Diagram

Program Design

Reaction Matrix

Example Sketch

Video