int TrigPinLift = 11;
int TrigPinPorward= 5;
int TrigPinReght = 9;
int EchoPinLift = 10;
int EchoPinPorward = 3;
int EchoPinReght = 6;
long DurationLift, DistanceLift ;
long DurationPorward, DistancePorward ;
long DurationReght, DistanceReght ;
//L298N motor and DC motor
//Each of the middle pins of the L298N is represented by an LED
int MotorLeftForward = 4;
int MotorLeftBackward = 7;
int MotorRightForward = 8;
int MotorRightBackward = 12;

void setup() {
//For Ultrasonic Sensors
pinMode(Led1, OUTPUT);
pinMode(Led2, OUTPUT);
pinMode(Led3, OUTPUT);
pinMode(EchoPinLift, INPUT);
pinMode(EchoPinPorward, INPUT);
pinMode(EchoPinReght, INPUT);
pinMode(TrigPinLift, OUTPUT);
pinMode(TrigPinPorward, OUTPUT);
pinMode(TrigPinReght, OUTPUT);

//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

//Lift ultrasonic sensor 
digitalWrite(TrigPinLift, LOW);
delayMicroseconds(2);
digitalWrite(TrigPinLift, HIGH);
delayMicroseconds(10);
digitalWrite(TrigpinLift, LOW);

DurationLift = pulseIn(EchoPinLift, HIGH);
DistanceLift = DurationLift*0.034/2;

//Porward ultrasonic sensor 
digitalWrite(TrigPinPorward, LOW);
delayMicroseconds(2);
digitalWrite(TrigPinPorward, HIGH);
delay(10);
digitalWrite(TrigPinPorward, LOW);

DurationPorward = pulseIn(EchoPinPorward, HIGH);
DistancePorward = DurationPorward*0.034/2;
 
//Reght ultrasonic sensor 
digitalWrite(TrigPinReght, LOW);
delayMicroseconds(2);
digitalWrite(TrigPinReght, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPinReght, LOW);

DurationReght = pulseIn(EchoPinReght, HIGH);
DistanceReght = DurationReght*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 (distancePorward>20) {
//Move forward: The Porward 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 (distanceLift);
Serial.print ("--");
Serial.print (distancePorward);
Serial.print ("--");
Serial.print (distanceReght);
Serial.print (" : ");
Serial.println ("Moving toward");
}

else if ((distancePorward<=20)&((distanceLift)>(distanceReght))) {
//Rotate left: The porward 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 (distanceLift);
Serial.print ("--");
Serial.print (distancePorward);
Serial.print ("--");
Serial.print (distanceReght);
Serial.print (" : ");
Serial.println ("Moving Forward");
}

else if ((distancePorward<=20)&((distanceLift)<(distanceReght))) {
//Rotate right: The porward sensor DETECTS obstacle in front...
//...AND the rigt 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 (distanceLift);
Serial.print ("--");
Serial.print (distancePorward);
Serial.print ("--");
Serial.print (distanceReght);
Serial.print (" : ");
Serial.println ("Rotating Right");

}

else if ((distancePorward<=20)&((distanceLift<=20)&&(distanceReght<=20))) {
//Move Backward: The porward sensor DETECTS obstacle in front...
//...And the left sensor DETECTS obstacle in front...
//...And  the right sensor DETECTS obstacle in front.
//0-1-0-1 or (Low-High-Low-High) = Backward
digitalWrite(MotorLeftForward, LOW);
digitalWrite(MotorLeftBackward, HIGH);
digitalWrite(MotorRightForward, LOW);
digitalWrite(MotorRightBackward, HIGH);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceLift);
Serial.print ("--");
Serial.print (distancePorward);
Serial.print ("--");
Serial.print (distanceReght);
Serial.print (" : ");
Serial.println ("Moving Backward");

}

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