from machine import Pin, ADC, PWM, I2C
import time
from ssd1306 import SSD1306_I2C
i2c=I2C(0,scl=Pin(1),sda=Pin(0),freq=200000)
oled = SSD1306_I2C(128 ,64,i2c)
time.sleep(0.1) # Wait for USB to become ready
#variables captura si nomas hice este comentario porque son demasiadas y no se como hacer memorias
A1 = 90
A2 = 90
A3 = 90
A4 = 90
A5 = 90
A6 = 90
A7 = 90
A8 = 90
B1 = 90
B2 = 90
B3 = 90
B4 = 90
B5 = 90
B6 = 90
B7 = 90
B8 = 90
C1 = 90
C2 = 90
C3 = 90
C4 = 90
C5 = 90
C6 = 90
C7 = 90
C8 = 90
D1 = 90
D2 = 90
D3 = 90
D4 = 90
D5 = 90
D6 = 90
D7 = 90
D8 = 90
E1 = 90
E2 = 90
E3 = 90
E4 = 90
E5 = 90
E6 = 90
E7 = 90
E8 = 90
F1 = 90
F2 = 90
F3 = 90
F4 = 90
F5 = 90
F6 = 90
F7 = 90
F8 = 90
#final feliz i guess bro
paroemergencia = Pin(7, Pin.IN, Pin.PULL_DOWN)
paroemergencialast = 0
paro = 0
parolast = 1
blahblah = 0
autopuesto = 0
autolas = 0
autonum = 0
captura = Pin(22, Pin.IN, Pin.PULL_DOWN)
capturalast = 0
numcap = 1
numlast = 0
borrar = Pin(26, Pin.IN, Pin.PULL_DOWN)
borrarlast = 0
manual = False
autosetup = False
botonmanual = Pin(3, Pin.IN, Pin.PULL_DOWN)
botonauto = Pin(4, Pin.IN, Pin.PULL_DOWN)
luzr = (Pin(13, Pin.OUT))
luza = (Pin(14, Pin.OUT))
luzv = (Pin(15, Pin.OUT))
sensor = Pin(2, Pin.IN, Pin.PULL_DOWN)
angulo1 = 90
angulo11 = 0
angulo2 = 90
angulo22 = 0
angulo3 = 90
angulo33 = 0
angulo4 = 90
angulo44 = 0
angulo5 = 90
angulo55 = 0
angulo6 = 90
angulo66 = 0
vel =0
boton = Pin(5, Pin.IN, Pin.PULL_DOWN)
boton2 = Pin(6, Pin.IN, Pin.PULL_DOWN)
botonanterior = 0
botonanterior2 = 0
servo1 = PWM(16)
servo1.freq(50)
servo2 = PWM(17)
servo2.freq(50)
servo3 = PWM(18)
servo3.freq(50)
servo4 = PWM(19)
servo4.freq(50)
servo5 = PWM(20)
servo5.freq(50)
servo6 = PWM(21)
servo6.freq(50)
joy = ADC(28)
jlast = 0
servoactual = 1
servolast = 9
servo1.duty_u16(int(4915.125))
servo2.duty_u16(int(4915.125))
servo3.duty_u16(int(4915.125))
servo4.duty_u16(int(4915.125))
servo5.duty_u16(int(4915.125))
servo6.duty_u16(int(4915.125))
while True:
print(paro, parolast)
if paroemergencia.value() == 1 and paroemergencialast == 0:
time.sleep_ms(20)
paro += 1
paroemergencialast = paroemergencia.value()
paroemergencialast = paroemergencia.value()
time.sleep_ms(20)
if paro == 0:
if parolast == 1 and paro == 0:
autopuesto = 0
oled.fill(0)
oled.text("PARO DES", 0, 0)
oled.show()
parolast = paro
if botonmanual.value() == 1 and botonauto.value() != 1:
manual = True
autonum = 0
autopuesto = 0
blahblah = 0
oled.fill(0)
oled.text(str(servoactual), 110, 0)
oled.text("A1:" + str(angulo1), 0, 0)
oled.text("A2:" + str(angulo2), 0, 10)
oled.text("A3:" + str(angulo3), 0, 20)
oled.text("A4:" + str(angulo4), 0, 30)
oled.text("A5:" + str(angulo5), 0, 40)
oled.text("A6:" + str(angulo6), 0, 50)
oled.show()
if botonmanual.value() != 1 and autolas == 0 and botonauto.value() == 1:
manual = False
autonum += 1
autolas = botonauto.value()
autolas = botonauto.value()
time.sleep_ms(20)
if manual == True:
j = joy.read_u16()
if boton.value() == 1 and botonanterior == 0 and boton2.value() != 1:
servoactual += 1
time.sleep(.5)
if boton.value() != 1 and botonanterior2 == 0 and boton2.value() == 1:
servoactual -= 1
time.sleep(.5)
botonanterior = boton.value()
botonanterior2 = boton2.value()
if servoactual == 7:
servoactual = (servoactual-6)
time.sleep(1)
if servoactual == 0:
servoactual = (servoactual+6)
time.sleep(1)
if servoactual != servolast:
print("\033[2J\033[H", end="")
oled.fill(0)
oled.text(str(servoactual), 110, 0)
oled.text("A1:" + str(angulo1), 0, 0)
oled.text("A2:" + str(angulo2), 0, 10)
oled.text("A3:" + str(angulo3), 0, 20)
oled.text("A4:" + str(angulo4), 0, 30)
oled.text("A5:" + str(angulo5), 0, 40)
oled.text("A6:" + str(angulo6), 0, 50)
oled.show()
print(servoactual)
servolast = servoactual
if j != jlast:
print(j)
jlast = j
if j < 24000 or j > 40000:
luzr.value(0)
luzv.value(1)
else:
luzr.value(1)
luzv.value(0)
if sensor.value() == 1:
luza.value(1)
else:
luza.value(0)
if servoactual == 1:
if j < 24000:
vel = (24000 - j)/10000
angulo1 += vel
if j > 40000:
vel = (40000 - j)/10000
angulo1 += vel
angulo1 = max(0, min(180, angulo1))
pulso = 500 + (angulo1 / 180) * 2000
servo1.duty_u16(int(pulso * 65535 / 20000))
time.sleep_ms(20)
if servoactual == 2:
if j < 24000:
vel = (24000 - j)/10000
angulo2 += vel
if j > 40000:
vel = (40000 - j)/10000
angulo2 += vel
angulo2 = max(0, min(180, angulo2))
pulso = 500 + (angulo2 / 180) * 2000
servo2.duty_u16(int(pulso * 65535 / 20000))
time.sleep_ms(20)
if servoactual == 3:
if j < 24000:
vel = (24000 - j)/10000
angulo3 += vel
if j > 40000:
vel = (40000 - j)/10000
angulo3 += vel
angulo3 = max(0, min(180, angulo3))
pulso = 500 + (angulo3 / 180) * 2000
servo3.duty_u16(int(pulso * 65535 / 20000))
time.sleep_ms(20)
if servoactual == 4:
if j < 24000:
vel = (24000 - j)/10000
angulo4 += vel
if j > 40000:
vel = (40000 - j)/10000
angulo4 += vel
angulo4 = max(0, min(180, angulo4))
pulso = 500 + (angulo4 / 180) * 2000
servo4.duty_u16(int(pulso * 65535 / 20000))
time.sleep_ms(20)
if servoactual == 5:
if j < 24000:
vel = (24000 - j)/10000
angulo5 += vel
if j > 40000:
vel = (40000 - j)/10000
angulo5 += vel
angulo5 = max(0, min(180, angulo5))
pulso = 500 + (angulo5 / 180) * 2000
servo5.duty_u16(int(pulso * 65535 / 20000))
time.sleep_ms(20)
if servoactual == 6:
if j < 24000:
vel = (24000 - j)/10000
angulo6 += vel
if j > 40000:
vel = (40000 - j)/10000
angulo6 += vel
angulo6 = max(0, min(180, angulo6))
pulso = 500 + (angulo6 / 180) * 2000
servo6.duty_u16(int(pulso * 65535 / 20000))
time.sleep_ms(20)
if angulo1 != angulo11 or angulo2 != angulo22 or angulo3 != angulo33 or angulo4!= angulo44 or angulo5 != angulo55 or angulo6 != angulo66:
print("A1:",angulo1, "A2:",angulo2, "A3:",angulo3, "A4:",angulo4, "A5:",angulo5, "A6:",angulo6)
oled.fill(0)
oled.text(str(servoactual), 110, 0)
oled.text("A1:" + str(angulo1), 0, 0)
oled.text("A2:" + str(angulo2), 0, 10)
oled.text("A3:" + str(angulo3), 0, 20)
oled.text("A4:" + str(angulo4), 0, 30)
oled.text("A5:" + str(angulo5), 0, 40)
oled.text("A6:" + str(angulo6), 0, 50)
oled.show()
angulo11 = angulo1
angulo22 = angulo2
angulo33 = angulo3
angulo44 = angulo4
angulo55 = angulo5
angulo66 = angulo6
time.sleep_ms(20)
if captura.value() == 1 and capturalast == 0:
oled.fill(0)
if numcap >= 9:
numcap = 8
if numcap == 8:
A8 = angulo1
B8 = angulo2
C8 = angulo3
D8 = angulo4
E8 = angulo5
F8 = angulo6
print("Capturada POS 8")
oled.text("POS8", 90, 20)
if numcap == 7:
A7 = angulo1
B7 = angulo2
C7 = angulo3
D7 = angulo4
E7 = angulo5
F7 = angulo6
print("Capturada POS 7")
oled.text("POS7", 90, 20)
if numcap == 6:
A6 = angulo1
B6 = angulo2
C6 = angulo3
D6 = angulo4
E6 = angulo5
F6 = angulo6
print("Capturada POS 6")
oled.text("POS6", 90, 20)
if numcap == 5:
A5 = angulo1
B5 = angulo2
C5 = angulo3
D5 = angulo4
E5 = angulo5
F5 = angulo6
print("Capturada POS 5")
oled.text("POS5", 90, 20)
if numcap == 4:
A4 = angulo1
B4 = angulo2
C4 = angulo3
D4 = angulo4
E4 = angulo5
F4 = angulo6
print("Capturada POS 4")
oled.text("POS4", 90, 20)
if numcap == 3:
A3 = angulo1
B3 = angulo2
C3 = angulo3
D3 = angulo4
E3 = angulo5
F3 = angulo6
print("Capturada POS 3")
oled.text("POS3", 90, 20)
if numcap == 2:
A2 = angulo1
B2 = angulo2
C2 = angulo3
D2 = angulo4
E2 = angulo5
F2 = angulo6
print("Capturada POS 2")
oled.text("POS2", 90, 20)
if numcap == 1:
A1 = angulo1
B1 = angulo2
C1 = angulo3
D1 = angulo4
E1 = angulo5
F1 = angulo6
print("Capturada POS 1")
oled.text("POS1", 90, 20)
oled.text(str(servoactual), 110, 0)
oled.text("A1:" + str(angulo1), 0, 0)
oled.text("A2:" + str(angulo2), 0, 10)
oled.text("A3:" + str(angulo3), 0, 20)
oled.text("A4:" + str(angulo4), 0, 30)
oled.text("A5:" + str(angulo5), 0, 40)
oled.text("A6:" + str(angulo6), 0, 50)
oled.show()
numcap += 1
capturalast = captura.value()
if borrar.value() == 1 and borrarlast == 0:
numcap -= 1
oled.fill(0)
if numcap <= 0:
numcap = 1
if numcap == 1:
A1 = 0
B1 = 0
C1 = 0
D1 = 0
E1 = 0
F1 = 0
print("BORRADA POS 1")
oled.text("POSV", 90, 20)
if numcap == 2:
A2 = 0
B2 = 0
C2 = 0
D2 = 0
E2 = 0
F2 = 0
print("BORRADA POS 2")
oled.text("POS1", 90, 20)
if numcap == 3:
A3 = 0
B3 = 0
C3 = 0
D3 = 0
E3 = 0
F3 = 0
print("BORRADA POS 3")
oled.text("POS2", 90, 20)
if numcap == 4:
A4 = 0
B4 = 0
C4 = 0
D4 = 0
E4 = 0
F4 = 0
print("BORRADA POS 4")
oled.text("POS3", 90, 20)
if numcap == 5:
A5 = 0
B5 = 0
C5 = 0
D5 = 0
E5 = 0
F5 = 0
print("BORRADA POS 5")
oled.text("POS4", 90, 20)
if numcap == 6:
A6 = 0
B6 = 0
C6 = 0
D6 = 0
E6 = 0
F6 = 0
print("BORRADA POS 6")
oled.text("POS5", 90, 20)
if numcap == 7:
A7 = 0
B7 = 0
C7 = 0
D7 = 0
E7 = 0
F7 = 0
print("BORRADA POS 7")
oled.text("POS6", 90, 20)
if numcap == 8:
A8 = 0
B8 = 0
C8 = 0
D8 = 0
E8 = 0
F8 = 0
print("BORRADA POS 8")
oled.text("POS7", 90, 20)
oled.text(str(servoactual), 110, 0)
oled.text("A1:" + str(angulo1), 0, 0)
oled.text("A2:" + str(angulo2), 0, 10)
oled.text("A3:" + str(angulo3), 0, 20)
oled.text("A4:" + str(angulo4), 0, 30)
oled.text("A5:" + str(angulo5), 0, 40)
oled.text("A6:" + str(angulo6), 0, 50)
oled.show()
borrarlast = borrar.value()
if autonum == 1 and autopuesto != autonum:
luzr.value(1)
luzv.value(0)
oled.fill(0)
oled.text("Auto DESACTIVADO", 0, 0)
oled.text("Presione de", 0, 20)
oled.text("nuevo para", 0, 30)
oled.text("iniciar", 0, 40)
oled.show()
servo1.duty_u16(int(4915.125))
servo2.duty_u16(int(4915.125))
servo3.duty_u16(int(4915.125))
servo4.duty_u16(int(4915.125))
servo5.duty_u16(int(4915.125))
servo6.duty_u16(int(4915.125))
autopuesto = autonum
if autonum == 2:
if numcap <= 0 and blahblah == 0:
oled.fill(0)
oled.text("Memoria vacía", 0, 0)
oled.text("Capture POS", 0, 20)
oled.text("en modo", 0, 30)
oled.text("manual", 0, 40)
oled.show()
blahblah = 1
if numcap >= 1:
luzv.value(1)
luzr.value(0)
oled.fill(0)
oled.text("Auto ACTIVADO", 0, 0)
oled.text("Presione de", 0, 20)
oled.text("nuevo para", 0, 30)
oled.text("detener", 0, 40)
oled.show()
if numcap >= 1:
servo1.duty_u16(int(((500 + (A1 / 180) * 2000) * 65535) / 20000))
servo2.duty_u16(int(((500 + (B1 / 180) * 2000) * 65535) / 20000))
servo3.duty_u16(int(((500 + (C1 / 180) * 2000) * 65535) / 20000))
servo4.duty_u16(int(((500 + (D1 / 180) * 2000) * 65535) / 20000))
servo5.duty_u16(int(((500 + (E1 / 180) * 2000) * 65535) / 20000))
servo6.duty_u16(int(((500 + (F1 / 180) * 2000) * 65535) / 20000))
time.sleep(1)
if numcap >= 2:
servo1.duty_u16(int(((500 + (A2 / 180) * 2000) * 65535) / 20000))
servo2.duty_u16(int(((500 + (B2 / 180) * 2000) * 65535) / 20000))
servo3.duty_u16(int(((500 + (C2 / 180) * 2000) * 65535) / 20000))
servo4.duty_u16(int(((500 + (D2 / 180) * 2000) * 65535) / 20000))
servo5.duty_u16(int(((500 + (E2 / 180) * 2000) * 65535) / 20000))
servo6.duty_u16(int(((500 + (F2 / 180) * 2000) * 65535) / 20000))
time.sleep(1)
if numcap >= 3:
servo1.duty_u16(int(((500 + (A3 / 180) * 2000) * 65535) / 20000))
servo2.duty_u16(int(((500 + (B3 / 180) * 2000) * 65535) / 20000))
servo3.duty_u16(int(((500 + (C3 / 180) * 2000) * 65535) / 20000))
servo4.duty_u16(int(((500 + (D3 / 180) * 2000) * 65535) / 20000))
servo5.duty_u16(int(((500 + (E3 / 180) * 2000) * 65535) / 20000))
servo6.duty_u16(int(((500 + (F3 / 180) * 2000) * 65535) / 20000))
time.sleep(1)
if numcap >= 4:
servo1.duty_u16(int(((500 + (A4 / 180) * 2000) * 65535) / 20000))
servo2.duty_u16(int(((500 + (B4 / 180) * 2000) * 65535) / 20000))
servo3.duty_u16(int(((500 + (C4 / 180) * 2000) * 65535) / 20000))
servo4.duty_u16(int(((500 + (D4 / 180) * 2000) * 65535) / 20000))
servo5.duty_u16(int(((500 + (E4 / 180) * 2000) * 65535) / 20000))
servo6.duty_u16(int(((500 + (F4 / 180) * 2000) * 65535) / 20000))
time.sleep(1)
if numcap >= 5:
servo1.duty_u16(int(((500 + (A5 / 180) * 2000) * 65535) / 20000))
servo2.duty_u16(int(((500 + (B5 / 180) * 2000) * 65535) / 20000))
servo3.duty_u16(int(((500 + (C5 / 180) * 2000) * 65535) / 20000))
servo4.duty_u16(int(((500 + (D5 / 180) * 2000) * 65535) / 20000))
servo5.duty_u16(int(((500 + (E5 / 180) * 2000) * 65535) / 20000))
servo6.duty_u16(int(((500 + (F5 / 180) * 2000) * 65535) / 20000))
time.sleep(1)
if numcap >= 6:
servo1.duty_u16(int(((500 + (A6 / 180) * 2000) * 65535) / 20000))
servo2.duty_u16(int(((500 + (B6 / 180) * 2000) * 65535) / 20000))
servo3.duty_u16(int(((500 + (C6 / 180) * 2000) * 65535) / 20000))
servo4.duty_u16(int(((500 + (D6 / 180) * 2000) * 65535) / 20000))
servo5.duty_u16(int(((500 + (E6 / 180) * 2000) * 65535) / 20000))
servo6.duty_u16(int(((500 + (F6 / 180) * 2000) * 65535) / 20000))
time.sleep(1)
if numcap >= 7:
servo1.duty_u16(int(((500 + (A7 / 180) * 2000) * 65535) / 20000))
servo2.duty_u16(int(((500 + (B7 / 180) * 2000) * 65535) / 20000))
servo3.duty_u16(int(((500 + (C7 / 180) * 2000) * 65535) / 20000))
servo4.duty_u16(int(((500 + (D7 / 180) * 2000) * 65535) / 20000))
servo5.duty_u16(int(((500 + (E7 / 180) * 2000) * 65535) / 20000))
servo6.duty_u16(int(((500 + (F7 / 180) * 2000) * 65535) / 20000))
time.sleep(1)
if numcap >= 8:
servo1.duty_u16(int(((500 + (A8 / 180) * 2000) * 65535) / 20000))
servo2.duty_u16(int(((500 + (B8 / 180) * 2000) * 65535) / 20000))
servo3.duty_u16(int(((500 + (C8 / 180) * 2000) * 65535) / 20000))
servo4.duty_u16(int(((500 + (D8 / 180) * 2000) * 65535) / 20000))
servo5.duty_u16(int(((500 + (E8 / 180) * 2000) * 65535) / 20000))
servo6.duty_u16(int(((500 + (F8 / 180) * 2000) * 65535) / 20000))
time.sleep(1)
if autonum >= 3:
autopuesto = autonum
time.sleep_ms(20)
autonum = 1
if paro == 1 and parolast == 0:
autonum = 0
oled.fill(0)
oled.text("PARO", 0, 0)
oled.show()
luzv.value(0)
luzr.value(0)
parolast = paro
print(paro)
if paro == 2:
paro = 0
1
2
3
4
5
EX
Loading
ssd1306
ssd1306