75 lines
2.3 KiB
Python
75 lines
2.3 KiB
Python
|
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()
|