from machine import Pin as pin ,PWM
import utime , time 
trigd = pin( 0 , pin.OUT , value = 0)
echod = pin( 1 , pin.IN )
servo = PWM(pin(2))
servo.freq(50)
echov = pin( 3 , pin.IN)
trigv = pin( 4 , pin.OUT , value = 0)
ledg  = pin( 5 , pin.OUT)
ledr = pin( 6 , pin.OUT)

def ultrasond( ):
    v = 0.034
    dis = temp = x2 = x = 0
    trigd.on()
    utime.sleep_us(150)
    trigd.off()
    while echod.value() == 0 :
        x= utime.ticks_us()
    while echod.value() == 1 :
        x2 = utime.ticks_us()
    temp = x2 - x
    print(temp,'temp')
    dis = int( temp * v /2) 
    print(dis,"distance")
    return(dis)

def ultrasonv():
    tem = dis = t1 = t2 = 0
    trigv.on()
    utime.sleep_us(10)
    trigv.off()
    while echov.value() == 0 :
        t1 = utime.ticks_us()
    while echov.value() == 1 :
        t2 = utime.ticks_us()
    tem = t2 - t1
    dis = int(tem * 0.034/2)
    return (dis)
    
def angle (d,f):
    
    d1 = ((d * 65)/1.8) + 1500
    f1 = ((f * 65)/1.8) + 1500
    servo.duty_u16(int(d1))
    utime.sleep(1)
    servo.duty_u16(int(f1))
    utime.sleep(1)


while True :
    vol = ultrasonv()
    if (vol >= 200):
        ledr.on()
        ledg.off()
    if (vol < 200):
        ledg.on()
        ledr.off()
        dis = ultrasond()
        if (dis  <= 100):
            angle(0,100)
            time.sleep(2)
            angle(0,0)