//Ultrasonic Sensors
int TrigPinD = 2;
int TrigPinP = 13;
int TrigPinT = 12;
int EchoPinD = 3;
int EchoPinP = 5;
int EchoPinT = 4;
long durationD, distanceD;
long durationP, distanceP;
long durationT, distanceT;
//L298N motor and DC motor
//Each of the middle pins of the L298N is represented by an LED
int MotorDForward = 7;
int MotorDBackward = 6;
int MotorTForward = 8;
int MotorTBackward = 9;

void setup() {

//For Ultrasonic Sensors
pinMode(TrigPinP, OUTPUT);
pinMode(TrigPinD, OUTPUT);
pinMode(TrigPinT, OUTPUT);
pinMode(EchoPinP, INPUT);
pinMode(EchoPinD, INPUT);
pinMode(EchoPinT, INPUT);

//For L298N (middle pins for DC motors)
pinMode(MotorDForward, OUTPUT);
pinMode(MotorDBackward, OUTPUT);
pinMode(MotorTForward, OUTPUT);
pinMode(MotorTBackward, OUTPUT);
Serial.begin(9600);
}

void loop() {
//ACTIVATE Ultrasonic Sensors first

//D ultrasonic sensor
digitalWrite(TrigPinD,LOW);
delayMicroseconds(2);
digitalWrite(TrigPinD,HIGH);
delayMicroseconds(10);
digitalWrite(TrigPinD,LOW);

durationD = pulseIn(EchoPinD,HIGH);
distanceD = durationD*0.034/2;

//P ultrasonic sensor
digitalWrite(TrigPinP,LOW);
delayMicroseconds(2);
digitalWrite(TrigPinP,HIGH);
delayMicroseconds(10);
digitalWrite(TrigPinP,LOW);

durationP = pulseIn(EchoPinP,HIGH);
distanceP = durationP*0.034/2;

//T ultrasonic sensor
digitalWrite(TrigPinT,LOW);
delayMicroseconds(2);
digitalWrite(TrigPinT,HIGH);
delayMicroseconds(10);
digitalWrite(TrigPinT,LOW);

//measuring the distance
durationT = pulseIn(EchoPinT,HIGH);
distanceT = durationT*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 T
//0-1-1-0 or (Low-High-High-Low) = Rotate D

if ((distanceP>30)&&(distanceD>20)&&(distanceT>20)) {
//Move forward: The P sensor detects NO obstacle in P
//1-0-1-0 or (High-Low-High-Low) = Forward
digitalWrite(MotorDForward, HIGH);
digitalWrite(MotorDBackward, LOW);
digitalWrite(MotorTForward, HIGH);
digitalWrite(MotorTBackward, LOW);

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

else if ((distanceP>30)&&(distanceD<20)&&(distanceT<20)) {
  if ((distanceD-distanceT)>5){
digitalWrite(MotorDForward, LOW);
digitalWrite(MotorDBackward, HIGH);
digitalWrite(MotorTForward, HIGH);
digitalWrite(MotorTBackward, LOW);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceD);
Serial.print ("--");
Serial.print (distanceP);
Serial.print ("--");
Serial.print (distanceT);
Serial.print (" : ");
Serial.println ("Swerving D");
 }
else if ((distanceD-distanceT)<-5){
digitalWrite(MotorDForward, HIGH);
digitalWrite(MotorDBackward, LOW);
digitalWrite(MotorTForward, LOW);
digitalWrite(MotorTBackward, HIGH);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceD);
Serial.print ("--");
Serial.print (distanceP);
Serial.print ("--");
Serial.print (distanceT);
Serial.print (" : ");
Serial.println ("Swerving T");
 }


else {
//Move forward: The P sensor detects NO obstacle in P
//1-0-1-0 or (High-Low-High-Low) = Forward
digitalWrite(MotorDForward, HIGH);
digitalWrite(MotorDBackward, LOW);
digitalWrite(MotorTForward, HIGH);
digitalWrite(MotorTBackward, LOW);

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

else if ((distanceP>30)&&(distanceD>20)&&(distanceT<20)) {
digitalWrite(MotorDForward, LOW);
digitalWrite(MotorDBackward, HIGH);
digitalWrite(MotorTForward, HIGH);
digitalWrite(MotorTBackward, LOW);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceD);
Serial.print ("--");
Serial.print (distanceP);
Serial.print ("--");
Serial.print (distanceT);
Serial.print (" : ");
Serial.println ("Rotating D");
}

else if ((distanceP<=30)&((distanceD)>(distanceT))) {
//Rotate D: The P sensor DETECTS obstacle in P...
//...AND the D sensor detects detects farther than the objects detected by the T sensor. 
//0-1-1-0 or (Low-High-High-Low) = Rotate D
digitalWrite(MotorDForward, LOW);
digitalWrite(MotorDBackward, HIGH);
digitalWrite(MotorTForward, HIGH);
digitalWrite(MotorTBackward, LOW);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceD);
Serial.print ("--");
Serial.print (distanceP);
Serial.print ("--");
Serial.print (distanceT);
Serial.print (" : ");
Serial.println ("Rotating D");
}

//NOTE: ADD "else if" codes for turning T and for stopping and moving backwards if all sensors detect equal distances (or robot is trapped)
else if ((distanceP>30)&&(distanceD<20)&&(distanceT>20)) {
digitalWrite(MotorDForward, HIGH);
digitalWrite(MotorDBackward, LOW);
digitalWrite(MotorTForward, LOW);
digitalWrite(MotorTBackward, HIGH);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceD);
Serial.print ("--");
Serial.print (distanceP);
Serial.print ("--");
Serial.print (distanceT);
Serial.print (" : ");
Serial.println ("Rotating T");
}
else if ((distanceP<=30)&((distanceD)<(distanceT))) {
//Rotate T: The P sensor DETECTS obstacle in P...
//...AND the T sensor detects detects farther than the objects detected by the D sensor. 
//1-0-0-1 or (High-Low-Low-High) = Rotate T
digitalWrite(MotorDForward, HIGH);
digitalWrite(MotorDBackward, LOW);
digitalWrite(MotorTForward, LOW);
digitalWrite(MotorTBackward, HIGH);

//Check sensor values for easier troubleshooting of problems in actual circuit
Serial.print (distanceD);
Serial.print ("--");
Serial.print (distanceP);
Serial.print ("--");
Serial.print (distanceT);
Serial.print (" : ");
Serial.println ("Rotating T");
}

else {
//Stop
digitalWrite(MotorDForward, LOW);
digitalWrite(MotorDBackward, LOW);
digitalWrite(MotorTForward, LOW);
digitalWrite(MotorTBackward, LOW);
}
}

//Write to Christalyn Kaye Duetes