BOE Bot navigation with whiskers and visible light
This is useful as navigation by visible light along will not stop your robot from hitting an obstacle. But how to we go about writing a code to allow both kinds of navigation? Consider the following:
- We want the robot to navigate using visible light most of the time, except when...
- ...the whiskers touch something.
- Also, we want the whisker navigation to include the get-out-of-corners code.
How do we do this?
Let's have a look at the navigation by whiskers code. The full code is here. Below we look at a snippit only
BOE Bot whisker navigation code (partial)
...
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
}
}
...The code can be simplified as follows:
- Read whiskers
- check to see if we are in a corner
- Navigate with whiskers
- if both whiskers in contact, back up and turn
- if left whisker in contact, back up and turn right
- opposite for right whisker
ELSE if no whiskers in contact, go forward.
We will retain this structure, but in the final ELSE, when no whiskers are in contact, we let the robot navigate with the photosensitive eyes. This is what I have done in the program included below. Instead of a simple //forward(20)// command when no whiskers are in contact, I have made a call to the //photo_navigate()// function. This is a function I have written based on the photo navigation code you have already used. Additionally, I have modified it to make the navigation smoother. These detials are not essential, but make for a better behaved robot.
One more thing: the code allows you to switch off photo-navigation. If you think there is a bug in your circuit or something is wrong with the code, set //photo_nav = false// and the code will use the whiskers only.
BOE Bot Navigation with whiskers and phototransistors
/*
* A. J. Misquitta
* Robotics with the BOE Shield - LightSeekingShieldBot
* Roams toward light and away from shade and uses whiskers for
* contact navigation.
* The algorithm used is:
* 1: whiskers override everything
* 2: when no whiskers are pressed, photo navigation takes over.
* 3: photo navigation accepts a delta_photo variable to avoid
* fluctuations. 20 seems a reasonable choice.
* 4: LEDs indicate choices made. They should not fluctuate
* excessively.
* 5: photo-navigation sensitivity is set using the photo_sensitivity
* variable. 3000.0 seems to be a good value.
*/
#include <Servo.h> // Include servo library
Servo servoLeft; // Declare left and right servos
Servo servoRight;
// Pins for whiskers and LEDs
int pinLwskr = 5;
int pinRwskr = 7;
int pinLled = 2;
int pinRled = 11;
// Pins for light sensors:
int pinLsensor = 8;
int pinRsensor = 6;
// Pin for buzzer
int pinBuzz = 5;
// some data for whiskers algorithm:
byte whiskersOld;
byte counter; // For counting alternate corners
boolean photo_nav = true; // Set to false to disable photo-nav
int delta_photo = 20; // Used to suppress photo-fluctuations
float photo_sensitivity = 3000.0; // small == less sensitivity
/*
* Motor speeds are variable in the photo-navigation section,
* but are fixed in the whiskers-navigation part.
*/
int forwardL = 1700;
int forwardR = 1300;
int reverseL = 1300;
int reverseR = 1700;
int neutral = 1500;
void setup() // Built-in initialization block
{
tone(pinBuzz, 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
// Initialise the whiskers:
counter = 0;
whiskersOld = 2;
Serial.begin(9600); // Output to Serial
while (! Serial); // Wait untilSerial is ready
Serial.println("BOE bot. Welcome!");
}
void loop()
{
// Main loop. This auto-repeats.
// =============================
// The idea here is to give the whiskers precedence. If there is no
// input from the whiskers then use photo-navigation
//
//whiskers = 2 * digitalRead(pinLwskr);
//whiskers += digitalRead(pinRwskr);
whiskers = read_whiskers();
if (Serial.available()) // In general, output to Serial
{ // only if input is detected.
Serial.print("Whiskers = ");
Serial.println(whiskers);
}
/*
* The Get-Out-Of-A-Corner code:
* Works by saving the previous whisker states. If an alternation
* is detected for a certain number (3 or 4) of times, BOEbot is in
* a corner (likely!). In that case, best to try a near-reverse.
*/
if((whiskers == 1) || (whiskers == 2)) // One whisker pressed?
{ // Alternate from last time?
if (whiskers != whiskersOld)
{
counter++; // Increase count by one
whiskersOld = whiskers;
if(counter == 3) // Stuck in a corner?
{
whiskers == 0; // Set up for U-turn
counter = 0; // Clear alternate corner count
}
}
else // Not alternate from last time
{
counter = 0; // Clear alternate corner count
}
}
/*
* Basic whiskers navigation code:
* Control is passed to the photo-navigation function only if
* neither whisker is pressed.
*/
if(whiskers == 0) // If both whiskers contact
{
digitalWrite(pinLled, HIGH);
digitalWrite(pinRled, HIGH);
backward(1000); // Back up 1 second
turnLeft(800); // Turn left about 120 degrees
}
else if(whiskers == 1) // If only left whisker contact
{
digitalWrite(pinLled, HIGH);
digitalWrite(pinRled, LOW);
backward(1000); // Back up 1 second
turnRight(400); // Turn right about 60 degrees
}
else if(whiskers == 2) // If only right whisker contact
{
digitalWrite(pinRled, HIGH);
digitalWrite(pinLled, LOW);
backward(1000); // Back up 1 second
turnLeft(400); // Turn left about 60 degrees
}
else // Otherwise, no whisker contact
{
// No whisker contact so use photo-navigation (if allowed):
digitalWrite(pinLled, LOW);
digitalWrite(pinRled, LOW);
if (photo_nav)
{
photo_navigate();
}
else
{
forward(20); // Forward 1/50 of a second
}
}
}
byte read_whiskers()
{
// 0 : both touch
// 1 : left
// 2 : right
// 3 : neither touches
byte whiskers;
whiskers = 2 * digitalRead(pinLwskr);
whiskers += digitalRead(pinRwskr);
//Serial.println(whiskers);
return(whiskers);
}
void forward(int time) // Forward function
{
servoLeft.writeMicroseconds(forwardL); // Left wheel counterclockwise
servoRight.writeMicroseconds(forwardR); // Right wheel clockwise
delay(time); // Maneuver for time ms
}
void turnLeft(int time) // Left turn function
{
servoLeft.writeMicroseconds(reverseL); // Left wheel clockwise
servoRight.writeMicroseconds(forwardR); // Right wheel clockwise
delay(time); // Maneuver for time ms
}
void turnRight(int time) // Right turn function
{
servoLeft.writeMicroseconds(forwardL); // Left wheel counterclockwise
servoRight.writeMicroseconds(reverseR); // Right wheel counterclockwise
delay(time); // Maneuver for time ms
}
void backward(int time) // Backward function
{
servoLeft.writeMicroseconds(reverseL); // Left wheel clockwise
servoRight.writeMicroseconds(reverseR); // Right wheel counterclockwise
delay(time); // Maneuver for time ms
}
void photo_navigate()
{
// Photo navigation function
//
float tLeft = float(rcTime(pinLsensor)); // Get left light & make float
float tRight = float(rcTime(pinRsensor)); // Get right light & make float
float ndShade; // Normalized differential shade
ndShade = tRight / (tLeft+tRight) - 0.5; // Calculate it and subtract 0.5
int speedLeft, speedRight; // Declare speed variables
if (ndShade > 0.0) // Shade on right?
{ // Slow down left wheel
speedLeft = int(200.0 - (ndShade * photo_sensitivity));
speedLeft = constrain(speedLeft, -200, 200);
speedRight = 200; // Full speed right wheel
}
else // Shade on Left?
{ // Slow down right wheel
speedRight = int(200.0 + (ndShade * photo_sensitivity));
speedRight = constrain(speedRight, -200, 200);
speedLeft = 200; // Full speed left wheel
}
/*
* Stutter Suppression
* ===================
* If the left and right servo speeds are too close (i.e. not much
* difference in light on the two sides) then we may encounter a
* stutter: that is, the motors can rapidly alternate in speed and
* direction. To suppress this we use the following code. If the
* two speeds are less than delta_photo apart, the two are assigned
* the same speed, taken as the average of the two.
*/
if (abs(speedLeft-speedRight) < delta_photo)
{
int avg = (speedLeft + speedRight)/2;
speedLeft = avg;
speedRight = avg;
}
/*
* Write out the speeds if input is detected on the serial port
*/
if (Serial.available())
{
Serial.print("Speeds L= ");
Serial.print(speedLeft);
Serial.print(" R= ");
Serial.println(speedRight);
}
/*
* Get rolling!!!
*/
maneuver(speedLeft, speedRight, 50); // Set wheel speeds
}
long rcTime(int pin) // rcTime measures decay at pin
{
/*
* This is an RC-time operation. Because we need to measure the decay
* time fairly rapidly, the capacitance in parallel with the photo-transistor
* is quite small (in this case, 0.1 microFarad).
* Time constant = R * C
* = (V/I) * C
* Here, V = 5 V (set by digitalWrite(pin, HIGH)
* I varies depending on incident light.
* = 1.75 mA (bright) => R = V/I = 3 kOhm => RC = 3e-4 sec = 300 msec
* = 0.25 ma (dim) => R = V/I = 20 kOhm => RC = 0.002 sec = 2000 msec
* This is rough. In practice you will see a greater variation.
*/
pinMode(pin, OUTPUT); // Charge capacitor
digitalWrite(pin, HIGH); // ..by setting pin ouput-high
delay(5); // ..for 5 ms
pinMode(pin, INPUT); // Set pin to input
digitalWrite(pin, LOW); // ..with no pullup
long time = micros(); // Mark the time
while(digitalRead(pin)); // Wait for voltage < threshold
time = micros() - time; // Calculate decay time
return time; // Returns decay time
}
// maneuver function
void maneuver(int speedLeft, int speedRight, int msTime)
{
if (speedLeft < speedRight)
{
digitalWrite(pinRled, HIGH);
digitalWrite(pinLled, LOW);
}
else if (speedLeft > speedRight)
{
digitalWrite(pinRled, LOW);
digitalWrite(pinLled, HIGH);
}
else
{
digitalWrite(pinRled, LOW);
digitalWrite(pinLled, LOW);
}
servoLeft.writeMicroseconds(neutral + speedLeft); // Set left servo speed
servoRight.writeMicroseconds(neutral - speedRight); // Set right servo speed
if(msTime==-1) // if msTime = -1
{
servoLeft.detach(); // Stop servo signals
servoRight.detach();
}
delay(msTime); // Delay for msTime
}