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;

int ledPin = 2;                // choose the pin for the LED
int inputPin = 33;               // choose the input pin (for PIR sensor)
int pirState = LOW;             // we start, assuming no motion detected
int val = 0;                    // variable for reading the pin status


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Obstacle Avoiding Robot Using Ultrasonic V2");

  delay(random(500, 2000)); // delay for random time
  //Serial.begin(9600);
  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 trig pin as output
  pinMode(echoPin, INPUT); //set echo pin as input to capture reflected waves

  pinMode(ledPin, OUTPUT);      // declare LED as output
  pinMode(inputPin, INPUT);     // declare sensor as input

}

void loop() {
  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

  val = digitalRead(inputPin);  // read input value
  if (val == HIGH) {            // check if the input is HIGH
    digitalWrite(ledPin, HIGH);  // turn LED ON
    if (pirState == LOW) {
      // we have just turned on
      Serial.println("Motion detected!");
      // We only want to print on the output change, not state
      pirState = HIGH;
    }
  } else {
    digitalWrite(ledPin, LOW); // turn LED OFF
    if (pirState == HIGH) {
      // we have just turned of
      Serial.println("Motion ended!");
      // We only want to print on the output change, not state
      pirState = LOW;
    }
  }
  
  // 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);
  }
}