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
}