Datasheet

Tactile Navigation with Whiskers • Chapter 5
Robotics with the BOE Shield-Bot • 171
backward(1000); // Back up 1 second
turnLeft(800); // Turn left about 120 degrees
}
else if(wLeft == 0) // If only left whisker contact
{
pause(500); // Pause motion for 0.5 seconds
backward(1000); // Back up 1 second
turnRight(400); // Turn right about 60 degrees
}
else if(wRight == 0) // If only right whisker contact
{
pause(500); // Pause motion for 0.5 seconds
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
}
}
3. Solution:
void pause(int time) // Pause drive wheels
{
servoLeft.writeMicroseconds(1500); // Left wheel stay still
servoRight.writeMicroseconds(1500); // Right wheel stay still
delay(time); // Maneuver for time ms
}
4. Make sure not to call this pause function in the else condition because
the
forward function is only supposed to go forward for 20 ms before
checking the whiskers again.
void loop() // Main loop auto-repeats
{
byte wLeft = digitalRead(5); // Copy right result to wLeft
byte wRight = digitalRead(7); // Copy left result to wRight
if((wLeft == 0) && (wRight == 0)) // If both whiskers contact
{
pause(500); // Pause motion for 0.5 seconds
backward(1000); // Back up 1 second
turnLeft(800); // Turn left about 120 degrees
}
else if(wLeft == 0) // If only left whisker contact
{
pause(500); // Pause motion for 0.5 seconds
backward(1000); // Back up 1 second