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.
52 lines
1022 B
52 lines
1022 B
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)
|
|
|
|
|
|
|