from machine import Pin,PWM
import utime

speed = 500  #Speed range:0~1023
def motor(L12,L13,R14,R15):
    PWM(Pin(12)).duty(L12) 
    PWM(Pin(13)).duty(L13) 
    PWM(Pin(14)).duty(R14) 
    PWM(Pin(15)).duty(R15)
button = Pin(18, Pin.IN)
trig = 27

def motor(A1,A2,B1,B2):
    motor1.duty_u16(A1) 
    motor2.duty_u16(A2) 
    motor3.duty_u16(B1) 
    motor4.duty_u16(B2)

def getDistance(trig):
    trig = Pin(27,Pin.OUT)
    trig.value(0)
    utime.sleep_us(2)
    trig.value(1)
    utime.sleep_us(10)
    trig.value(0)
    trig = Pin(27,Pin.IN)
    while trig.value() == 0:
        start = utime.ticks_us()
    while trig.value() == 1:
        end = utime.ticks_us()
    d = (end - start) * 0.0343 / 2 
    return d



def loop():
    while button.value() == 1:
        pass
    else:
        while True:
            distance = getDistance(trig)
            if distance < 20:
                motor(0,speed,speed,0)
            else:
                motor(0,speed,0,speed)     

if __name__ == "__main__":
    loop() 

