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.
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.
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.
/* * 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 }
This sketch is a modified version of RoamingWithWhiskers, so we’ll just look at the new code for detecting and escaping corners.
First, three global byte variables are added: wLeftOld, wRightOld, and counter. The wLeftOld and wRightOld variables store the whisker states from a previous whisker contact so that they can be compared with the states of the current contact. Then counter is used to track the number of consecutive, alternate contacts.
byte wLeftOld; // Previous loop whisker values byte wRightOld; byte counter; // For counting alternate corners
These variables are initialized in the setup function. The counter variable can start with zero, but one of the “old” variables has to be set to 1. Since the routine for detecting corners always looks for an alternating pattern, and compares it to the previous alternating pattern, there has to be an initial alternate pattern to start with. So, wLeftOld and wRightOld are assigned initial values in the setup function before the loop function starts checking and modifying their values.
wLeftOld = 0; // Initialize previous whisker wRightOld = 1; // states counter = 0; // Initialize counter to 0
The first thing the code below // Corner Escape has to do is check if one or the other whisker is pressed. A simple way to do this is to use the not-equal operator (!= ) in an if statement. In English, if(wLeft != wRight) means “if the wLeft variable is not equal to the wRight variable…”
// Corner Escape if(wLeft != wRight) // One whisker pressed?
If they are not equal it means one whisker is pressed, and the sketch has to check whether it’s the opposite pattern as the previous whisker contact. To do that, a nested if statement checks if the current wLeft value is different from the previous one and if the current wRight value is different from the previous one. That’s if( (wLeft != wLeftOld) && (wRight != wRightOld)). If both conditions are true, it’s time to add 1 to the counter variable that tracks alternate whisker contacts. It’s also time to remember the current whisker pattern by setting wLeftOld equal to the current wLeft and wRightOld equal to the current wRight.
if((wLeft != wLeftOld) && (wRight != wRightOld)) { counter++; // Increase count by one wLeftOld = wLeft; // Record current for next rep wRightOld = wRight;
If this is the fourth consecutive alternate whisker contact, then it’s time to reset the counter variable to 0 and execute a U-turn. When the if(counter == 4) statement is true, its code block tricks the whisker navigation routine into thinking both whiskers are pressed. How does it do that? It sets both wLeft and wRight to zero. This makes the whisker navigation routine think both whiskers are pressed, so it makes a U-turn.
if(counter == 4) // Stuck in a corner? { wLeft = 0; // Set up whisker states for U-turn wRight = 0; counter = 0; // Clear alternate corner count } }
But, if the conditions in if((wLeft != wLeftOld) && (wRight != wRightOld)) are not all true, it means that this is not a sequence of alternating whisker contacts anymore, so the BOE Shield-Bot must not be stuck in a corner. In that case, the counter variable is set to zero so that it can start counting again when it really does find a corner.
else // Not alternate from last time { counter = 0; // Clear alternate corner count } }
One thing that can be tricky about nested if statements is keeping track of opening and closing braces for each statement’s code block. The picture below shows some nested if statements from the last sketch. In the Arduino editor, you can double-click on a brace to highlight its code block. But sometimes, printing out the code and simply drawing lines to connect opening and closing braces helps to see all the blocks at once, which is useful for finding bugs in deeply nested code.
In this picture, the if(wLeft != wRight) statement’s code block contains all the rest of the decision-making code. If it turns out that wLeft is equal to wRight, the Arduino skips to whatever code follows that last closing brace }. The second level if statement compares the old and new wLeft and wRight values with if ((wLeft != wLeftOld) && (wRight != wRightOld)). Notice that its code block ending brace is just below the one for the if(counter==4) block. The if ((wLeft != wLeftOld) && (wRight != wRightOld)) statement also has an else condition with a block that sets counter to zero if the whisker values are not opposite from those of the previous contact.
One of the if statements in EscapingCorners checks to see if counter has reached 4.