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))