from machine import Pin,  PWM
import utime 

trigger= Pin(2,Pin.OUT)
echo= Pin(3,Pin.IN)
yellow=Pin(12,Pin.OUT)
green=Pin(5,Pin.OUT)

servo = PWM(Pin(13))
servo. freq(50)

def set_angle(angle):
  duty= int((angle/180) *5000+200)
  servo.duty_u16(duty)

def measure_distance():

    trigger.low()
    utime.sleep_us(2)

    trigger.high()
    utime.sleep_us(10)
    trigger.low()

    while echo.value()== 0:
        pulse_start = utime.ticks_us()

    while echo.value()== 1:
        pulse_end = utime.ticks_us()

    duration = utime.ticks_diff(pulse_end,pulse_start)

    distance = (duration*0.343) / 2

    return distance

while True:
    
    distance = measure_distance()

    print("Distance:",distance,"cm")
    
    if distance < 10:
        set_angle(0)
        yellow_led.on()
        green_led.off()


    #elif distance < 20:
      #set_angle(45)


     #elif distance < 30:
       #set_angle(90)


      #elif distance < 40:
        #set_angle(135)


    else:
        set_angle(180)
        yellow.off()
        green.on()

    utime.sleep(0.2)