from Barra16 import Barra16
from Driver import Driver
from neopixel import NeoPixel
from time import sleep
from machine import Pin, UART
import _thread

sleep(1)

#-------------------------OBJETOS BARRA Y DRIVER------------------------------------------
barra = Barra16(18,19,20,21,27)
driver = Driver(4,6,5,7,9,8,10,hz=5000)

#------------------------------LED RGB------------------------------------------
neo = NeoPixel(Pin(23,Pin.OUT),1)

#-------------------------------BOTON------------------------------------------
boton = Pin(24,mode=Pin.IN, pull=Pin.PULL_UP)

#------------------------VARIABLES GENERALES------------------------------------------
start = False
vel=10000
bat = 7.5

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

#--------------------------VARIABLES PID------------------------------------------
kp=4
ki=0.1
kd=3
P=I=D=0
errorAnterior=0

#--------------------------PROGRAMA PRINCIPAL------------------------------------------
def principal():
    global kp,ki,kd,vel,bat,start
    global P,I,D,errorAnterior

    neo.fill((10,10,10))
    neo.write()
    barra.calibrar(veces=800)

    while True:
        #----------------------LISTO PERO NO CORRIENDO------------------------------------------
        if not start:
            driver.setVelocidad(0,0)
            neo.fill((0,10,0))
            neo.write()
            sleep(.01)
        #--------------------------CORRIENDO------------------------------------------
        else:
            neo.fill((0,0,10))
            neo.write()
            
            P=barra.getPos() #Obtener el error
            
            #EL VALOR DE I ACTUALMENTE NO FUNCIONA
            if(P>-100 and P<100):
                I=(I+P)
            else:
                I=0

            #CALCULAR PENDIENTE
            D=P-errorAnterior

            #ACTUALIZAR EL ERROR ANTERIOR AL ACTUAL
            errorAnterior=P
            #CALCULAR EL PID
            pid=(kp*P)+(ki*I)+(kd*D)
            #APLICAR LOS CABIOS DE VELOCIDAD
            driver.setVelocidad(vel-pid,vel+pid)


def comunicacion():
    global kp,ki,kd,vel,bat,start
    serial = UART(0, baudrate=9600, tx=Pin(12), rx=Pin(13))
    while True:
        if serial.any():
            cad=""
            cad=cad+str(serial.read(1).decode())
            if "{" in cad:
                while "}" not  in cad:
                    c=serial.read(1)
                    if(type(c)==bytes):
                        cad=cad+str(c.decode())
            cad=cad.replace("{","")
            cad=cad.replace("}","")
            print(cad)
            if "Estatus" in cad:
                print("{"+str(kp)+" "+str(ki)+" "+str(kd)+" "+str(vel)+" "+str(bat)+" "+str(start)+"}\n")
                serial.write("{"+str(kp)+" "+str(ki)+" "+str(kd)+" "+str(vel)+" "+str(bat)+" "+str(start)+"}\n")
            elif "Start" in cad:
                    start = not start
            else:
                data=cad.split(" ")
                if len(data) ==4:
                    print (data)
                    kp=float(data[0])
                    ki=float(data[1])
                    kd=float(data[2])
                    vel=int(data[3])
    

second_thread = _thread.start_new_thread(comunicacion, ())
principal()