import machine
import time

# Pin Setup
trig = machine.Pin(5, machine.Pin.OUT)
echo = machine.Pin(18, machine.Pin.IN)
servo = machine.PWM(machine.Pin(13), freq=50)

while True:
    # 1. Trigger the ultrasonic pulse
    trig.off()
    time.sleep_us(2)
    trig.on()
    time.sleep_us(10)
    trig.off()
    
    # 2. Measure echo duration (30ms timeout)
    duration = machine.time_pulse_us(echo, 1, 30000)
    
    if duration > 0:
        # Calculate distance in cm
        dist = (duration * 0.0343) / 2
        print("Distance:", dist, "cm")
        
        # 3. Servo logic based on distance
        if dist < 15:
            # 180 degrees (approx duty 123)
            servo.duty(123)
        elif dist < 30:
            # 90 degrees (approx duty 74)
            servo.duty(74)
        else:
            # 0 degrees (approx duty 26)
            servo.duty(26)
            
    time.sleep(0.5)