#Ultrasonic sensor pins
trigPin1 = 17
echoPin1 = 18
trigPin2 = 23
echoPin2 = 24
 
#Servo motor pins
servo1Pin = 12
servo2Pin = 13
servo3Pin = 14
servo4Pin = 15
 
#LED pins
whiteLEDPin = 8
redLEDPin = 9
greenLEDPin = 10
blueLEDPin = 11
 
#Buzzer pin
buzzerPin = 6
 
#Initialize GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(trigPin1, GPIO.OUT)
GPIO.setup(echoPin1, GPIO.IN)
GPIO.setup(trigPin2, GPIO.OUT)
GPIO.setup(echoPin2, GPIO.IN)
GPIO.setup(buzzerPin, GPIO.OUT)
GPIO.setup(whiteLEDPin, GPIO.OUT)
GPIO.setup(redLEDPin, GPIO.OUT)
GPIO.setup(greenLEDPin, GPIO.OUT)
GPIO.setup(blueLEDPin, GPIO.OUT)
 
#Initialize servos
servo1 = Servo(servo1Pin)
servo2 = Servo(servo2Pin)
servo3 = Servo(servo3Pin)
servo4 = Servo(servo4Pin)
 
#Initialize master switch
switch = Button(7)
 
system_on = False
 
def move_servos(distance1, distance2):
    if abs(distance1 - 400) <= 20 and abs(distance2 - 400) <= 20:
        servo1.max()
        LED(whiteLEDPin).on()
        time.sleep(1)
        LED(whiteLEDPin).off()
    elif abs(distance1 - 350) <= 20 and abs(distance2 - 350) <= 20:
        servo2.max()
        LED(redLEDPin).on()
        time.sleep(1)
        LED(redLEDPin).off()
    elif abs(distance1 - 350) <= 20 and abs(distance2 - 300) <= 20:
        servo3.max()
        LED(greenLEDPin).on()
        time.sleep(1)
        LED(greenLEDPin).off()
    elif abs(distance1 - 300) <= 20 and abs(distance2 - 300) <= 20:
        servo4.max()
        LED(blueLEDPin).on()
        time.sleep(1)
        LED(blueLEDPin).off()
 
def buzz_twice():
    for _ in range(2):
        GPIO.output(buzzerPin, GPIO.HIGH)
        time.sleep(0.5)
        GPIO.output(buzzerPin, GPIO.LOW)
        time.sleep(0.5)
 
def buzz_once():
    GPIO.output(buzzerPin, GPIO.HIGH)
    time.sleep(1)
    GPIO.output(buzzerPin, GPIO.LOW)
 
while True:
    if switch.is_pressed:
        system_on = not system_on
        time.sleep(0.5)  #debounce delay
 
    if system_on:
        GPIO.output(trigPin1, GPIO.LOW)
        time.sleep(0.2)
        GPIO.output(trigPin1, GPIO.HIGH)
        time.sleep(0.00001)
        GPIO.output(trigPin1, GPIO.LOW)
        pulse_start, pulse_end = 0, 0
        while GPIO.input(echoPin1) == 0:
            pulse_start = time.time()
        while GPIO.input(echoPin1) == 1:
            pulse_end = time.time()
        duration1 = pulse_end - pulse_start
        distance1 = duration1 * 17150
 
        GPIO.output(trigPin2, GPIO.LOW)
        time.sleep(0.2)
        GPIO.output(trigPin2, GPIO.HIGH)
        time.sleep(0.00001)
        GPIO.output(trigPin2, GPIO.LOW)
        pulse_start, pulse_end = 0, 0
        while GPIO.input(echoPin2) == 0:
            pulse_start = time.time()
        while GPIO.input(echoPin2) == 1:
            pulse_end = time.time()
        duration2 = pulse_end - pulse_start
        distance2 = duration2 * 17150
 
        if (abs(distance1 - 400) <= 20 and abs(distance2 - 400) <= 20) or \
                (abs(distance1 - 350) <= 20 and abs(distance2 - 350) <= 20) or \
                (abs(distance1 - 350) <= 20 and abs(distance2 - 300) <= 20) or \
                (abs(distance1 - 300) <= 20 and abs(distance2 - 300) <= 20):
            buzz_twice()
            move_servos(distance1, distance2)
        else:
            buzz_once()
            for led_pin in [whiteLEDPin, redLEDPin, greenLEDPin, blueLEDPin]:
                GPIO.output(led_pin, GPIO.HIGH)
            time.sleep(3)  # Buzz for 3 seconds
            for led_pin in [whiteLEDPin, redLEDPin, greenLEDPin, blueLEDPin]:
                GPIO.output(led_pin, GPIO.LOW)