2023-03-25 09:33:33 +00:00
|
|
|
from Adafruit_PCA9685 import PCA9685
|
|
|
|
|
|
|
|
|
|
class PWMDriver:
|
2023-05-27 09:30:02 +00:00
|
|
|
def __init__(self):
|
|
|
|
|
self.driver = PCA9685()
|
2023-03-25 09:33:33 +00:00
|
|
|
self.driver.set_pwm_freq(50)
|
|
|
|
|
self.servoProfiles = {
|
2023-05-27 09:30:02 +00:00
|
|
|
'default': {'range': 180, 'min': 65, 'max': 530},
|
|
|
|
|
'lidar': {'range': 180, 'min': 75, 'max': 510},
|
|
|
|
|
'china': {'range': 180, 'min': 75, 'max': 510},
|
|
|
|
|
'flag': {'range': 180, 'min': 100, 'max': 480},
|
|
|
|
|
'rev': {'range': 270, 'min': 95, 'max': 600}, # 552
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#définir la fréquence du signal de sortie à 50hz
|
|
|
|
|
def ini_esc(slot):
|
|
|
|
|
slef.driver.set_pwm(slot,0,307) #307 est le signal neutre sous 50 Hz (1.5 / 20 x 4096 = 307)
|
|
|
|
|
time.sleep(1)
|
|
|
|
|
|
|
|
|
|
def initialiser_esc():
|
|
|
|
|
HZ = 50 #fréquence du signal vers les ESC
|
|
|
|
|
self.driver.set_pwm_freq(HZ)
|
|
|
|
|
LESC = (1, 13, 8, 9) #liste des ESC (avant gauche, avant droit, arrière gauche, arrière droit)
|
|
|
|
|
ini_esc(LESC[0])
|
|
|
|
|
ini_esc(LESC[1])
|
|
|
|
|
ini_esc(LESC[2])
|
|
|
|
|
ini_esc(LESC[3])
|
|
|
|
|
|
2023-03-25 09:33:33 +00:00
|
|
|
def setAngle(self, slot, angle, profileName = 'default'):
|
2023-05-27 09:30:02 +00:00
|
|
|
profile = None
|
2023-03-25 09:33:33 +00:00
|
|
|
if type(profileName) is dict:
|
2023-05-27 09:30:02 +00:00
|
|
|
profile = profileName
|
2023-03-25 09:33:33 +00:00
|
|
|
if profile == None:
|
2023-05-27 09:30:02 +00:00
|
|
|
if profileName not in self.servoProfiles:
|
|
|
|
|
profileName = 'default'
|
2023-03-25 09:33:33 +00:00
|
|
|
profile = self.servoProfiles[profileName]
|
|
|
|
|
if 'range' not in profile:
|
2023-05-27 09:30:02 +00:00
|
|
|
profile['range'] = 180
|
2023-03-25 09:33:33 +00:00
|
|
|
if angle < 0 or angle > profile['range']:
|
2023-05-27 09:30:02 +00:00
|
|
|
print('PWMDriver: Invalid range passed ' + angle + ' but range is ' + profile['range'])
|
2023-03-25 09:33:33 +00:00
|
|
|
if slot > 15 or slot < 0:
|
2023-05-27 09:30:02 +00:00
|
|
|
print('> ERR!!! Invalid slot passed to setAngle!')
|
2023-03-25 09:33:33 +00:00
|
|
|
return
|
2023-05-27 09:30:02 +00:00
|
|
|
pulse = int(self.mappyt(angle, 0, profile['range'], profile['min'], profile['max']))
|
2023-03-25 09:33:33 +00:00
|
|
|
#print('setting slot', slot, 'to angle', angle, 'with profile', profileName, profile, pulse)
|
|
|
|
|
self.driver.set_pwm(slot, 0, pulse)
|
|
|
|
|
|
|
|
|
|
def setPwm(self, slot, off, on):
|
2023-05-27 09:30:02 +00:00
|
|
|
#self.driver.set_pwm(slot, off, on)
|
|
|
|
|
self.driver.set_pwm(slot, off,on)
|
2023-03-25 09:33:33 +00:00
|
|
|
|
|
|
|
|
def mappyt(self, x, inMin, inMax, outMin, outMax):
|
2023-05-27 09:30:02 +00:00
|
|
|
return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin
|
