from machine import Pin,PWM
import utime
import neopixel

trig = 27
pin = Pin(26, Pin.OUT)
np = neopixel.NeoPixel(pin, n=6, bpp=3, timing=1)
button = Pin(18, Pin.IN)
speed = 1023  #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)
def setup():
    for n in range(0,6):
        np[n] = (0,0,0)
    np.write()
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 >= 0 and distance < 10:
                for n in range(0,6):
                    np[n] = (255,0,0)
                    np.write()
                motor(speed,0,speed,0)
                utime.sleep(0.1)
                motor(0,0,0,0)
            elif distance >= 10 and distance < 20:
                for n in range(0,6):
                    np[n] = (0,255,0)
                    np.write()
                motor(0,speed,0,speed)
                utime.sleep(0.3)
                motor(0,0,0,0)
            elif distance > 20:
                for n in range(0,6):
                    np[n] = (0,0,255)
                    np.write()
                motor(0,0,0,0)
            
if __name__ == "__main__":
    setup()
    loop() # Call loop function
