from machine import Pin,PWM,ADC   
import time
import utime
import neopixel
button = Pin(18, Pin.IN)
buzzer = PWM(Pin(33))        
motor1 = PWM(Pin(12))
motor2 = PWM(Pin(13))
motor3 = PWM(Pin(14))
motor4 = PWM(Pin(15))
trig = 27
RGB_pin = Pin(26, Pin.OUT)
np = neopixel.NeoPixel(RGB_pin, n=6, bpp=3, timing=1)

QTI_L=Pin(39,Pin.IN,Pin.PULL_UP)
QTI_R=Pin(36,Pin.IN,Pin.PULL_UP)

def setup():
    buzzer.duty_u16(0)
    
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 Tracking():
    QTI_MAX = QTI_L.value() * 2 + QTI_R.value()
    if QTI_MAX == 0:
        motor(40000,0,40000,0)
    elif QTI_MAX == 1:
        motor(0,40000,0,20000)
    elif QTI_MAX == 2:
        motor(0,20000,0,40000)
    elif QTI_MAX == 3:
        motor(0,50000,0,50000)
def loop():
    while button.value() == 1:
        pass
    else:
        while True:
            distance = getDistance(trig)   
            print("The current distance is：{:.2f} cm".format(distance))
            if distance < 10:
                motor(0,0,0,0)
                for n in range(0,6):
                    np[n] = (255,0,0)
                np.write()
                buzzer.duty_u16(2000)
                buzzer.freq(262)
                time.sleep(0.3)
                for n in range(0,6):
                    np[n] = (0,0,0)
                np.write()
                buzzer.duty_u16(0)
                time.sleep(0.3)
            else:
                for n in range(0,6):
                    np[n] = (0,255,0)
                np.write()
                Tracking()
if __name__ == '__main__':
    setup()
    loop()

