98 lines
2.9 KiB
Python
98 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()
|