145 lines
5.1 KiB
Python
145 lines
5.1 KiB
Python
#!/usr/bin/python
|
|
|
|
import RPi.GPIO as GPIO
|
|
from Adafruit_PWM_Servo_Driver import PWM
|
|
import time
|
|
import math
|
|
|
|
class dw_PWM:
|
|
def __init__(self, controller, num, freq):
|
|
|
|
_SERVO_MIN_MS = 1.250 #ms
|
|
_SERVO_MAX_MS = 1.750 #ms
|
|
|
|
self.speed = 0
|
|
self.MC = controller
|
|
self.cnum = num
|
|
self.pin = 0
|
|
|
|
self.freq = freq
|
|
|
|
self.servo_min = math.trunc( ( _SERVO_MIN_MS * 4096 ) / (1000.0 / self.freq ) - 1 )
|
|
self.servo_max = math.trunc( ( _SERVO_MAX_MS * 4096 ) / (1000.0 / self.freq ) - 1 )
|
|
|
|
self.servo_zero = math.trunc( ( self.servo_min + self.servo_max ) / 2 ) # halfway = 0 degrees
|
|
|
|
|
|
if (num == 0):
|
|
self.pin = 9
|
|
elif (num == 1):
|
|
self.pin = 8
|
|
elif (num == 2):
|
|
self.pin = 10
|
|
elif (num == 3):
|
|
self.pin = 11
|
|
elif (num == 4):
|
|
self.pin = 12
|
|
elif (num == 5):
|
|
self.pin = 13
|
|
elif (num == 6):
|
|
self.pin = 0
|
|
elif (num == 7):
|
|
self.pin = 1
|
|
elif (num == 8):
|
|
self.pin = 2
|
|
elif (num == 9):
|
|
self.pin = 3
|
|
elif (num == 10):
|
|
self.pin = 5
|
|
elif (num == 11):
|
|
self.pin = 4
|
|
else:
|
|
raise NameError('Port must be between 1 and 12 inclusive')
|
|
|
|
# switch off
|
|
self.off()
|
|
|
|
def off(self):
|
|
self.MC.setPin(self.pin, 0)
|
|
|
|
def setAngle(self, angle):
|
|
pulse = self.servo_zero + ( (self.servo_zero - self.servo_min ) * angle / 80 )
|
|
print "angle=%s pulse=%s" % (angle, pulse)
|
|
#self.setPWMmS( pulse )
|
|
|
|
def setPWM(self, value):
|
|
if(value > 0):
|
|
self.MC._pwm.setPWM(self.pin, 0, value)
|
|
if(value == 0):
|
|
self.off()
|
|
|
|
def setPWMmS(self, length_ms):
|
|
self.setPWM( math.round( length_ms * 4096 ) / ( 1000 / self.freq ) )
|
|
|
|
def setPWMuS(self, length_us):
|
|
self.setPWM( math.round( length_ms * 4096 ) / ( 1000000 / self.freq ) )
|
|
|
|
def run(self, command, speed = 0):
|
|
if not self.MC:
|
|
return
|
|
|
|
|
|
class dw_PWMCONTROL:
|
|
|
|
def __init__(self, addr = 0x61, freq = 100):
|
|
self._i2caddr = addr # default addr on HAT
|
|
self._frequency = freq # default @60Hz PWM freq
|
|
# self.steppers = [ Adafruit_StepperMotor(self, 1), Adafruit_StepperMotor(self, 2) ]
|
|
self._pwm = PWM(addr, debug=False)
|
|
self._pwm.setPWMFreq(self._frequency)
|
|
# Just gonna default to high for now
|
|
|
|
self.servo = [ dw_PWM(self, m, freq) for m in range(6) ]
|
|
self.esc = [ dw_PWM(self, m, freq) for m in range(6, 12) ]
|
|
|
|
def setPin(self, pin, value):
|
|
if (pin < 0) or (pin > 15):
|
|
raise NameError('PWM pin must be between 0 and 15 inclusive')
|
|
if (value != 0) and (value != 1):
|
|
raise NameError('Pin value must be 0 or 1!')
|
|
if (value == 0):
|
|
self._pwm.setPWM(pin, 0, 4096)
|
|
if (value == 1):
|
|
self._pwm.setPWM(pin, 4096, 0)
|
|
|
|
def setAllPin(self, value):
|
|
if (pin < 0) or (pin > 15):
|
|
raise NameError('PWM pin must be between 0 and 15 inclusive')
|
|
if (value != 0) and (value != 1):
|
|
raise NameError('Pin value must be 0 or 1!')
|
|
if (value == 0):
|
|
self._pwm.setAllPWM(0, 4096)
|
|
if (value == 1):
|
|
self._pwm.setAllPWM(4096, 0)
|
|
|
|
def getESC(self, num):
|
|
if (num < 1) or (num > 6):
|
|
raise NameError('ESC must be between 1 and 6 inclusive')
|
|
return self.esc[num-1]
|
|
|
|
def getSERVO(self, num):
|
|
if (num < 1) or (num > 6):
|
|
raise NameError('Servo must be between 1 and 6 inclusive')
|
|
return self.servo[num-1]
|
|
|
|
def setAllPWM(self, value):
|
|
if(value > 0):
|
|
self._pwm.setAllPWM(0, value)
|
|
if(value == 0):
|
|
self.allOff()
|
|
|
|
def setAllPWMmS(self, value):
|
|
if(value > 0):
|
|
self._pwm.setAllPWM(0, value)
|
|
if(value == 0):
|
|
self.allOff()
|
|
|
|
def setAllPWMuS(self, value):
|
|
if(value > 0):
|
|
self._pwm.setAllPWM(0, value)
|
|
if(value == 0):
|
|
self.allOff()
|
|
|
|
def allOff(self):
|
|
this.setAllPin( 0 );
|