Activity 4: Artificial Intelligence for Escaping Corners

You may have noticed that with the last sketch, the BOE Shield-Bot tends to get stuck in corners.  As it enters a corner, its left whisker contacts the wall on the left, so it backs up and turns right.  When the BOE Shield-Bot moves forward again, its right whisker contacts the wall on the right, so it backs up and turns left.  Then it contacts the left wall again, and then the right wall again, and so on, until somebody rescues it from its predicament. 

Programming to Escape Corners

RoamingWithWhiskers can be expanded to detect this problem and act upon it.  The trick is to count the number of times that alternate whiskers make contact with objects.  To do this, the sketch has to remember what state each whisker was in during the previous contact.  Then, it has to compare those states to the current whisker contact states.  If they are opposite, then add 1 to a counter.  If the counter goes over a threshold that you (the programmer) have determined, then it’s time to do a U-turn and escape the corner, and also reset the counter.

This next sketch relies on the fact that you can nest if statements, one inside another.    The sketch checks for one condition, and if that condition is true, it checks for another condition within the first if statement’s code block.  We’ll use this technique to detect consecutive alternate whisker contacts in the next sketch.

Example Sketch: EscapingCorners

This sketch will cause your BOE Shield-Bot to execute a reverse and U-turn to escape a corner at either the fourth or fifth alternate whisker press, depending on which one was pressed first.

  • With the power switch in Position 1, enter, save, and upload EscapingCorners. 
  • Test this sketch pressing alternate whiskers as the BOE Shield-Bot roams.  It should execute its reverse and U-turn maneuver after either the fourth or fifth consecutive, alternate whisker contact.
/*
 * Robotics with the BOE Shield - EscapingCorners
 * Count number of alternate whisker contacts, and if it exceeds 4, get out
 * of the corner.
 */

#include <Servo.h>                           // Include servo library
 
Servo servoLeft;                             // Declare left and right servos
Servo servoRight;

byte wLeftOld;                               // Previous loop whisker values
byte wRightOld;                              
byte counter;                                // For counting alternate corners

void setup()                                 // Built-in initialization block
{
  pinMode(7, INPUT);                         // Set right whisker pin to input
  pinMode(5, INPUT);                         // Set left whisker pin to input  
  pinMode(8, OUTPUT);                        // Left LED indicator -> output
  pinMode(2, OUTPUT);                        // Right LED indicator -> output   

  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
 
  wLeftOld = 0;                              // Init. previous whisker states
  wRightOld = 1;  
  counter = 0;                               // Initialize counter to 0
}  
 
void loop()                                  // Main loop auto-repeats
{

  // Corner Escape

  byte wLeft = digitalRead(5);               // Copy right result to wLeft  
  byte wRight = digitalRead(7);              // Copy left result to wRight

  if(wLeft != wRight)                        // One whisker pressed?
  {                                          // Alternate from last time?
    if ((wLeft != wLeftOld) && (wRight != wRightOld))  
    {                                       
      counter++;                             // Increase count by one
      wLeftOld = wLeft;                      // Record current for next rep
      wRightOld = wRight;
      if(counter == 4)                       // Stuck in a corner?
      {
        wLeft = 0;                           // Set up for U-turn
        wRight = 0;
        counter = 0;                         // Clear alternate corner count
      }
    }  
    else                                     // Not alternate from last time
    {
      counter = 0;                           // Clear alternate corner count
    }
  }  

  // Whisker Navigation
  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
}