#include <Servo.h>
// define servos
Servo servo1,servo2,servo3,servo4;
// IR sensors pin
const int irSensorPin=2;
//Ultrasonic sensor pins
const int trigPin=10;
const int echoPin=11;
//Define the sorvos pins
const int servo1Pin=3;
const int servo2Pin=5;
const int servo3Pin=6;
const int servo4Pin=9;
long duration;
int distance;
const int thresholdDistance=100; // distance threshold in cm

//Varaible for non-blocking delay
unsigned long previousMillis=0;
unsigned long currentMillis;
const long interval=5000;// 5 seconds for servo3 and servo4
bool servoActive=false;

// measure distance from  sensors
  int measureDistance()
  {digitalWrite(trigPin,LOW);
   delayMicroseconds(2);
   digitalWrite(trigPin,HIGH);
   delayMicroseconds(10);
   digitalWrite(trigPin,LOW);
  duration=pulseIn(echoPin,HIGH);
  distance=duration*0.034/2;
  return distance;

  }

void setup() {
// Attached the servos to pins
servo1.attach(servo1Pin);
servo2.attach(servo2Pin);
servo3.attach(servo3Pin);
servo4.attach(servo4Pin);

//Set IR sensor to pin as Input
pinMode(irSensorPin, INPUT);

//set ultrasonic sensor pins
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);

// Initialize servos to initial position
servo1.write(0);
servo2.write(0);
servo3.write(0);
servo4.write(0);

//start serial communication for debugging
Serial.begin(9600);
}

void loop()
 {
  
  // read the IR sensor
  int irValue=digitalRead(irSensorPin);
   // the IR sensor is triggered, rotate the first two servos
  if(irValue==HIGH)
    { servo1.write(180);
    
    delay(5000);
    // measure the distance using the ultrasonic sensor
    servo2.write(0);
    servo3.write(0);
    servo4.write(0);
  distance=measureDistance();
  //if the object is within the threshold distance, rotate the other two servos for 5 seconds.
  unsigned long currentMillies=millis();
  //if(distance<thresholdDistance )
  if(distance<thresholdDistance && !servoActive)
  {
    servo2.write(60);
    servo3.write(90);
    servo4.write(120);
    
    previousMillis=currentMillis;
    servoActive=true;
    delay(5000);
    servo2.write(0);
    servo3.write(0);
    servo4.write(0);
    servoActive=false;
    
  }
  if(servoActive && currentMillis-previousMillis>=interval)
  { servo2.write(0);
    servo3.write(0);
    servo4.write(0);
    servoActive=false;
   
  }
  delay(5000);
  servo1.write(0);
 

    }
    else
    {
      servo1.write(0);
      
    }
    

 }