/* Using the 5 IR Distance to control the servo to avoid obstacles
Created August 13th, 2012
*/
#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 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(10); //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);
}
/*
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("------");
if ((Prox01 <= 10) || (Prox02 <= 10)) //If the distance is less than 15 inches
Steering.write(125); //turn right
else if ((Prox04 <= 10) || (Prox05 <= 10)) //If the distance is less then 15 inches
Steering.write(65); //turn left
else if ((Prox01 <= 15) || (Prox02 <= 15))
Steering.write(115);
else if ((Prox04 <= 15) || (Prox05 <= 15))
Steering.write(75);
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();
/*
else if ((Prox03 <= 15) || ((Prox01 >15) && (Prox02 >15) && (Prox03 >15) && (Prox04 >15) && (Prox05 >15)) || ((Prox01 <=15) && (Prox02 <=15) && (Prox04 <=15) && (Prox05 <=15))) //If the distance is less then 15 inches
Steering.write(105); //Go Straight
else if (((Prox01 <=15) && (Prox02 <=15)) || ((Prox01 <=15) && (Prox03 <=15)) || ((Prox01 <=15) && (Prox04 <=15)))
Steering.write(140);
else if (((Prox01 <=15) && (Prox05 <=15)) || ((Prox02 <=15) && (Prox04 <=15)))
Steering.write(105);
else if (((Prox05 <=15) && (Prox04 <=15)) || ((Prox05 <= 15) && (Prox03 <=15)) || ((Prox05 <=15) && (Prox02 <=15)))
Steering.write(50);
*/
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();
}
/* Serial.println("\nDistance in Inches Sensor 1: "); //Prints the line "Distance in Inches Sensor 1:"
Serial.print(Prox01); //Prints the data from Prox01
Serial.println("\nDistance in Inches Sensor 2: "); //Prints the line "Distance in Inches Sensor 2:"
Serial.print(Prox02); //Prints the data from Prox02
Serial.println("\nDistance in Inches Sensor 3: "); //Prints the line "Distance in Inches Sensor 3:"
Serial.print(Prox03); //Prints the data from Prox03
Serial.println("\nDistance in Inches Sensor 4: "); //Prints the line "Distance in Inches Sensor 4:"
Serial.print(Prox04); //Prints the data from Prox04
Serial.println("\nDistance in Inches Sensor 5: "); //Prints the line "Distance in Inches Sensor 5:"
Serial.print(Prox05); //Prints the data from Prox05
Serial.println("\n-------------------------------");
*/