#include <Servo.h>
 
const int trigPin1 = 2; 
const int echoPin1 = 3; 
const int trigPin2 = 4; 
const int echoPin2 = 5; 
const int buttonPin = 6; 
const int servo1Pin = 7; 
const int servo2Pin = 8; 
const int servo3Pin = 9; 
const int servo4Pin = 10; 
const int buttonPower = 11; 
const int led1Pin = 12; 
const int led2Pin = 13; 
const int led3Pin = A0; 
const int led4Pin = A1; 
 
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
 
bool powerOn = false;
 
void setup() {
  Serial.begin(9600);
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(buttonPower, OUTPUT);
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
  pinMode(led3Pin, OUTPUT);
  pinMode(led4Pin, OUTPUT);
  
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
  servo4.attach(servo4Pin);
}
 
void loop() {
  if (digitalRead(buttonPin) == LOW) {
    powerOn = !powerOn; 
    digitalWrite(buttonPower, powerOn);
    delay(200); 
  }
 
  if (powerOn) {
    long duration1, distance1, duration2, distance2;
 
    digitalWrite(trigPin1, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin1, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin1, LOW);
    duration1 = pulseIn(echoPin1, HIGH);
    distance1 = duration1 * 0.034 / 2;

    digitalWrite(trigPin2, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin2, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin2, LOW);
    duration2 = pulseIn(echoPin2, HIGH);
    distance2 = duration2 * 0.034 / 2;
 
    if (distance1 < 30 || distance2 < 30) { 
      servo1.write(90);
      digitalWrite(led1Pin, HIGH); 
      delay(1000); 
      servo2.write(90);
      digitalWrite(led2Pin, HIGH); 
      delay(1000); 
      servo3.write(90);
      digitalWrite(led3Pin, HIGH); 
      delay(1000); 
      servo4.write(90);
      digitalWrite(led4Pin, HIGH);
      delay(1000); 
    }
  } else {
    servo1.write(0);
    digitalWrite(led1Pin, LOW);
    servo2.write(0);
    digitalWrite(led2Pin, LOW);
    servo3.write(0);
    digitalWrite(led3Pin, LOW);
    servo4.write(0);
    digitalWrite(led4Pin, LOW);
  }
}