from machine import Pin,PWM,ADC   
import utime
import neopixel
button = Pin(18, Pin.IN)        
motor1 = PWM(Pin(12))
motor2 = PWM(Pin(13))
motor3 = PWM(Pin(14))
motor4 = PWM(Pin(15))
RGB_pin = Pin(26, Pin.OUT)
np = neopixel.NeoPixel(RGB_pin, n=6, bpp=3, timing=1)
left=ADC(Pin(35,Pin.PULL_DOWN))
right=ADC(Pin(34,Pin.PULL_DOWN))
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(A1,A2,B1,B2):
    motor1.duty_u16(A1) 
    motor2.duty_u16(A2) 
    motor3.duty_u16(B1) 
    motor4.duty_u16(B2) 

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 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:
            ADC_L = left.read()
            ADC_R = right.read()
            ADC_MAX = ADC_L + ADC_R
            if ADC_MAX > 500 and ADC_MAX < 6000:
                speed = int(Map(distance,500,6000,0,65535))
                for n in range(0,6):
                    np[n] = (255,255,0)
                np.write()
                Tracking()
            else if ADC_MAX < 500:
                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] = (255,0,0)
                np.write()
                Tracking()
                
if __name__ == '__main__':
    setup()
    loop()

