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.