diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..2e40325 Binary files /dev/null and b/.DS_Store differ diff --git a/Barra16.py b/Barra16.py index b294a43..688302f 100644 --- a/Barra16.py +++ b/Barra16.py @@ -1,4 +1,5 @@ from machine import Pin,ADC +from time import sleep class Barra16: #CONSTRUCTOR @@ -8,6 +9,7 @@ class Barra16: self.__s2=Pin(s2,Pin.OUT) self.__s3=Pin(s3,Pin.OUT) self.__out=ADC(Pin(out)) + self.__umbral=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] #METODOS PRIVADOS def __read(self,x): @@ -27,7 +29,24 @@ class Barra16: sen=0 for x in range(16): v=self.__read(x) - sen+=1 if v>50000 else 0 - pos+=500*(x+1) if v>50000 else 0 + sen+=1 if v>self.__umbral[x] else 0 + pos+=500*(x+1) if v>self.__umbral[x] else 0 return -1 if sen==0 else (pos/sen)-4250 + + def calibrar(self, veces=10000): + max = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] + min = [65535,65535,65535,65535,65535,65535,65535,65535,65535,65535,65535,65535,65535,65535,65535,65535,] + + for v in range(veces): + for x in range(16): + val=self.__read(x) + if min[x]>val: + min[x]=val + if max[x]-100 and P<100): - I=(I+error) +#--------------------------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: - I=0 - - D=P-errorAnterior - - errorAnterior=P - pid=(kp*P)+(ki*I)+(kd*D) - driver.setVelocidad(vel-pid,vel+pid) - + 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() +