BOE Bot: IR & Visible navigation code : 1

/*
 * AJM
 * Robotics with the BOE Shield - LightSeekingShieldBot
 * Roams toward light and away from shade and uses IRtriggers for 
 * contact navigation. 
 * The algorithm used is:
 * 1: IRtriggers override everything
 * 2: when no IRtriggers 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.
 * 6: Attempting to get out of corners by reducing photo sensitivity
 *    when corners are encountered. 
 */

#include <Servo.h>                           // Include servo library
 
Servo servoLeft;                             // Declare left and right servos
Servo servoRight;

// Pins for buzzer, IR receivers and IR-LEDs 
int pinBuzz = 6;
int pinLir = 8;
int pinRir = 4;
int pinLrecv = 9;
int pinRrecv = 5;
int pinLled = 11;
int pinRled = 2;
// Pins for visible light sensors:
int pinLsensor = 10;
int pinRsensor = 3;

// some data for IR-navigation algorithm:
byte IRtriggersOld;                           
byte counter;                                // For counting alternate corners
int irFreq = 38000;

boolean photo_nav = true;                   // Set to false to disable photo-nav
int delta_photo = 40;                        // Used to suppress photo-fluctuations
float photo_sensitivity_default = 2000.0;
float photo_sensitivity = 2000.0;            // small == less sensitivity

/* 
 * Motor speeds are variable in the photo-navigation section,
 * but are fixed in the IRtriggers-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 IR navigation variables:
  counter = 0;
  IRtriggersOld = 2;
  
  pinMode(pinLrecv, INPUT);
  pinMode(pinLir, OUTPUT);   // Left IR LED & Receiver
  pinMode(pinRrecv, INPUT);
  pinMode(pinRir, OUTPUT);   // Right IR LED & Receiver
  pinMode(pinLled, OUTPUT);  // Left and Right LEDs
  pinMode(pinRled, OUTPUT);
  
  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 IRtriggers precedence. If there is no
  // input from the IRtriggers then use photo-navigation
  //
  
  int IRtriggers;
  
  IRtriggers = read_IRreceivers();

  if (Serial.available())                    // In general, output to Serial
  {                                          // only if input is detected.
    Serial.print("IR triggers = ");
    Serial.println(IRtriggers);
  }

  /* 
   * The Get-Out-Of-A-Corner code:
   * Works by saving the previous IR trigger 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((IRtriggers == 1) || (IRtriggers == 2)) // One side triggered?
  {                                          // Alternate from last time?
    if (IRtriggers != IRtriggersOld)  
    {                                       
      counter++;                             // Increase count by one
      IRtriggersOld = IRtriggers;
      photo_sensitivity -= 100.0;
      if(counter == 3)                       // Stuck in a corner?
      {
        IRtriggers == 0;                       // Set up for U-turn
        counter = 0;                         // Clear alternate corner count
        photo_sensitivity = photo_sensitivity_default;
      }
    }  
    else                                     // Not alternate from last time
    {
      counter = 0;                           // Clear alternate corner count
    }
  }  

  /*
   * Basic IRtriggers navigation code:
   * Control is passed to the photo-navigation function only if
   * neither IR detector is triggered.
   */
  if(IRtriggers == 0)          // If both IRtriggers contact
  {
    digitalWrite(pinLled, HIGH);
    digitalWrite(pinRled, HIGH);
    backward(500);                          // Back up 0.5 second
    turnLeft(50);                          // Turn left about 60 degrees
  }
  else if(IRtriggers == 1)                   // If only left detector triggered
  {
    digitalWrite(pinLled, HIGH);
    digitalWrite(pinRled, LOW);
    //backward(200);                           // Back up 0.2 second
    turnRight(50);
    //pivotRight(100);
  }
  else if(IRtriggers == 2)                   // If only right detector triggered
  {
    digitalWrite(pinRled, HIGH);
    digitalWrite(pinLled, LOW);
    //backward(200);                           // Back up 0.2 second
    turnLeft(50);
    //pivotLeft(100);
  }
  else                                       // Otherwise, no IR trigger
  {
    // No IR trigger so use photo-navigation (if allowed):
    digitalWrite(pinLled, LOW);
    digitalWrite(pinRled, LOW);
    if (photo_nav)
    {
      photo_navigate(100);
    }
    else
    {
      forward(100);
    }
  }
}

// IR Object Detection Function

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
} 
byte read_IRreceivers()
{
  // Return:
  // 0 : both trigger
  // 1 : left
  // 2 : right
  // 3 : neither trigger
  byte IRtriggers;
  int irL = irDetect(pinLir, pinLrecv, irFreq);
  int irR = irDetect(pinRir, pinRrecv, irFreq);
  IRtriggers = 2 * irL;
  IRtriggers += irR;
  //Serial.println(IRtriggers);
  return(IRtriggers);
}

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 pivotLeft(int time)                      // Left pivot function
{
  servoLeft.writeMicroseconds(reverseL);     // Left wheel clockwise
  servoRight.writeMicroseconds(neutral);     // Right wheel stops
  delay(time);                               // Maneuver for time ms
}

void pivotRight(int time)                    // Right pivot function
{
  servoLeft.writeMicroseconds(neutral);      // Left wheel stops
  servoRight.writeMicroseconds(reverseR);    // Right wheel counterclockwise
  delay(time);                               // Maneuver for time ms
}

void photo_navigate(int timems) 
{
  // 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);
    Serial.print("   Reading L=");
    Serial.print(tLeft);
    Serial.print("   Reading R=");
    Serial.print(tRight);
    Serial.print("   ndShade =");
    Serial.println(ndShade);
  }

  /*
   * Get rolling!!!
   */
  maneuver(speedLeft, speedRight, timems);
}

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 (abs(speedLeft) < abs(speedRight))
  {
    digitalWrite(pinRled, HIGH);
    digitalWrite(pinLled, LOW);
  } 
  else if (abs(speedLeft) > abs(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
}