#include "Servo.h"
int echoPin = 2;
int trigPin = 3;
int LEDR = 13;
int LEDV = 12;
int pir = 31;
bool valpin ;
Servo servo1;
Servo servo2;

float duration;
float distance;
float detection = 196.59;

void setup() {
  // put your setup code here, to run once:
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(LEDR, OUTPUT);
  pinMode(LEDV, OUTPUT);
  pinMode(pir, INPUT);
  servo1.attach(4);
  servo2.attach(5);
  servo1.write(0);
  servo2.write(0);
  delay(1000);
  //Serial.begin(9600);
}

void loop() 
{
  // put your main code here, to run repeatedly:
  valpin = digitalRead(pir);

  digitalWrite(trigPin, LOW);
  delayMicroseconds(3);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = (duration*0.0343)/2;
  //Serial.println(duration);
  //Serial.print("Distance: ");
  //Serial.println(distance);
  //delay(1000);
  if(distance<detection){
    digitalWrite(LEDR, HIGH);
    //digitalWrite(LEDV, LOW);
    
    servo1.write(90);
    servo2.write(90);
    
    if(valpin==HIGH)
      {
      digitalWrite(LEDR, LOW);
      servo1.write(0);
      servo2.write(0);
      delay(3000);
      digitalWrite(LEDR, LOW);
      digitalWrite(LEDV, HIGH);
      
    }
    else {
    servo1.write(90);
    servo2.write(90);
    digitalWrite(LEDV, LOW);
    
    }
  }
  
  else {
    digitalWrite(LEDR, LOW);
    digitalWrite(LEDV, HIGH);
     servo1.write(0);
    servo2.write(0);
    //delay(3000);
  }
    
}