#Prueba de concepto de Robot Futbolito -- STEM
from time import sleep as t #Importar libreria de tiempo
from machine import Pin, PWM, ADC #importal libreria para pines, para control PWM y para conversor Digital-Analogico

#Definiendo pines PWM de cada motor y su frecuencia
MI_D = PWM(Pin(14), freq = 1000)
MI_T = PWM(Pin(15), freq = 1000)
MD_D = PWM(Pin(11), freq = 1000)
MD_T = PWM(Pin(13), freq = 1000)

#Definiendo pines de Servomotores
S_Dire = PWM(Pin(12), freq = 50, min_duty = 1638, max_duty = 8192)
S_Disp = PWM(Pin(10), freq = 50, min_duty = 1638, max_duty = 8192)

#Definiendo pines de potenciometros
Direccion = ADC(Pin(26))
Sentido = ADC(Pin(27))
Disparo = ADC(Pin(28))

#Variables generales
u_16 = 65535
g_max = 180

def servo(servo, valor, estado):
    if estado == 0:
        ancho = 1638 + int((valor / g_max) * (8192 - 1638))\
        servo.duty_u16(ancho)
    elif estado == 1:
        ancho = 1638 + int((valor / u_16) * (8192 - 1638))\
        servo.duty_u16(ancho)

def motor(ab, bb, cb, db, estado):  #Se mandan valores de 0 a 100, estado 0 es porcentual, 1 es u_16
        #PWM de 16Bits (0 al 65535)
    if estado == 0:
        a = (u_16 / 100)*ab
        b = (u_16 / 100)*bb
        c = (u_16 / 100)*cb
        d = (u_16 / 100)*db

        MI_D.duty_u16(a)    
        MD_D.duty_u16(b) 
        MI_T.duty_u16(c)
        MD_T.duty_u16(d)

    elif estado == 1:
        MI_D.duty_u16(ab)    
        MD_D.duty_u16(bb) 
        MI_T.duty_u16(cb)
        MD_T.duty_u16(db)
   

try:
    while True:
        vDireccion = (Direccion.read_u16()/u_16) * (100)
        vSentido = Sentido.read_u16()
        vDisparo = Disparo.read_u16()

        servo(S_Dire, vDireccion, 1)
        servo(S_Disp, vDisparo, 1)
    
    if vSentido > 55:
        vel = ((vSentido - 50)/50)*100
        motor(vel, vel, 0, 0, 0)
    elif vSentido < 55:
        vel = ((50 - vSentido)/50)*100
        motor(0, 0, vel, vel, 0)



except KeyboardInterrupt:
    print('FIN')