Activity 3: Navigation with Whiskers

Previously, our sketches only made the BOE Shield-Bot execute a list of movements predefined by you, the programmer.  Now that you can write a sketch to make the Arduino monitor whisker switches and trigger action in response, you can also write a sketch that lets the BOE Shield-Bot drive and select its own maneuver if it bumps into something. This is an example of autonomous robot navigation.

Whisker Navigation Overview

The RoamingWithWhiskers sketch makes the BOE Shield-Bot go forward while monitoring its whisker inputs, until it encounters an obstacle with one or both of them.  As soon as the Arduino senses whisker electrical contact, it uses an if…else if…else statement to decide what to do.  The decision code checks for various whisker pressed/not pressed combinations, and calls navigation functions from Chapter 4 to execute back-up-and-turn maneuvers.  Then, the BOE Shield-Bot resumes forward motion until it bumps into another obstacle.

Example Sketch: RoamingWithWhiskers

Let’s try the sketch first, and then take a closer look at how it works.

  • Set the 3-position switch to position 1.
  • Reconnect the BOE Shield-Bot’s battery pack to the Arduino.
  • Enter, save, and upload RoamingWithWhiskers.
  • Disconnect the BOE Shield-Bot from its programming cable, and set the power switch to 2.
  • Put the BOE Shield-Bot on the floor, and try letting it roam.  When it contacts obstacles in its path with its whisker switches, it should back up, turn, and then roam in a new direction.
// Robotics with the BOE Shield - RoamingWithWhiskers
// Go forward.  Back up and turn if whiskers indicate BOE Shield bot bumped
// into something.

#include <Servo.h>                           // Include servo library
 
Servo servoLeft;                             // Declare left and right servos
Servo servoRight;
 
void setup()                                 // Built-in initialization block
{
  pinMode(7, INPUT);                         // Set right whisker pin to input
  pinMode(5, INPUT);                         // Set left whisker pin to input  

  tone(4, 3000, 1000);                       // Play tone for 1 second
  delay(1000);                               // Delay to finish tone

  servoLeft.attach(13);                      // Attach left signal to pin 13
  servoRight.attach(12);                     // Attach right signal to pin 12
}  
 
void loop()                                  // Main loop auto-repeats
{
  byte wLeft = digitalRead(5);               // Copy left result to wLeft  
  byte wRight = digitalRead(7);              // Copy right result to wRight

  if((wLeft == 0) && (wRight == 0))          // If both whiskers contact
  {
    backward(1000);                          // Back up 1 second
    turnLeft(800);                           // Turn left about 120 degrees
  }
  else if(wLeft == 0)                        // If only left whisker contact
  {
    backward(1000);                          // Back up 1 second
    turnRight(400);                          // Turn right about 60 degrees
  }
  else if(wRight == 0)                       // If only right whisker contact
  {
    backward(1000);                          // Back up 1 second
    turnLeft(400);                           // Turn left about 60 degrees
  }
  else                                       // Otherwise, no whisker contact
  {
    forward(20);                             // Forward 1/50 of a second
  }
}

void forward(int time)                       // Forward function
{
  servoLeft.writeMicroseconds(1700);         // Left wheel counterclockwise
  servoRight.writeMicroseconds(1300);        // Right wheel clockwise
  delay(time);                               // Maneuver for time ms
}

void turnLeft(int time)                      // Left turn function
{
  servoLeft.writeMicroseconds(1300);         // Left wheel clockwise
  servoRight.writeMicroseconds(1300);        // Right wheel clockwise
  delay(time);                               // Maneuver for time ms
}

void turnRight(int time)                     // Right turn function
{
  servoLeft.writeMicroseconds(1700);         // Left wheel counterclockwise
  servoRight.writeMicroseconds(1700);        // Right wheel counterclockwise
  delay(time);                               // Maneuver for time ms
}

void backward(int time)                      // Backward function
{
  servoLeft.writeMicroseconds(1300);         // Left wheel clockwise
  servoRight.writeMicroseconds(1700);        // Right wheel counterclockwise
  delay(time);                               // Maneuver for time ms
}