
<<TableOfContents()>>

= 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. [[AJMPublic/teaching/arduino-pi/projects/arduino/boe-bot/whiskers-navigation-code|The full code is here.]] Below we look at a snippit only

[[attachment:Bot_whisker_navigation_code__partial_|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. 

[[attachment:__BOE_Bot_Navigation_with_whiskers_and_phototransistors | 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
}

}}}