//Ultrasonic Sensors
int TrigPinLeft = 13;
int TrigPinFront = 12;
int TrigPinRight = 2;
int EchoPinLeft = 5;
int EchoPinFront = 4;
int EchoPinRight = 3;
long durationLeft, distanceLeft;
long durationFront, distanceFront;
long durationRight, distanceRight;
//L298N motor and DC motor
//Each of the middle pins of the L298N is represented by an LED
int MotorLeftForward = 10;
int MotorLeftBackward = 9;
int MotorRightForward = 8;
int MotorRightBackward = 7;

void setup() {

//For Ultrasonic Sensors
pinMode(TrigPinFront, OUTPUT);
pinMode(TrigPinLeft, OUTPUT);
pinMode(TrigPinRight, OUTPUT);
pinMode(EchoPinFront, INPUT);
pinMode(EchoPinLeft, INPUT);
pinMode(EchoPinRight, INPUT);

//For L298N (middle pins for DC motors)
pinMode(MotorLeftForward, OUTPUT);
pinMode(MotorLeftBackward, OUTPUT);
pinMode(MotorRightForward, OUTPUT);
pinMode(MotorRightBackward, OUTPUT);
Serial.begin(9600);
}

void loop() {
//ACTIVATE Ultrasonic Sensors first

//Left ultrasonic sensor
digitalWrite(TrigPinLeft,LOW);
delayMicroseconds(2);
digitalWrite(TrigPinLeft,HIGH);
delayMicroseconds(10);
digitalWrite(TrigPinLeft,LOW);

durationLeft = pulseIn(EchoPinLeft,HIGH);
distanceLeft = durationLeft*0.034/2;

//Front ultrasonic sensor
digitalWrite(TrigPinFront,LOW);
delayMicroseconds(2);
digitalWrite(TrigPinFront,HIGH);
delayMicroseconds(10);
digitalWrite(TrigPinFront,LOW);

durationFront = pulseIn(EchoPinFront,HIGH);
distanceFront = durationFront*0.034/2;

//Right ultrasonic sensor
digitalWrite(TrigPinRight,LOW);
delayMicroseconds(2);
digitalWrite(TrigPinRight,HIGH);
delayMicroseconds(10);
digitalWrite(TrigPinRight,LOW);

//measuring the distance
durationRight = pulseIn(EchoPinRight,HIGH);
distanceRight = durationRight*0.034/2;



//==========================================
//ACTIVATE MOTORS based on sensor readings.
//Eech L298N middle pin is represented by an LED.
//REMEMBER tentative motor patterns below:
//1-0-1-0 or (High-Low-High-Low) = Forward
//0-1-0-1 or (Low-High-Low-High) = Backward
//1-0-0-1 or (High-Low-Low-High) = Rotate right
//0-1-1-0 or (Low-High-High-Low) = Rotate left

if ((distanceFront>20)&&(distanceLeft>20)&&(distanceRight>20)) {
//Move forward: The front sensor detects NO obstacle in front
//1-0-1-0 or (High-Low-High-Low) = Forward
digitalWrite(MotorLeftForward, HIGH);
digitalWrite(MotorLeftBackward, LOW);
digitalWrite(MotorRightForward, HIGH);
digitalWrite(MotorRightBackward, LOW);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceLeft);
Serial.print ("--");
Serial.print (distanceFront);
Serial.print ("--");
Serial.print (distanceRight);
Serial.print (" : ");
Serial.println ("Moving forward");
}

if ((distanceFront>20)&&(distanceLeft<20)&&(distanceRight<20)) {
//Move forward: The front sensor detects NO obstacle in front
//1-0-1-0 or (High-Low-High-Low) = Forward
digitalWrite(MotorLeftForward, HIGH);
digitalWrite(MotorLeftBackward, LOW);
digitalWrite(MotorRightForward, HIGH);
digitalWrite(MotorRightBackward, LOW);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceLeft);
Serial.print ("--");
Serial.print (distanceFront);
Serial.print ("--");
Serial.print (distanceRight);
Serial.print (" : ");
Serial.println ("Moving forward");
}


else if ((distanceFront>20)&&(distanceLeft>20)&&(distanceRight<20)) {
digitalWrite(MotorLeftForward, LOW);
digitalWrite(MotorLeftBackward, HIGH);
digitalWrite(MotorRightForward, HIGH);
digitalWrite(MotorRightBackward, LOW);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceLeft);
Serial.print ("--");
Serial.print (distanceFront);
Serial.print ("--");
Serial.print (distanceRight);
Serial.print (" : ");
Serial.println ("Rotating Left");
}

else if ((distanceFront<=20)&((distanceLeft)>(distanceRight))) {
//Rotate left: The front sensor DETECTS obstacle in front...
//...AND the left sensor detects detects farther than the objects detected by the right sensor. 
//0-1-1-0 or (Low-High-High-Low) = Rotate left
digitalWrite(MotorLeftForward, LOW);
digitalWrite(MotorLeftBackward, HIGH);
digitalWrite(MotorRightForward, HIGH);
digitalWrite(MotorRightBackward, LOW);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceLeft);
Serial.print ("--");
Serial.print (distanceFront);
Serial.print ("--");
Serial.print (distanceRight);
Serial.print (" : ");
Serial.println ("Rotating Left");
}

//NOTE: ADD "else if" codes for turning right and for stopping and moving backwards if all sensors detect equal distances (or robot is trapped)
else if ((distanceFront>20)&&(distanceLeft<20)&&(distanceRight>20)) {
digitalWrite(MotorLeftForward, HIGH);
digitalWrite(MotorLeftBackward, LOW);
digitalWrite(MotorRightForward, LOW);
digitalWrite(MotorRightBackward, HIGH);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceLeft);
Serial.print ("--");
Serial.print (distanceFront);
Serial.print ("--");
Serial.print (distanceRight);
Serial.print (" : ");
Serial.println ("Rotating Right");
}
else if ((distanceFront<=20)&((distanceLeft)<(distanceRight))) {
//Rotate right: The front sensor DETECTS obstacle in front...
//...AND the right sensor detects detects farther than the objects detected by the left sensor. 
//1-0-0-1 or (High-Low-Low-High) = Rotate Right
digitalWrite(MotorLeftForward, HIGH);
digitalWrite(MotorLeftBackward, LOW);
digitalWrite(MotorRightForward, LOW);
digitalWrite(MotorRightBackward, HIGH);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceLeft);
Serial.print ("--");
Serial.print (distanceFront);
Serial.print ("--");
Serial.print (distanceRight);
Serial.print (" : ");
Serial.println ("Rotating Right");
}

else {
//Stop
digitalWrite(MotorLeftForward, LOW);
digitalWrite(MotorLeftBackward, LOW);
digitalWrite(MotorRightForward, LOW);
digitalWrite(MotorRightBackward, LOW);
}
}