You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
110 lines
3.3 KiB
110 lines
3.3 KiB
from Barra16 import Barra16
|
|
from Driver import Driver
|
|
from neopixel import NeoPixel
|
|
from time import sleep
|
|
from machine import Pin, UART
|
|
import _thread
|
|
|
|
sleep(3)
|
|
|
|
#-------------------------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()
|
|
|
|
|