Example Sketch: AvoidTableEdge
For the most part, programming your BOE Shield-Bot to navigate around a tabletop without going over the edge is a matter of adjusting the if…else if…else statements from FastIrRoaming. First of all, instead of backing up, it will need to go forward 20 ms at a time when it sees objects with both detectors. It will also need to turn toward objects instead of away from them, and it will need to turn for more than 20 ms when it sees the drop-off. 375 ms turns seem to work well, but it will be up to you to adjust that value for best performance.
- Open FastIrNavigation and save it as AvoidTableEdge.
- Modify the sketch so that it matches the Example Sketch. Pay close attention to the conditions and maneuver calls in the loop function. The condition that used to go forward for 20 ms now backs up for 250 ms. Likewise, the condition that used to back up now goes forward for 20 ms. Also, the condition that used to call for a 20 ms right turn now calls for a 375 ms left turn, and the condition that used to call for a 20 ms left turn now calls for a 375 ms right turn.
- Test the sketch on your electrical tape or paint delimited course.
- If you decide to try a tabletop, remember to follow the testing and spotting tips discussed earlier.
/*
* Robotics with the BOE Shield - AvoidTableEdge
* Adaptation of FastIrRoaming for table edge avoidance
*/
#include <Servo.h> // Include servo library
Servo servoLeft; // Declare left and right servos
Servo servoRight;
void setup() // Built-in initialization block
{
pinMode(10, INPUT); pinMode(9, OUTPUT); // Left IR LED & Receiver
pinMode(3, INPUT); pinMode(2, OUTPUT); // Right IR LED & Receiver
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
}
void loop() // Main loop auto-repeats
{
int irLeft = irDetect(9, 10, 38000); // Check for object on left
int irRight = irDetect(2, 3, 38000); // Check for object on right
if((irLeft == 0) && (irRight == 0)) // Both sides see table surface
{
maneuver(200, 200, 20); // Forward 20 milliseconds
}
else if(irLeft == 0) // Left OK, drop-off on right
{
maneuver(-200, 200, 375); // Left for 375 ms
}
else if(irRight == 0) // Right OK, drop-off on left
{
maneuver(200, -200, 375); // Right for 375 ms
}
else // Drop-off straight ahead
{
maneuver(-200, -200, 250); // Backward 250 ms before retry
}
}
int irDetect(int irLedPin, int irReceiverPin, long frequency)
{
tone(irLedPin, frequency, 8); // IRLED 38 kHz for at least 1 ms
delay(1); // Wait 1 ms
int ir = digitalRead(irReceiverPin); // IR receiver -> ir variable
delay(1); // Down time before recheck
return ir; // Return 1 no detect, 0 detect
}
void maneuver(int speedLeft, int speedRight, int msTime)
{
// speedLeft, speedRight ranges: Backward Linear Stop Linear Forward
// -200 -100......0......100 200
servoLeft.writeMicroseconds(1500 + speedLeft); // Set left servo speed
servoRight.writeMicroseconds(1500 - speedRight); // Set right servo speed
if(msTime==-1) // if msTime = -1
{
servoLeft.detach(); // Stop servo signals
servoRight.detach();
}
delay(msTime); // Delay for msTime
}
How AvoidTableEdge Works
Since AvoidTableEdge is just FastIrRoaming with a modified if…else if…else statement in it’s loop function, let’s look at the two statements side by side.
// From FastIrRoaming
if((irLeft == 0) && (irRight == 0))
{
maneuver(-200, -200, 20);
}
else if(irLeft == 0)
{
maneuver(200, -200, 20);
}
else if(irRight == 0)
{
maneuver(-200, 200, 20);
}
else
{
maneuver(200, 200, 20);
}
|
//From AvoidTableEdge
if((irLeft == 0) && (irRight == 0))
{
maneuver(200, 200, 20);
}
else if(irLeft == 0)
{
maneuver(-200, 200, 375);
}
else if(irRight == 0)
{
maneuver(200, -200, 375);
}
else
{
maneuver(-200, -200, 250);
}
}
|
In response to if((irLeft == 0) && (irRight == 0)), FastIrRoaming backs up with maneuver(-200, -200, 20) because both IR detectors see an obstacle. In contrast, AvoidTableEdge goes forward with maneuver(200, 200, 20) because both IR detectors see the table, which means it’s safe to move forward for another 20 ms.
In response to else if(irLeft == 0), FastIrRoaming turns right for 20 ms, taking a step toward avoiding an obstacle on the left with maneuver(200, -200, 20). AvoidTableEdge instead turns away from a drop-off that must be on it’s right. That’s because the first if statement where both IR detectors could see the table was not true. If it was, the if…else if…else statement would not have made it to this else if condition. It means the left IR detector does see the table, but the right one does not. So the code makes the BOE Shield turn left for 0.375 seconds with maneuver(-200, 200, 375). This should make it avoid a drop-off that must have been detected on the right.
In response to else if (irRight == 0), FastIrRoaming turns left for 20 ms, taking an incremental step toward avoiding an obstacle on the right. At this point in the if…else if…else statement, we know that the right IR detector does see an object, but the left one does not. Again, that’s because, it would have handled the condition where both IR detectors see the table with the first if statement. So, the left IR detector does not see the object and turns right for 0.375 seconds with maneuver(200, -200, 375).
Your Turn
The 375 ms turns to avoid the table edge can be adjusted for different applications. For example, if the BOE Shield-Bot is supposed to hug the edge of the table, smaller turns might be useful. In a contest where the BOE Shield-Bot is supposed to push objects out of an area, a larger turn (but not too large) would be better so that it zigzags back and forth across the table.
You can modify the code to make shallower turns by using a smaller msTime parameter value in the maneuver function calls. For example, if you change the 375 in maneuver(-200, 200, 375) to 300, it will make shallower left turns. If you change it to 450, it will make sharper left turns.
- Modify AvoidTableEdge so that it closely follows the edge of the simulated drop-off course. Adjust the msTime parameter values in calls to maneuver to make the BOE Shield-Bot execute smaller turns when it sees the drop-off. How small can you make the turn before it tries to fall off?
- Experiment with pivoting away from the table edge to make the BOE Shield-Bot roam inside the perimeter instead of following the edge.