Bitwise Obstacle Avoidance Tweak

Around the time I was rewriting the example sketch for the obstacle avoidance tutorial I stumbled across this great tutorial on bit math on the Arduino Playground. While reading it I found a good, simple use for bit math that would make my obstacle avoidance sketch a bit more efficient.

There is a popular quote, usually attributed to Don Knuth: “premature optimization is the root of all evil.” Premature optimization occurs when we make improvements to efficiency that complicate the program, reduce readability, or cause us to ignore larger, more structural improvement possibilities.

The following is definitely a premature optimization of the obstacle avoidance example sketch. However, I think it’s okay in this case because it allows us to practice a fun new technique: bit math!


Background

First thing’s first: if you haven’t read the Arduino Playground tutorial on bit math, read it now. It’s good, it’s easy to read, and it provides much more background than I’m going to.

In my obstacle avoidance example I used a multi dimensional array of bools to store the sensor states for each possible reaction, the reaction matrix. Each bool value occupies one byte, so the size of the reaction matrix in bytes is the number of sensors times the number of reactions. In our case it’s 24 bytes.

Each byte is made of 8 bits, each of which have two states. A bool value only has two states, so in theory a bool could be represented by a single bit. Further, a byte could be interpreted as an array of 8 bools. Cool, right? This means, if the number of sensors is 8 or less, the size of our reaction matrix in bytes is just the number of reactions. The size of our reaction matrix is only 8 bytes now!

Additionally, our compareCases()  can be totally eliminated by this. It formerly involved two nested for loops to compare the values in the sensor array with the reaction matrix. The number of comparisons was the equal to the number of sensors times the number of reactions. Now all we have to do is compare the thisCase  byte directly to each possible case in the switch statement. Total number of comparisons is equal to the number of cases. We just cut the number of comparisons for each sensor update down from 24 to 8! This happens ten times per second, so it’s actually kind of significant.


Example Sketch

/*
 * Obstacle avoidance bit math 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 6 // PWM pin for right hand motor
#define RH_DIR1 9 // direction control for right hand motor
                  // BIN1 pin on motor controller
#define RH_DIR2 8 // direction control for right hand motor
                    // BIN2 pin on motor controller

#define LH_PWM 11 // PWM pin for left hand motor
#define LH_DIR1 13 // direction control for right hand motor
                     // AIN1 pin on motor controller
#define LH_DIR2 12 // direction control for left hand motor
                     // AIN2 pin on motor controller
                     
#define DEFAULT_SPEED 30 // default PWM level for the motors
#define TURN_DISTANCE 20 // 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

NewPing sonar[NUM_SONAR] = { // array of sonar sensor objects
  NewPing(A2, A2, MAX_DISTANCE), // right
  NewPing(A1, A1, MAX_DISTANCE), // front
  NewPing(A0, A0, MAX_DISTANCE) // left
};

/* list of cases
    B0000000  0: no obstacles
    B0000001  1: obstacle to the right
    B0000010  2: obstacle in front
    B0000011  3: obstacles front and right
    B0000100  4: obstacle to the left
    B0000101  5: obstacles left and right
    B0000110  6: obstacles front and left
    B0000111  7: obstacles front, left, and right
*/

byte thisCase = 0;

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

void loop() {
  updateSensor();
  switch (thisCase) {
    // no obstacles
    case B00000000:
      straightForward();
      break;
    // obstacle to the right
    case B00000001:
      turnLeft(30);
      break;
    // obstacle in front
    case B00000010:
      turnLeft(90);
      break;
    // obstacles front and right
    case B00000011:
      turnLeft(90);
      break;
    // obstacle to the left
    case B00000100:
      turnRight(30);
      break;
    // obstacles left and right
    case B00000101:
      turnLeft(180);
      break;
    // obstacles front and left
    case B00000110:
      turnRight(90);
      break;
    // obstacles front, left, and right
    case B00000111:
      turnLeft(180);
      break;
  }
  delay(100); 
}

void updateSensor() {
    thisCase = 0;
    for (int i = 0; i < NUM_SONAR; i++) {
        int distance = sonar[i].ping_cm();
        if (distance == 0) 
            distance = MAX_DISTANCE;
        if (distance < TURN_DISTANCE)
            thisCase |= (1 << 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);
}

// makes a turn by stopping one motor
// accepts an int, 0 or 1 for left or right turn respectively
void turnLeft(int deg) {
  setBothSpeeds(0);
  delay(100);
  setLeftBackward(); // set left motor to run backward
  setBothSpeeds(DEFAULT_SPEED); // set speeds to 1/2 default
  delay(10 * deg); // allow time for the bot to turn
                   // turn time is approx 5ms per degree
  straightForward(); // resume driving forward at default speed
}

void turnRight(int deg) {
  setBothSpeeds(0);
  delay(100);
  setRightBackward(); // set right motor to run backward
  setBothSpeeds(DEFAULT_SPEED); // set speeds to 1/2 default
  delay(10 * deg); // allow time for the bot to turn
                      // turn time is approx 5ms per degree
  straightForward(); // resume driving forward at default speed
}

Sketch Notes

First, the reaction matrix is stored in an array of bytes called cases. The bits in each byte represent a sensor. The right sensor is represented by the least significant bit, the front is the second, and the left is the third least significant bit. There is room in each byte for five more sensors, but we’re only using three.

I think defining the reaction matrix is pretty self explanatory. The only slightly tough thing is building the thisCase byte based on the sensor inputs. We start with a byte consisting of all zeroes: 0b00000000. Then, if the reading from sensor i is less than the reaction distance, we want to change bit i to 1 (we want to flip bit i). We start with 0b00000001 and shift it to the left by i places using the bitwise shift operator: <<. This creates a byte containing a 1 followed by i zeroes. For instance:

int a = 1 << 2; // a is equal to 0b00000100 (or 4)

Then we use the bitwise OR operator to flip the appropriate bit in the thisCase byte. thisCase |= (1 << i);  flips bit i in the thisCase byte.

Once the thisCase byte is built it only needs to be compared to every element in the reaction matrix.


Coming Soon

A post on my PID motor control library!

It makes using DC motors much  easier. It requires motor encoders to provide feedback to the speed and position control routines, however. So I’ll probably do posts on using encoders and PID control as well.

 

Obstacle Avoidance

 

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.

Wiring diagram for obstacle avoidanceIt’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, dr for reaction distance. When the distance from one or more of Colin’s sensors to the nearest obstacle is less than dr Colin 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 > dr > dr > dr Drive forward
Front > dr < dr > dr Turn left 90°
Front and right > dr < dr < dr Turn left 90°
Front and left < dr < dr > dr Turn right 90°
Front, left and right < dr < dr < dr Turn left 180°
Left and right < dr > dr < dr Turn left 180°
Right > dr > dr < dr Turn left 45°
Left < dr > dr > dr 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