From a2fbacf5d9a487b471f4de2c87d766209bc9ad32 Mon Sep 17 00:00:00 2001
From: Miguel Delgado <miguel.delgado.lop@gmail.com>
Date: Mon, 18 Nov 2024 20:02:27 -0600
Subject: [PATCH] =?UTF-8?q?Calibraci=C3=B3n=20de=20la=20barra=20y=20Comuni?=
 =?UTF-8?q?caci=C3=B3n=20Bluetooth?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .DS_Store   | Bin 0 -> 6148 bytes
 Barra16.py  |  23 ++++++++++-
 Follower.py | 112 +++++++++++++++++++++++++++++++++++++++-------------
 3 files changed, 106 insertions(+), 29 deletions(-)
 create mode 100644 .DS_Store

diff --git a/.DS_Store b/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..2e40325c9fcf8f45856244d5ef77154bc83af2bc
GIT binary patch
literal 6148
zcmeHKy-veG47N*#NGu&0Z)8IB4UQ^2K}Qx;Dix$gsZ`jr@n(I5PHemgpY5Y+6qS(y
zK1=qO^Uq!K4d+-ybpE!QiA+VLh8oJdnXy>hbR?7Fseo#GY}T{IQ`c@sndco=Tgi>A
z<wahH^*h`D7;C$xS#G)|eD&e;;p4j9-wordALHBSlXsikqLe}g$N(8217zS|GJu|K
zQeOtrk^wS6296Ba{g9xBxnLjYR|kxb06_bVcfqsI62N2*U@q7P!UNGL1xBfs5ksRK
z{#5dE!9Fm`Mg3&#Q%=@0p{Sn@fAVzE97szB$iR?+i&##r|KCH;%>Tn8uE+ox_)`pM
z)hwDhKB@H9(Z^Y@P0&Z^w;<QkVZ0Rsy%l3&t@!FBFWEEVxnLjY<?wqskUs*(3xy2)
G1_NKUWhuD;

literal 0
HcmV?d00001

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]<val:
+                    max[x]=val
+            ##sleep(.00001)
+        
+        for x in range(16):
+            self.__umbral[x]=int((max[x]+min[x])/2)
+
             
\ No newline at end of file
diff --git a/Follower.py b/Follower.py
index d43a2f6..e183c92 100644
--- a/Follower.py
+++ b/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
-      
-        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()
+