107 lines
2.9 KiB
Python
107 lines
2.9 KiB
Python
import time
|
|
from PCA9685 import PCA9685
|
|
|
|
|
|
class Motor:
|
|
def __init__(self):
|
|
self.pwm = PCA9685(0x40, debug=True)
|
|
self.pwm.setPWMFreq(50)
|
|
|
|
def duty_range(self, duty1, duty2, duty3, duty4):
|
|
if duty1 > 4095:
|
|
duty1 = 4095
|
|
elif duty1 < -4095:
|
|
duty1 = -4095
|
|
|
|
if duty2 > 4095:
|
|
duty2 = 4095
|
|
elif duty2 < -4095:
|
|
duty2 = -4095
|
|
|
|
if duty3 > 4095:
|
|
duty3 = 4095
|
|
elif duty3 < -4095:
|
|
duty3 = -4095
|
|
|
|
if duty4 > 4095:
|
|
duty4 = 4095
|
|
elif duty4 < -4095:
|
|
duty4 = -4095
|
|
return duty1, duty2, duty3, duty4
|
|
|
|
def left_Upper_Wheel(self, duty):
|
|
if duty > 0:
|
|
self.pwm.setMotorPwm(0, 0)
|
|
self.pwm.setMotorPwm(1, duty)
|
|
elif duty < 0:
|
|
self.pwm.setMotorPwm(1, 0)
|
|
self.pwm.setMotorPwm(0, abs(duty))
|
|
else:
|
|
self.pwm.setMotorPwm(0, 4095)
|
|
self.pwm.setMotorPwm(1, 4095)
|
|
|
|
def left_Lower_Wheel(self, duty):
|
|
if duty > 0:
|
|
self.pwm.setMotorPwm(3, 0)
|
|
self.pwm.setMotorPwm(2, duty)
|
|
elif duty < 0:
|
|
self.pwm.setMotorPwm(2, 0)
|
|
self.pwm.setMotorPwm(3, abs(duty))
|
|
else:
|
|
self.pwm.setMotorPwm(2, 4095)
|
|
self.pwm.setMotorPwm(3, 4095)
|
|
|
|
def right_Upper_Wheel(self, duty):
|
|
if duty > 0:
|
|
self.pwm.setMotorPwm(6, 0)
|
|
self.pwm.setMotorPwm(7, duty)
|
|
elif duty < 0:
|
|
self.pwm.setMotorPwm(7, 0)
|
|
self.pwm.setMotorPwm(6, abs(duty))
|
|
else:
|
|
self.pwm.setMotorPwm(6, 4095)
|
|
self.pwm.setMotorPwm(7, 4095)
|
|
|
|
def right_Lower_Wheel(self, duty):
|
|
if duty > 0:
|
|
self.pwm.setMotorPwm(4, 0)
|
|
self.pwm.setMotorPwm(5, duty)
|
|
elif duty < 0:
|
|
self.pwm.setMotorPwm(5, 0)
|
|
self.pwm.setMotorPwm(4, abs(duty))
|
|
else:
|
|
self.pwm.setMotorPwm(4, 4095)
|
|
self.pwm.setMotorPwm(5, 4095)
|
|
|
|
def setMotorModel(self, duty1, duty2, duty3, duty4):
|
|
duty1, duty2, duty3, duty4 = self.duty_range(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()
|