/*
Uses the Vex Range Finder in conjunction with 
Pin 13 Triggers the Pulse (Yellow lead)
Pin 10 Recieves the Echo  (Orange lead)
*/

const int Trig_pin1 =  A2;   // Triggers Pulse for RIGHT Ultrasonic Sensor
const int Echo_pin1 = A3;     // Recevies Echo for RIGHT Ultrasonic Sensor
const int Trig_pin2 = A4;   // Triggers Pulse for LEFT Ultrasonic Sensor
const int Echo_pin2 = A5;     // Receives Echo for LEFT Ultrasonic Sensor
const int Trig_pin3 =  A0;   // Triggers Pulse for RIGHT Ultrasonic Sensor
const int Echo_pin3 = A1;     // Recevies Echo for RIGHT Ultrasonic Sensor
long duration1;            // Time it takes for pulse to bounce back to RIGHT Ultrasonic Sensor
long duration2;             // Time it takes for pulse to bounce back to LEFT Ultrasonic Sensor
long duration3;             // Time it takes for pulse to bounce back to LEFT Ultrasonic Sensor
const int motor1 = 10;
const int motor2 = 11;
const int motor3 = 9;


void setup() {
Serial.begin(9600);           // Set up Serial library at 9600 bps

  Serial.println("Initializing...");  // Print Initializing... to confirm code is working with Serial Library 
  Serial.println ("Running");
  // initialize the pulse pin as output:
  pinMode(Trig_pin1, OUTPUT);     
  // initialize the pulse pin as output:
  pinMode(Trig_pin2, OUTPUT);     
  // initialize the echo_pin pin as an input:
  pinMode(Echo_pin1, INPUT);
  // initialize the echo_pin pin as an input:
  pinMode(Echo_pin2, INPUT);
  // initialize the pulse pin as output:
  pinMode(Trig_pin3, OUTPUT);
  // initialize the echo_pin pin as an input:
  pinMode(Echo_pin3, INPUT);
}

void loop(){
  long duration, inches, cm;
  analogWrite(Trig_pin1, 0);
  delayMicroseconds(2);
  analogWrite(Trig_pin1, 255);
  delayMicroseconds(10);
  analogWrite(Trig_pin1, 0);
  duration1 = pulseIn(Echo_pin1, 255);
  Serial.println("duration1: ");
  
  // convert the time into a distance
  inches = microsecondsToInches(duration1);
  cm = microsecondsToCentimeters(duration1);
  
  if(inches < 24 )
  analogWrite(motor1, 255);
  
  if(inches > 25)
  analogWrite(motor1, 0);

  Serial.print(inches);
  Serial.print("in, ");
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();

  delay(10);
  
  /*analogWrite(Trig_pin2, 0);
  delayMicroseconds(2);
  analogWrite(Trig_pin2, 255);
  delayMicroseconds(10);
  analogWrite(Trig_pin2, 0);
  duration1 = pulseIn(Echo_pin2, 255);
  Serial.println("duration2: ");
  
    // convert the time into a distance
  inches = microsecondsToInches(duration1);
  cm = microsecondsToCentimeters(duration1);
  
  if(inches < 24 )
  analogWrite(motor2, 255);
  
  if(inches > 25)
  analogWrite(motor2, 0);

  Serial.print(inches);
  Serial.print("in, ");
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();

  delay(1000); */
  
  analogWrite(Trig_pin3, 0);
  delayMicroseconds(2);
  analogWrite(Trig_pin3, 255);
  delayMicroseconds(10);
  analogWrite(Trig_pin3, 0);
  duration1 = pulseIn(Echo_pin3, 255);
  Serial.println("duration3: ");
  
    // convert the time into a distance
  inches = microsecondsToInches(duration1);
  cm = microsecondsToCentimeters(duration1);
  
  if(inches < 24 )
  while(1==1)
  analogWrite(motor3, 255);
  delay(50);
  analogWrite(motor3, 255);
  
  if(inches > 25)
  analogWrite(motor3, 0);


  Serial.print(inches);
  Serial.print("in, ");
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();

  delay(10); 
}

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;
}

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 / 29 / 2;
}