182 lines
5.3 KiB
Python
182 lines
5.3 KiB
Python
from time import sleep
|
|
|
|
'''
|
|
Abstration of motorized platform
|
|
'''
|
|
from PWMDriver import PWMDriver
|
|
drive = PWMDriver()
|
|
|
|
class MotorizedPlatform:
|
|
esc_speed_map = {
|
|
'frontLeft': 50,
|
|
'frontRight': 50,
|
|
'backLeft': 50,
|
|
'backRight': 50
|
|
}
|
|
escMapping = {
|
|
'frontLeft': {
|
|
'slot': 12, # 7
|
|
'range': [307, 410]
|
|
},
|
|
'frontRight': {
|
|
'slot': 13,
|
|
'range': [307, 410]
|
|
},
|
|
'backLeft': {
|
|
'slot': 8,
|
|
'range': [307, 410]
|
|
},
|
|
'backRight': {
|
|
'slot': 9,
|
|
'range': [307, 410]
|
|
}
|
|
}
|
|
escSlotsList= [12,13,8,9]
|
|
def set_range(self, name:str, mini:int, maxi:int):
|
|
self.escMapping[name]['range'] = [mini,maxi]
|
|
def init(self):
|
|
if drive != None:
|
|
for k,v in self.escMapping.items():
|
|
# 307 est le signal neutre sous 50 Hz (1.5 / 20 x 4096 = 307)
|
|
drive.setPwm(v['slot'], 0, 307)
|
|
|
|
#escSlots = [7, 6, 8, 9]
|
|
def setSpeed(self, values):
|
|
# print('values: '+str(values))
|
|
for k,v in values.items():
|
|
self.set_esc_speed(k, v)
|
|
|
|
def set_all(self, value):
|
|
for k in self.escMapping.keys():
|
|
self.set_esc_speed(k, value)
|
|
|
|
def set_esc_speed(self, esc_name, value):
|
|
if value < 0:
|
|
value = value * 1.185
|
|
drive.setPwm(
|
|
self.escMapping[esc_name]['slot'],
|
|
0,
|
|
self.convert_speed_to_esc(value, self.escMapping[esc_name]['range'])
|
|
)
|
|
|
|
# équivalent de la fonction map() de arduino
|
|
def mappyt(self, x, inMin, inMax, outMin, outMax):
|
|
return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin
|
|
|
|
# fonction esc pour une vitesse de moteur de -100 à 100()
|
|
def convert_speed_to_esc(self, speed, pwm_range=[307, 410]):
|
|
return round(
|
|
self.mappyt(speed, 0, 100, pwm_range[0], pwm_range[1])
|
|
)
|
|
|
|
def eastTranslation(self, speed):
|
|
self.setSpeed({
|
|
'frontLeft': -speed, 'frontRight': -speed,
|
|
'backLeft': speed, 'backRight': speed
|
|
})
|
|
|
|
def southTranslation(self, speed):
|
|
self.northTranslation(-speed)
|
|
|
|
def northTranslation(self, speed):
|
|
self.setSpeed({
|
|
'frontLeft': -speed, 'frontRight': speed,
|
|
'backLeft': -speed, 'backRight': speed
|
|
})
|
|
|
|
def westTranslation(self, speed):
|
|
self.eastTranslation(- speed)
|
|
|
|
def clockwiseRotation(self, speed):
|
|
a = self.convert_speed_to_esc(speed)
|
|
r = self.convert_speed_to_esc(-speed)
|
|
self.setSpeed({
|
|
'frontLeft': a,
|
|
'frontRight': a,
|
|
'backLeft': r,
|
|
'backRight': r
|
|
})
|
|
|
|
def antiClockwiseRotation(self, speed):
|
|
self.clockwiseRotation(-speed)
|
|
|
|
def northEastTranslation(self, speed):
|
|
a = self.convert_speed_to_esc(speed)
|
|
s = self.convert_speed_to_esc(0)
|
|
self.setSpeed({
|
|
'frontLeft': a,
|
|
'frontRight': s,
|
|
'backLeft': s,
|
|
'backRight': a
|
|
})
|
|
|
|
def southWestTranslation(self, speed):
|
|
self.northEastTranslation(-speed)
|
|
|
|
def northWestTranslation(self, speed):
|
|
self.setSpeed({
|
|
'frontLeft': 0,
|
|
'frontRight': speed,
|
|
'backLeft': 0,
|
|
'backRight': speed
|
|
})
|
|
|
|
def southEastTranslation(self, speed):
|
|
self.northWestTranslation(-speed)
|
|
|
|
def stop(self):
|
|
self.init()
|
|
"""
|
|
realescMapping = {
|
|
'frontLeft': {
|
|
'slot': 12, # 7
|
|
'range': [307, 410]
|
|
},
|
|
'frontRight': {
|
|
'slot': 13,
|
|
'range': [307, 410]
|
|
},
|
|
'backLeft': {
|
|
'slot': 8,
|
|
'range': [307, 410]
|
|
},
|
|
'backRight': {
|
|
'slot': 9,
|
|
'range': [307, 410]
|
|
}
|
|
}
|
|
motornamelist = ['frontLeft','frontRight','backLeft','backRight']
|
|
for i in range(len(motornamelist)):
|
|
esc_name = motornamelist[i]
|
|
drive.setPwm(
|
|
realescMapping[esc_name]['slot'],
|
|
0,
|
|
self.convert_speed_to_esc(0, realescMapping[esc_name]['range'])
|
|
)
|
|
"""
|
|
|
|
def oldstop(self):
|
|
self.escMapping = {
|
|
'frontLeft': {
|
|
'slot': 12, # 7
|
|
'range': [307, 410]
|
|
},
|
|
'frontRight': {
|
|
'slot': 13,
|
|
'range': [307, 410]
|
|
},
|
|
'backLeft': {
|
|
'slot': 8,
|
|
'range': [307, 410]
|
|
},
|
|
'backRight': {
|
|
'slot': 9,
|
|
'range': [307, 410]
|
|
}
|
|
}
|
|
self.setSpeed({
|
|
'frontLeft': 0,
|
|
'frontRight': 0,
|
|
'backLeft': 0,
|
|
'backRight': 0
|
|
})
|