/*
  Distance Sensor Test

  Code for determining if the robot is close to an obstacle.
  If it is, backs off, changes direction, and then resumes what it was doing prior.

 */


#include <Servo.h>

// Variable to determine if the LED is on or off
int sensorLEDoff;
int sensorLEDon;

// Variable for storing the value difference between LED on and off
int compareValue;

// Tell the Arduino there is a standard servo
Servo StandardServo1;

// Tell the Arduino there are to continuous servos
Servo ContinuousServo1;  
Servo ContinuousServo2;

// establish variables for duration of the ping,
// and the distance result in inches:
long duration, inches;

void setup() {
  
  // Attach the standard servo to pin 5
  StandardServo1.attach(5);

  // Attach the continuous servos to pins 6 and 7
  ContinuousServo1.attach(6); 
  ContinuousServo2.attach(7);
  
  // Set the standard servo to a neutral position
  StandardServo1.write(70);            
                      
  //Pause the continuous rotation servos
  ContinuousServo1.write(94); 
  ContinuousServo2.write(94); 

  //Wait for 2 seconds
  delay(2000); 
}

void loop() {

    // Finally, read the distance sensor to see if anything is in the way.
    distanceMeasurement();
    
    // If something is in the way, stop and backup
    if(inches <10){
      backOff();
    }

  
}

// A function which stops the robot, backs off, and changes direction.
void backOff(){

    //STOP!
    stopDriving();  
    delay(1000);

    //look down
    StandardServo1.write(30); 
    delay(2000);

    //look back up
    StandardServo1.write(70);
    delay(2000);

    //reverse
    backward();
    delay(1000);

    //stop
    stopDriving();
    delay(1000);

    //turn right
    right();
    delay(1000);

    //and stop
    stopDriving();
    delay(1000);

}

//Function for distance sensing
void distanceMeasurement(){

  // 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:
  pinMode(4, OUTPUT);
  digitalWrite(4, LOW);
  delayMicroseconds(2);
  digitalWrite(4, HIGH);
  delayMicroseconds(5);
  digitalWrite(4, 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(4, INPUT);
  duration = pulseIn(4, HIGH);

  // convert the time into a distance
  inches = microsecondsToInches(duration);
}

long microsecondsToInches(long microseconds) {
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second).  This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  return microseconds / 74 / 2;
}


void stopDriving() {
  ContinuousServo1.write(94); 
  ContinuousServo2.write(94); 
}

void forward(){
  ContinuousServo1.write(84);
  ContinuousServo2.write(104); 
}

void backward(){
  ContinuousServo1.write(104);
  ContinuousServo2.write(84); 
}

void right(){
  ContinuousServo1.write(104);
  ContinuousServo2.write(104); 
}

void left(){
  ContinuousServo1.write(84);
  ContinuousServo2.write(84); 
}