//CODE MODIFICATION CREDITS
//fb.com/shaambhavbhandari21
//fb.com/Bishow420
#include <Servo.h> // Include Servo Library.
#include <NewPing.h> // Include Newping Library.
int in1 = 7; // L298N PINS
int in2 = 8;
int in3 = 9;
int in4 = 10;
int enA = 5; // L298N SPEED CONTROL ENABLE
int enB = 6;
#define TRIGGER_PIN 11
#define ECHO_PIN 12
#define MAX_DISTANCE 250 // Max Ping Distance
Servo servo_motor;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
boolean goesForward = false; // DO NOTHING AT START
int distance = 100;
//CODE THAT WILL RUN ONCE
void setup()
{
//L298N PIN AS OUTPUT
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);

servo_motor.attach(3); //SERVO TO PIN 3
servo_motor.write(90); //MOVE SERVO 90 INITIAL POSITION
delay(2000);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
Serial.begin(9600); //SERIAL BEGIN FOR BLUETOOTH
}

void loop()
{
if (Serial.available()) //IF BLUETOOTH AVILABLE
BLUETOOTH(); //GO TO BLUETOOTH FUNCTION
}

//SERVO LOOK RIGHT AND ULTRASONIC READ DISTANCE
int lookRight()
{
servo_motor.write(15);
delay(500);
int distance = readPing();
delay(100);
servo_motor.write(90);
return distance;
}
//SERVO LOOK LEFT AND ULTRASONIC READ DISTANCE
int lookLeft()
{
servo_motor.write(180);
delay(500);
int distance = readPing();
delay(100);
servo_motor.write(90);
return distance;
}
//READ DISTANCE
int readPing()
{
delay(100);
int cm = sonar.ping_cm();
if (cm == 0)
{
cm = 250;   //if distance = 0cm sistance is 250cm
}
return cm;
}
void forward() //GO FORWRD FUNCTION
{
analogWrite(enA, 220); //speed is higher the enA & enB value (0-255)
analogWrite(enB, 220);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
}

void backward() //GO BACKWARD FUNCTION
{
analogWrite(enA, 220);
analogWrite(enB, 220);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}
void turn_right() //TURN RIGHT FUNCTION
{
analogWrite(enA, 220);
analogWrite(enB, 220);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}

void turn_left() //TURN LEFT FUNCTION
{
analogWrite(enA, 220);
analogWrite(enB, 220);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
}
void st() //STOP FUNCTION
{
analogWrite(enA, 190);
analogWrite(enB, 190);
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}
void BLUETOOTH() //BLUETOOTH FUNCTION
{
// YOUR DEFAULTS
char data = Serial.read(); //VARIABLE VALUE IS SERIAL VALUE
Serial.println(data); //PRINT SERIAL VALUE
if (data == 'F') //IF SERIAL VALUE IS "F" CALL FORWARD FUNCTION
{
forward();
Serial.println("BLUETOOTH MODE"); //PRINT BLUETOOTH MODE IN SERIAL MONITOR
}
else if (data == 'B') //IF SERIAL VALUE IS "B" CALL BACKWARD FUNCTION
{
backward();
Serial.println("BLUETOOTH MODE");
}
else if (data == 'L') //IF SERIAL VALUE IS "L" CALL TURN LEFT FUNCTION
{
turn_left();
Serial.println("BLUETOOTH MODE");
}
else if (data == 'R') //IF SERIAL VALUE IS "R" CALL TURN RIGHT FUNCTION
{
turn_right();
Serial.println("BLUETOOTH MODE");
}
else if (data == 'S') //IF SERIAL VALUE IS "S" CALL STOP FUNCTION
{
st();
Serial.println("BLUETOOTH MODE");
}
else if (data == 'I') //IF SERIAL VALUE IS "I" CALL TURN AUTO FUNCTION
AUTO();
}
void AUTO() //AUTO FUNCTION
{
Serial.println("AUTO MODE"); //PRINT AUTO MODE IN SERIAL MONITOR
int distanceRight = 0;
int distanceLeft = 0;
delay(50);
if (distance <= 40) //IF DISTANCE IS LESS THEN OR EQUAL TO 40 CM
{
st(); //CALL STOP FUNCTION
delay(300);
backward();//CALL BACKWARD FUNCTION
delay(400);
st();//CALL STOP FUNCTION
delay(300);
distanceRight = lookRight(); //TURN SERVO TO RIGHT AND STORE DISTANCE
delay(300);
distanceLeft = lookLeft(); //TURN SERVO TO LEFT AND STORE DISTANCE
delay(300);

if (distanceRight >= distanceLeft) //COMPARE DISTANCE RIGHT AND LEFT
{
turn_right(); //IF RIGHT IS GREAT CALL RIGHT
delay(800);
st();
}
else
{
turn_left(); //ELSE CALL LEFT
delay(800);
st();
}

}
else
{
forward(); //IF DISTANCE IS MORE THEN 40cm GO FORWARD
}
distance = readPing();
}