Datasheet

Tactile Navigation with Whiskers • Chapter 5
Robotics with the BOE Shield-Bot • 163
#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
}
}