import time
from PCA9685 import PCA9685
class Motor:
    def __init__(self):
        self.pwm = PCA9685(0x40, debug=True)
        self.pwm.setPWMFreq(50)
    def left_Upper_Wheel(self,duty):
        if duty>0:
            self.pwm.setMotorPwm(1,0)
            self.pwm.setMotorPwm(0,duty)
        elif duty<0:
            self.pwm.setMotorPwm(0,0)
            self.pwm.setMotorPwm(1,abs(duty))
        else:
            self.pwm.setMotorPwm(0,4096)
            self.pwm.setMotorPwm(1,4096)
    def left_Lower_Wheel(self,duty):
        if duty>0:
            self.pwm.setMotorPwm(2,0)
            self.pwm.setMotorPwm(3,duty)
        elif duty<0:
            self.pwm.setMotorPwm(3,0)
            self.pwm.setMotorPwm(2,abs(duty))
        else:
            self.pwm.setMotorPwm(2,4096)
            self.pwm.setMotorPwm(3,4096)
    def right_Upper_Wheel(self,duty):
        if duty>0:
            self.pwm.setMotorPwm(7,0)
            self.pwm.setMotorPwm(6,duty)
        elif duty<0:
            self.pwm.setMotorPwm(6,0)
            self.pwm.setMotorPwm(7,abs(duty))
        else:
            self.pwm.setMotorPwm(6,4096)
            self.pwm.setMotorPwm(7,4096)
    def right_Lower_Wheel(self,duty):
        if duty>0:
            self.pwm.setMotorPwm(5,0)
            self.pwm.setMotorPwm(4,duty)
        elif duty<0:
            self.pwm.setMotorPwm(4,0)
            self.pwm.setMotorPwm(5,abs(duty))
        else:
            self.pwm.setMotorPwm(4,4096)
            self.pwm.setMotorPwm(5,4096)
            
 
    def setMotorModel(self,duty1,duty2,duty3,duty4):
        self.left_Upper_Wheel(duty1)
        self.left_Lower_Wheel(duty2)
        self.right_Upper_Wheel(duty3)
        self.right_Lower_Wheel(duty4)
            
            
PWM=Motor()          
def loop(): 
    PWM.setMotorModel(2000,2000,2000,2000)       #Forward
    time.sleep(3)
    PWM.setMotorModel(-2000,-2000,-2000,-2000)   #Back
    time.sleep(3)
    PWM.setMotorModel(-500,-500,2000,2000)       #Left 
    time.sleep(3)
    PWM.setMotorModel(2000,2000,-500,-500)       #Right    
    time.sleep(3)
    PWM.setMotorModel(0,0,0,0)                   #Stop
    
def destroy():
    PWM.setMotorModel(0,0,0,0)                   
if __name__=='__main__':
    try:
        loop()
    except KeyboardInterrupt:  # When 'Ctrl+C' is pressed, the child program destroy() will be  executed.
        destroy()