/*
 Autonomous roving robot
 Parts:
   -Arduino Uno
   -Parallax PING))) ultrasonic sensor
   -Adafruit motor shield http://www.ladyada.net/make/mshield/
   -2WD Mobile Platform http://www.makershed.com/ProductDetails.asp?ProductCode=MKSEEED7
   -Lever switch (for back bumper)
   -Indicator LED
   -Energizer XPAL XP8000 battery (custom cable and self-built regulator for motors)
 Original motion functions and design from Robot Living 8/30/2010
 Code borrowed from example sketches:
   -Ping
 Original code created June 2011
 by Daniel Gentleman
 thoughtfix@gmail.com
 http://thoughtfix.com

Additional credits:
 Victor Brilon
 Mark Balliet
 luckylarry.co.uk for the math on the Sharp sensor
 Advice from arduino.cc forum
 Advice from from #arduino on irc.freenode.net
 Advice from adafruit forums
 Further inspiration
      -Adafruit Industries
      -Make Magazine
 Adafruit Motor shield library

 Wiring (remember, analog 0 is also digital 14)
 - Left motor to Motor 1
 - Right motor to Motor 2
 - LEDs on digital 15 and 16
 - PING))) on digital 17
 - Sharp IR sensors on digital 18 and 19
 - Lever switch (bumper attached) on digital 14
 - USB power (from a USB battery) to the Arduino
 - Regulated power (I used an Energizer XPAL XP8000 regulated to +5v) for servos
*/


#include <AFMotor.h>

AF_DCMotor motorL(1, MOTOR12_1KHZ);
AF_DCMotor motorR(2, MOTOR12_1KHZ);


int BSensor = 14; //Back sensor
int FLEDPin = 15; //Forward LED
int BLEDPin = 16; //Backup LED
int pingPin = 17; //Front sensor
int FSensorR = 18; //Front right sensor
int FSensorL = 19;  //Front left sensor
int count = 0;
int b=0;
int bs=0;
int sensorValue;
int forwardSensor;
int sensorValueB=1;
int leftSensor=0;
int rightSensor=0;
int obstacle=0;


void setup() {
  Serial.begin(9600);
  randomSeed(analogRead(0));
  pinMode(BLEDPin, OUTPUT);
  pinMode(FLEDPin, OUTPUT);
  pinMode(BSensor, INPUT);
  pinMode(pingPin, INPUT);
  pinMode(FSensorR, INPUT);
  pinMode(FSensorL, INPUT);
  motorL.setSpeed(200);
  motorR.setSpeed(200);
  motorL.run(RELEASE);
  motorR.run(RELEASE);
}

void loop()
{
  uint8_t i;
  Forward();
}

void Forward () // Charge forth!
{
  digitalWrite(FLEDPin, HIGH);
  digitalWrite(BLEDPin, LOW);
  ForwardSensor ();
  if (obstacle == 0){ //unless something is in the way
  Serial.println("Going forward.");
  motorL.run(FORWARD);
  motorR.run(FORWARD);
  }

}

void ForwardSensor ()
{
  leftSensor = (ForwardSensorLeft());
  rightSensor =(ForwardSensorRight());
  forwardSensor = (getDistance());
  delay(30);
  //  Serial.print(leftSensor);
  //  Serial.println(" left");
  //  Serial.print(rightSensor);
  //  Serial.println(" right");
  //  Serial.print(forwardSensor);
  //  Serial.println(" front");
  if (forwardSensor <=30 || leftSensor <= 30 || rightSensor <= 30 )
  { // I gave 10cm of "wiggle room" so it doesn't turn endlessly.
    obstacle=1;
    if (ForwardSensorRight() > (ForwardSensorLeft()-10)){
      RightBackward();
    }
    else if (ForwardSensorRight() < (ForwardSensorLeft()-10)){
      LeftBackward();
    }
    else{
      Backward();
    }
  }
  else {
    obstacle = 0;
  }
  sensorValue=0;
  rightSensor=0;
  leftSensor=0;
}

int ForwardSensorRight ()
{ // This function is here so you don't have to re-write code
  // if you use a different sensor.
  sensorValue = irDistance(FSensorR);
  return sensorValue;
}

int ForwardSensorLeft ()
{ // This function is here so you don't have to re-write code
  // if you use a different sensor.
  sensorValue = irDistance(FSensorL);
  return sensorValue;
}

void BackwardSensor ()
{
  sensorValueB = digitalRead(BSensor);
  if (sensorValueB == 1)
  {
    Serial.println("Object detected while going backwards.");
    motorL.run(RELEASE);
    motorR.run(RELEASE);
    for (bs=0; bs <= 3; bs++) // Add blinky lights for personality on detecting object when backing up.
    {
      digitalWrite(FLEDPin, HIGH);
      digitalWrite(BLEDPin, HIGH);
      delay(5);
      digitalWrite(FLEDPin, LOW);
      digitalWrite(BLEDPin, LOW);
      delay (5);
    }
    b=31;
    bs=0;
    Forward();

  }
  sensorValueB = 0;
}

void Backward ()
{
  motorL.run(RELEASE);
  motorR.run(RELEASE);
  digitalWrite(FLEDPin, LOW);
  digitalWrite(BLEDPin, HIGH);
  Serial.println("Going backwards.");

  do
  {
    b++;
    motorL.run(BACKWARD);
    motorR.run(BACKWARD);
    delay(20);
    BackwardSensor ();
  }
  while (b < 20);

  b=0;
}


void LeftBackward ()
{
  motorL.run(RELEASE);
  motorR.run(RELEASE);
  digitalWrite(FLEDPin, LOW);
  digitalWrite(BLEDPin, HIGH);
  Serial.println("Going left backwards.");

  do
  {
    b++;
    //    Serial.print("b = ");
    //    Serial.print(b);
    motorL.run(BACKWARD);
    motorR.run(RELEASE);
    BackwardSensor ();
    delay (20);
  }
  while (b < 20);

  b=0;
}

void RightBackward ()
{
  motorL.run(RELEASE);
  motorR.run(RELEASE);
  digitalWrite(FLEDPin, LOW);
  digitalWrite(BLEDPin, HIGH);
  Serial.println("Going right backwards.");

  do
  {
    b++;
    //    Serial.print("b = ");
    //    Serial.print(b);
    motorL.run(RELEASE);
    motorR.run(BACKWARD);
    BackwardSensor ();
    delay (20);
  }
  while (b < 20);

  b=0;
}

int getDistance()
{
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
reread:  // takes another reading if cm=0
  long duration, cm;
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

  // convert the time into a distance
  cm = microsecondsToCentimeters(duration);
  //  Serial.print(cm);
  //  Serial.print("cm");
  //  Serial.println();
  delay(100);
  //  if (cm == 0) {
  //    goto reread;
  //  }
  return cm;
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return (microseconds/58);
}

int irDistance(int irPin) {
  float volts = analogRead(irPin)*0.0048828125;   // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
  int distance = 32.5*pow(volts, -1.10);          // theretical distance 32.5/ (1/Volts)S
  //  Serial.print(distance);
  //  Serial.print(" from ");
  //  Serial.println(irPin);
  delay (10);
  return distance;        // print the raw analog value to serial port
}