from Barra16 import Barra16
from Driver import Driver
from neopixel import NeoPixel
from time import sleep
from machine import Pin

barra = Barra16(18,19,20,21,27)
driver = Driver(4,6,5,7,9,8,10,hz=5000)


neo = NeoPixel(Pin(23,Pin.OUT),1)
boton = Pin(24,mode=Pin.IN, pull=Pin.PULL_UP)

start = False

def interrupcion(pin):
    global start
    start= not start
boton.irq(trigger=Pin.IRQ_FALLING, handler=interrupcion)

#-----------------------------------PID VARS------------------------------------------
kp=4
ki=0.000001
kd=3
P=I=D=0
error =0
errorAnterior=0

vel=10000
while True:

    if not start:
        driver.setVelocidad(0,0)
        neo.fill((0,10,0))
        neo.write()
        sleep(.01)
    else:
        neo.fill((0,0,10))
        neo.write()
        P=barra.getPos()
        if(P>-100 and P<100):
            I=(I+error)
        else:
            I=0
      
        D=P-errorAnterior
      
        errorAnterior=P
        pid=(kp*P)+(ki*I)+(kd*D)
        driver.setVelocidad(vel-pid,vel+pid)