from machine import Pin,PWM,ADC   
import utime
import neopixel
button = Pin(18, Pin.IN)        
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():
    for n in range(0,6):
        np[n] = (0,0,0)
    np.write()
    
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 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(speed,0,speed,0)
    elif QTI_MAX == 1:
        motor(0,speed,speed/2,0)
    elif QTI_MAX == 2:
        motor(speed/2,0,0,speed)
    elif QTI_MAX == 3:
        motor(0,speed,0,speed)
        
def Map(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

def loop():
    while button.value() == 1:
        pass
    else:
        while True:
            distance = getDistance(trig)   
            print("The current distance is：{:.2f} cm".format(distance))
            if distance > 5 and distance < 30:
                speed = int(Map(distance,5,30,100,1023))
                Tracking()
            elif distance <= 5:
                for n in range(0,6):
                    np[n] = (255,0,0)
                np.write()
                motor(0,0,0,0)
            else:
                for n in range(0,6):
                    np[n] = (0,255,0)
                np.write()
                speed = 1000
                Tracking()
                
if __name__ == '__main__':
    setup()
    loop()
