From 6b78079b8dbd2d73d36ed92c015aac6ce3731806 Mon Sep 17 00:00:00 2001
From: Miguel Delgado <miguel.delgado.lop@gmail.com>
Date: Tue, 5 Nov 2024 07:14:23 -0600
Subject: [PATCH] Estructura Inicial

---
 Barra16.py  | 33 +++++++++++++++++++++++++++++++++
 Driver.py   | 42 ++++++++++++++++++++++++++++++++++++++++++
 Follower.py | 52 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 127 insertions(+)
 create mode 100644 Barra16.py
 create mode 100644 Driver.py
 create mode 100644 Follower.py

diff --git a/Barra16.py b/Barra16.py
new file mode 100644
index 0000000..b294a43
--- /dev/null
+++ b/Barra16.py
@@ -0,0 +1,33 @@
+from machine import Pin,ADC
+class Barra16:
+
+    #CONSTRUCTOR
+    def __init__(self,s0,s1,s2,s3,out):
+        self.__s0=Pin(s0,Pin.OUT)
+        self.__s1=Pin(s1,Pin.OUT)
+        self.__s2=Pin(s2,Pin.OUT)
+        self.__s3=Pin(s3,Pin.OUT)
+        self.__out=ADC(Pin(out))
+
+    #METODOS PRIVADOS
+    def __read(self,x):
+        bits=bin(x)
+        bits=bits.split("b")[1]
+        for i in range (len(bits),4):
+            bits="0"+bits
+        self.__s0.value(bits[3]=='1')
+        self.__s1.value(bits[2]=='1')
+        self.__s2.value(bits[1]=='1')
+        self.__s3.value(bits[0]=='1')
+        return self.__out.read_u16()
+    
+    #METODOS PUBLICOS
+    def getPos(self):
+        pos=0
+        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
+        return -1 if sen==0 else (pos/sen)-4250
+            
\ No newline at end of file
diff --git a/Driver.py b/Driver.py
new file mode 100644
index 0000000..dc75b7c
--- /dev/null
+++ b/Driver.py
@@ -0,0 +1,42 @@
+from machine import Pin,PWM
+
+class Driver:
+
+    def __init__(self, pwma,ain1,ain2,stby,bin2,bin1,pwmb,hz=2000):
+        self.__pwma=PWM(Pin(pwma), freq=hz, duty_u16=0)
+        self.__ain1 = Pin(ain1, Pin.OUT)
+        self.__ain2 = Pin(ain2, Pin.OUT)
+        self.__stby = Pin(stby, Pin.OUT)
+        self.__bin2 = Pin(bin2, Pin.OUT)
+        self.__bin1 = Pin(bin1, Pin.OUT)
+        self.__pwmb = PWM(Pin(pwmb), freq=hz, duty_u16=0)
+        self.__stby.on()#Enciende el driver
+
+    def setVelocidad(self, motIzq, motDer):
+        #Validar que este en el rango
+        if(motDer>65535):
+            motDer=65535
+        if motIzq>65535 :
+            motIzq=65535 
+        if(motDer<-65535):
+            motDer=-65535
+        if motIzq<-65535 :
+            motIzq=-65535 
+        #REVISAR DIRECCION
+        if (motIzq >= 0):
+            self.__ain1.on()
+            self.__ain2.off()
+        else:
+            self.__ain1.off()
+            self.__ain2.on()
+            motIzq=motIzq*-1 #HACER POSITIVO
+        if (motDer >= 0):
+            self.__bin1.off()
+            self.__bin2.on()
+        else:
+            self.__bin1.on()
+            self.__bin2.off()
+            motDer=motDer*-1 #HACER POSITIVO
+        self.__pwma.duty_u16(int(motIzq))
+        self.__pwmb.duty_u16(int(motDer))
+
diff --git a/Follower.py b/Follower.py
new file mode 100644
index 0000000..d43a2f6
--- /dev/null
+++ b/Follower.py
@@ -0,0 +1,52 @@
+from Barra16 import Barra16
+from Driver import Driver
+from neopixel import NeoPixel
+from time import sleep
+from machine import Pin
+
+barra = Barra16(18,19,20,21,27)
+driver = Driver(4,6,5,7,9,8,10,hz=5000)
+
+
+neo = NeoPixel(Pin(23,Pin.OUT),1)
+boton = Pin(24,mode=Pin.IN, pull=Pin.PULL_UP)
+
+start = False
+
+def interrupcion(pin):
+    global start
+    start= not start
+boton.irq(trigger=Pin.IRQ_FALLING, handler=interrupcion)
+
+#-----------------------------------PID VARS------------------------------------------
+kp=4
+ki=0.000001
+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)
+        else:
+            I=0
+      
+        D=P-errorAnterior
+      
+        errorAnterior=P
+        pid=(kp*P)+(ki*I)+(kd*D)
+        driver.setVelocidad(vel-pid,vel+pid)
+        
+