import machine
import time

# Pin configuration
TRIG_PIN = 5
ECHO_PIN = 18
SERVO_PIN = 19

# Set up components
trig = machine.Pin(TRIG_PIN, machine.Pin.OUT)
echo = machine.Pin(ECHO_PIN, machine.Pin.IN)
servo = machine.PWM(machine.Pin(SERVO_PIN), freq=50)

def set_angle(angle):
    # Convert degrees (0-180) to ESP32 duty cycle (20-120)
    duty = int(((angle / 180) * 100) + 20)
    servo.duty(duty)

def get_distance():
    # Clean low pulse first
    trig.value(0)
    time.sleep_us(2)
    
    # 10us trigger pulse to start sensor
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)
    
    # Measure the bounce back time
    while echo.value() == 0:
        start_time = time.ticks_us()
        
    while echo.value() == 1:
        end_time = time.ticks_us()
        
    # Calculate distance in cm based on speed of sound
    duration = time.ticks_diff(end_time, start_time)
    return (duration * 0.0343) / 2

# Initial gate state (Closed)
set_angle(0)
print("System 1 ready.")

while True:
    distance = get_distance()
    print("Distance:", round(distance, 1), "cm")
    
    # Car gets within 100cm -> open gate. Otherwise -> keep closed.
    if distance <= 100:
        print("Car detected. Gate opening...")
        set_angle(90)
    else:
        set_angle(0)
        
    time.sleep(1)