// NAME: MOHAMAD MIRZA AIMAN BIN ZULKIPLE (52224121157)
// LABWORK 3: Mobile Robot Startup

#define Red_Led    15    //Pin GIOP13 which is connected to Red Led   
#define Blue_Led   5    //Pin GIOP12  which is connected to Blue Led 
#define Pir_Sensor 4     //Pin GIOP14 which is connected to Pir Sensor  
int PirValue;             //Declare PirValue 

int trigPin = 32; // trig pin of HC-SR04
int echoPin = 35; // Echo pin of HC-SR04
int revleft4 = 12; //REVerse motion of Left motor
int fwdleft5 = 14; //ForWarD motion of Left motor
int revright6 = 25; //REVerse motion of Right motor
int fwdright7 = 26; //ForWarD motion of Right motor
int enable1PinA = 13;
int enable1PinB = 27;
long duration, distance;

void setup() {

  Serial.begin(115200);
  Serial.println("Obstacle Avoiding Robot Using Ultrasonic V2");

  delay(random(500,2000)); // delay for random time
  //Serial.begin(9600);
  pinMode(Pir_Sensor,INPUT);        // Pir_Sensor is Input
  pinMode(Red_Led,OUTPUT);          // Red_Led is Output
  pinMode(Blue_Led,OUTPUT);         // Blue_Led is Output
  pinMode(revleft4, OUTPUT);        // set Motor pins as output
  pinMode(fwdleft5, OUTPUT);
  pinMode(revright6, OUTPUT);
  pinMode(fwdright7, OUTPUT);
  pinMode(enable1PinA, OUTPUT);     // enable motor
  pinMode(enable1PinB, OUTPUT);    //
  pinMode(trigPin, OUTPUT);       // set trigger pin as OUTPUT
  pinMode(echoPin, INPUT);        //set echo pin as INPUT to capture reflected waves
}

void loop() {
  PirValue=digitalRead(Pir_Sensor);             // PirValue will read Pir_Sensor
   digitalWrite(Red_Led,PirValue);              // PirValue will read Red_Led
   digitalWrite(Blue_Led,PirValue);             // PirValue will read Blue_Led

 if(PirValue==HIGH) {                         // PirValue is High Now
    digitalWrite(Red_Led,HIGH);              // Red_Led will turn ON
    digitalWrite(Blue_Led,LOW);              // Blue Led will turn OFF
    Serial.begin(115200);                    // Baud 115200
    Serial.println("MOTION DETECTED");       // "MOTION DETECTED" statement will display.
    delay(1000);                             // delay 1000 miliseconds.
    } 

      else
   digitalWrite(Blue_Led,HIGH);                 // Blue_Led is turn ON
   Serial.begin(115200);                       // Baud 115200
   Serial.println(" NO MOTION DETECTED");    // "NO MOTION DETECTED" statement will appear when Pir_Sensor not detect HOUSEBREAKER.
  digitalWrite(trigPin, LOW);
  delayMicroseconds(10);

  digitalWrite(trigPin, HIGH); // send waves for 10 us
  delayMicroseconds(10);

  duration = pulseIn(echoPin, HIGH); // receive reflected waves
  distance = duration / 58.2; // convert to distance
  delay(10);

  Serial.println(distance);  
  digitalWrite(enable1PinA, HIGH); // enable motor A
  digitalWrite(enable1PinB, HIGH); // enable motor B

  // If you dont get proper movements of your robot then alter the pin numbers
  if (distance > 19)
  {
    digitalWrite(fwdright7, HIGH); // move forward
    digitalWrite(revright6, LOW);
    digitalWrite(fwdleft5, HIGH);
    digitalWrite(revleft4, LOW);
    Serial.println("MOVE FORWARD!");
  }

  if (distance < 18)
  {
    digitalWrite(fwdright7, LOW); //Stop
    digitalWrite(revright6, LOW);
    digitalWrite(fwdleft5, LOW);
    digitalWrite(revleft4, LOW);
    Serial.println("STOP!");
    delay(500);

    digitalWrite(fwdright7, LOW); //movebackword
    digitalWrite(revright6, HIGH);
    digitalWrite(fwdleft5, LOW);
    digitalWrite(revleft4, HIGH);
    Serial.println("MOVE BACKWARD!");
    delay(500);

    digitalWrite(fwdright7, LOW); //Stop
    digitalWrite(revright6, LOW);
    digitalWrite(fwdleft5, LOW);
    digitalWrite(revleft4, LOW);
    Serial.println("STOP!");
    delay(100);

    digitalWrite(fwdright7, HIGH);
    digitalWrite(revright6, LOW);
    digitalWrite(revleft4, LOW);
    digitalWrite(fwdleft5, LOW);
    delay(500);
    
  }
}