/* Using the 5 IR Distance to control the servo to avoid obstacles

Created August 13th, 2012

Edited on November 23rd, 2012
--made turns less jerky

Edited on December 22nd, 2012
--used the "map" function to have the IR sensors create a ratio with the servo angle based on the distance from walls/ obstacles


*/

#include "pitches.h"

#include <Servo.h>  //include the Servo library

#include <DistanceGP2Y0A21YK.h>  //includes the proximity sensor's library

Servo Steering;  //Creates the servo object named "Steering"
Servo Motor;     //Creates the servo object for the motor

DistanceGP2Y0A21YK Dist01;  //defines Dist01 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist02;  //defines Dist02 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist03;  //defines Dist03 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist04;  //defines Dist04 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist05;  //defines Dist05 as the sharp GP2Y0A21YK

int Prox01;  //creates the integer for the first IR sensor
int Prox02;  //creates the integer for the second IR sensor
int Prox03;  //creates the integer for the third IR sensor
int Prox04;  //creates the integer for the fourth IR sensor
int Prox05;  //creates the integer for the fifth IR sensor


int rightSide;
int leftSide;



int spd = 100;  //sets the speed of the dc motor

int melody[] = {
  NOTE_F6, 0, NOTE_F6, 0, NOTE_F6, 0, NOTE_F6, 0};  //melody to be played when backing the car up
int noteDurations[] = {
   2,6,2,6,2,6,2,6 };

int ResetLed = 12;    //Reset LED
int ReverseLed = 11;  //Reverse LED
int AlertLed = 13;    //Alert (Backwards) LED

void setup()
{
  Serial.begin(9600);  //opens the serial monitor at 9600 baud
  Dist01.begin(A0);    //tells program "Dist01" will be used by the serial monitor
  Dist02.begin(A1);    //tells program "Dist02" will be used by the serial monitor
  Dist03.begin(A2);    //tells program "Dist03" will be used by the serial monitor
  Dist04.begin(A3);    //tells program "Dist04" will be used by the serial monitor
  Dist05.begin(A4);    //tells program "Dist05" will be used by the serial monitor
  
  
  Steering.attach(7);  //attaches the steering servo to pin 10
  Motor.attach(9);      //attached the motor to PWM pin 9
  
   
  Serial.println("\n  --Sensor01--Sensor02--Sensor03--Sensor04--Sensor05--");

  pinMode(ReverseLed, OUTPUT);
  digitalWrite(ReverseLed, LOW);

  pinMode(AlertLed,OUTPUT);
  digitalWrite(AlertLed, LOW);

  pinMode(ResetLed, OUTPUT);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW); 
  Motor.write(spd);
  
  Steering.write(125);
  delay(1000);
  Steering.write(65);
  delay(1000);
  digitalWrite(ResetLed, HIGH);
  delay(500);
  digitalWrite(ResetLed, LOW);
  Steering.write(95);
  
  
  
}

/*

Although below, it says "getDistanceCentimeter", it really converts the data into Inches---make a very big note for this!!

*/




void loop()
{
  Prox01 = Dist01.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist01)
  Prox02 = Dist02.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist02)
  Prox03 = Dist03.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist03)
  Prox04 = Dist04.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist04)
  Prox05 = Dist05.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist05)

Serial.print(" ------");
Serial.print(Prox01);
Serial.print("--------");
Serial.print(Prox02);
Serial.print("--------");
Serial.print(Prox03);
Serial.print("--------");
Serial.print(Prox04);
Serial.print("--------");
Serial.print(Prox05);
Serial.println("------");
Serial.println(rightSide);
Serial.println(leftSide);
delay(50);



if ((Prox01 <= 10) || (Prox02 <= 10))
  Steering.write(125);
else if ((Prox04 <= 10) || (Prox05 <= 10))
  Steering.write(65);
else if ((Prox01 <= 15) || (Prox02 <= 15))
  Steering.write(115);
else if ((Prox04 <= 15) || (Prox05 <= 15))
  Steering.write(75);
else if ((Prox01 <= 20) || (Prox02 <= 20))
  Steering.write(110);
else if ((Prox04 <= 20) || (Prox05 <= 20))
  Steering.write(90);
else if ((Prox01 <= 25) || (Prox02 <= 25))
  Steering.write(105);
else if ((Prox04 <= 25) || (Prox05 <= 25))
  Steering.write(85);
else if ((Prox01 >15) && (Prox02 >15) && (Prox03 >15) && (Prox04 >15) && (Prox05 >15))
  Steering.write(95);



else if (Prox03 <= 20)
  FrontAlert();

//delay(100);

}

void BackUpNoise() {                                   //Makes the "Backing Up" noise
  for (int thisNote = 0; thisNote < 9; thisNote++) {
    int noteDuration = 1000/noteDurations[thisNote];
    tone(8, melody[thisNote], noteDuration);
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
    noTone(8);
  }
}


void REVERSE() {                                        //Throws the car in reverse and turns left
  Motor.write(140);
  digitalWrite(ReverseLed, HIGH);
  delay(500);
  Steering.write(95);
  Motor.write(130);
  delay(500);
  Motor.write(110);
  delay(500);
  Motor.write(115);
  BackUpNoise();
  Motor.write(110);
  Steering.write(120);
  delay(1500);
  Steering.write(60);
  Motor.write(100);
  delay(500);
  Motor.write(spd);
  digitalWrite(ReverseLed, LOW);
  
}

void FrontAlert() {                                     //Warns that the car senses something in front of it
  Steering.write(95);
  digitalWrite(AlertLed, HIGH);
  delay(100);
  digitalWrite(AlertLed, LOW);
  delay(100);
  digitalWrite(AlertLed, HIGH);
  delay(100);
  digitalWrite(AlertLed, LOW);
  delay(100);
  digitalWrite(AlertLed, HIGH);
  delay(100);
  digitalWrite(AlertLed, LOW);
  delay(100);
  REVERSE();
  
}

//read both sensors from left side, take each value and map it to the servo (between 90 and 180)
//read both sensors from right side, take each value and map it to the servo (between 0 and 90)

/*if ((Prox01 >15) && (Prox02 >15) && (Prox03 >15) && (Prox04 >15) && (Prox05 >15))
  Steering.write(95);
  
else if (Prox01 <=25)
  {
    leftSide = Prox01;
    leftSide = map(leftSide, 5, 25, 130, 95);
    Steering.write(leftSide);
  }
  
  else if (Prox02 <=25)
  {
    leftSide = Prox02;
    leftSide = map(leftSide, 5, 25, 130, 95);
    Steering.write(leftSide);
  }
 
  else if (Prox04 <=25)
  {
    rightSide = Prox04;
    rightSide = map(rightSide, 5, 25, 65, 95);
    Steering.write(rightSide);
  }
 
  else if (Prox05 <=25)
  {
    rightSide = Prox05;
    rightSide = map(rightSide, 5, 25, 65, 95);
    Steering.write(rightSide);
  }
  
  else if (Prox03 <=10)
  {
    Steering.write(135);
    delay(500);
  }


*/