Wall Follower Robot

Version 1

Version 2

I decided to make a robot that follows a wall on its right side. I have an NXShield-Dx, a shield that sits on the Arduino Uno and connects it to the motors and sensors of the Mindstorms NXT. The NXT motors are great, and I used one NXT ultrasonic sensor. With the Arduino I could also use an Arduino compatible distance sensor so I got a range sensor at Radio Schack (on sale for $15!). In both designs I put the NXT sensor facing forward to detect obstacles directly in front that the side detector can't see, and the Radio Shack sensor directed to the right side to detect the wall. For power I
attached the leads from a 6 X AA battery holder I had to the handy NXShield-Dx's wire lead terminals.

The Goal

I wanted the robot to follow a wall with various contours, from diagonals moving towards and away from the robot's right side, perpendicular corners obstructing and falling away from the robot, and curves. I set up courses with boxes, books, doors and walls.

The Algorithm(s)

With the first robot the program did the following:

• start the loop 
• get the distance in mm from both ultrasonic sensors.
• check the wall conditions
• if the wall sensor detects the wall closer than the "inner threshold" (which changed a lot over versions), turn away from the wall
• else turn towards the wall
• check the front conditions
• if the front sensor detects something in front, point turn left 90 degrees 

This worked pretty well but I felt it could be improved. The main problem was the spread out build, with the motors at the side. Also, the if/else algorithm of the wall sensor makes it wag side-to-side a bit, which is not very efficient if one goal is to move as quickly as it can, which I didn't think about until the second version.

The second robot did the following:

• start the loop
• get the distance in mm from both ultrasonic sensors.
• check the wall conditions
• if the wall sensor detects the wall closer than the "inner threshold," turn away from the wall
• else if the wall is detected beyond the "inner threshold" and inside the "outer threshold," go straight
• else turn towards the wall
• check the front conditions
• if the front sensor detects something in front, point turn left 90 degrees 

The smaller size of this build with the motors underneath allowed it to stay closer to the wall and make the the right angle turn in a smaller space. And the added straightway behavior between the inner and outer thresholds allowed it to move much more quickly and efficiently. The only problem it encountered as a result of the increased speed was that if it went above 50% speed around a hard right corner it would stray too far from the wall and end up coming at it straight on.

Program

Here is the Arduino program I ended up with. The NXShield has a very nice library you can see here.

#include <Wire.h>
#include <NXShield.h>
#include <NXTUS.h>
NXShield  nxshield;
NXTUS       sonar1;
const int sig = 7;
const int faster = 70;
const int slower = 0;
const int innerThreshold = 4;
const int outerThreshold = 7;
const int frontThreshold = 13;
long duration, inches, cm;

void setup() {
  Serial.begin(9600);
  delay(2000);                // wait two seconds, allowing time to
  // activate the serial monitor
  nxshield.init( SH_HardwareI2C );
  // Wait until user presses GO button to continue the program
  nxshield.waitForButtonPress(BTN_GO);

  sonar1.init( &nxshield, SH_BBS1 );
  // reset motors.
  nxshield.bank_a.motorReset();
}

void loop() {
  measureDistance();  //side distance
  int  usDist;
  usDist = sonar1.getDist();  //front distance
//  Serial.println(usDist);
  //check wall
  if(cm < innerThreshold) {
    //hard left
    nxshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Forward, faster);
    nxshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Forward, slower);
  } else if(cm >= innerThreshold && cm <= outerThreshold) {
    //straight
    nxshield.bank_a.motorRunUnlimited(SH_Motor_Both, SH_Direction_Forward, 50);
  } else {
    //soft right
    nxshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Forward, slower+16);
    nxshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Forward, faster);  
  }
  if(usDist < frontThreshold) {
    nxshield.bank_a.motorRunUnlimited(SH_Motor_1, SH_Direction_Forward, 100);
    nxshield.bank_a.motorRunUnlimited(SH_Motor_2, SH_Direction_Reverse, 100);
    delay(250);
  }
  delay(10);
}

void measureDistance() {

  pinMode(sig, OUTPUT);
  digitalWrite(sig, LOW);
  delay(2);
  digitalWrite(sig, HIGH);
  delay(10);
  digitalWrite(sig, LOW);

  pinMode(sig, INPUT);
  duration = pulseIn(sig, HIGH);

  inches = microsecondsToInches(duration);
  cm = microsecondsToCentemeters(duration);
/*
  Serial.print(inches);
  Serial.print(" in, ");
  Serial.print(cm);
  Serial.print(" centemeters");
  Serial.println();
  */
}

long microsecondsToInches(long microseconds) {
  return microseconds / 74 / 2;
}

long microsecondsToCentemeters(long microseconds) {
  return microseconds / 29 / 2;
}

Learning From the Radio Shack Ultrasonic Sensor

I've used the NXT ultrasonic sensor many times before. The only problem with it from my perspective is that it's a black box. Even in the NXShield library the class function for getting the distance reading is pretty opaque:

uint8_t NXTUS::getDist(){
    writeByte(0x41,0x02);
    delay(20);  // required for ultrasonic to work.
    return readByte(0x42);
}

There is some processing going on in the NXT US itself but it's hidden from view.
By contrast, the Radio Shack sensor reveals its secrets by how it must be wired and programmed. I knew conceptually that both US sensors emit an ultrasonic signal and detect the signal when it is reflected back, converting the time it takes into a unit of measurement. The Radio Shack sensor is wired to the Arduino on 3 pins, SIG to pin 7, VCC to the 5V pin, and GND to GND. To get up and running with it Radio Shack provides an example Arduino program, and I found a simpler program here. I took the latter, simpler program and gave it its own function in my program:

void measureDistance() {

  pinMode(sig, OUTPUT);
  digitalWrite(sig, LOW);
  delay(2);
  digitalWrite(sig, HIGH);
  delay(10);
  digitalWrite(sig, LOW);

  pinMode(sig, INPUT);
  duration = pulseIn(sig, HIGH);

  inches = microsecondsToInches(duration);
  cm = microsecondsToCentemeters(duration);
}

long microsecondsToInches(long microseconds) {
  return microseconds / 74 / 2;
}

long microsecondsToCentemeters(long microseconds) {
  return microseconds / 29 / 2;
}

What's revealed here is the process by which the sensor measures distance. The signal pin is first set to output, set to high, or pulsed on, which emits the signal. Then it is quickly set to an input pin to report back the reflected signal, and the duration is measured with a pulseIn function, which must measure the time it takes for the signal pin to register high again, or the reflected ultrasonic pulse coming back. That duration is then converted to inches and centimeters right there before out eyes. Very cool to see and understand much better how the sensor works, and even get a sense of how long it takes to go through its send/receive cycle.