Calibración de la barra y Comunicación Bluetooth

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]<val:
+                    max[x]=val
+            ##sleep(.00001)
+        
+        for x in range(16):
+            self.__umbral[x]=int((max[x]+min[x])/2)
+
+            

Follower.py

@@ -2,51 +2,109 @@ from Barra16 import Barra16
 from Driver import Driver
 from neopixel import NeoPixel
 from time import sleep
-from machine import Pin
+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)
 
-#-----------------------------------PID VARS------------------------------------------
+#--------------------------VARIABLES PID------------------------------------------
 kp=4
-ki=0.000001
+ki=0.1
 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)
+#--------------------------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
+            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)
+
 
-        D=P-errorAnterior
+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])
     
-        errorAnterior=P
-        pid=(kp*P)+(ki*I)+(kd*D)
-        driver.setVelocidad(vel-pid,vel+pid)
 
+second_thread = _thread.start_new_thread(comunicacion, ())
+principal()