diff --git a/Robot-copy-of-eurobot-2020-main b/Robot-copy-of-eurobot-2020-main new file mode 160000 index 0000000..f0dbb17 --- /dev/null +++ b/Robot-copy-of-eurobot-2020-main @@ -0,0 +1 @@ +Subproject commit f0dbb17e830c7d70283bebdbdc246b375ef9c863 diff --git a/Robot.py b/Robot.py index f41571a..bf3351b 100644 --- a/Robot.py +++ b/Robot.py @@ -2,7 +2,7 @@ import RobotCompletCode Controls = RobotCompletCode.MotorizedPlatform() class Robot: def init(self): - Controls.init() + Controls.init() def set_range(self, name:str, mini:int, maxi:int): Controls.set_range(name,mini,maxi) def translation(self, direction, vitesse): @@ -26,15 +26,26 @@ class Robot: print('error') return 'error' def rotation(self, direction, vitesse): + print(direction, vitesse) if direction == 'CW': + print('vitesse') Controls.clockwiseRotation(vitesse) elif direction == 'ACW': + print(vitesse) Controls.antiClockwiseRotation(vitesse) else: print('error') return 'error' - def turbine(self, vitesse): - pass + + def turbine_aspirer(self): + Controls.set_raw_esc('turbine', 230) + + def turbine_stop(self): + Controls.set_raw_esc('turbine', 0) + + def turbine_souffler(self): + Controls.set_raw_esc('turbine', 251) + def tuyau(self, axe, vitesse): #axe : vertical / horizontal pass diff --git a/RobotCompletCode.py b/RobotCompletCode.py index 0d1394b..61cc21b 100644 --- a/RobotCompletCode.py +++ b/RobotCompletCode.py @@ -11,7 +11,8 @@ class MotorizedPlatform: 'frontLeft': 50, 'frontRight': 50, 'backLeft': 50, - 'backRight': 50 + 'backRight': 50, + 'turbine': 50 } escMapping = { 'frontLeft': { @@ -29,16 +30,24 @@ class MotorizedPlatform: 'backRight': { 'slot': 9, 'range': [307, 410] - } + }, + 'turbine': { + 'slot': 7, + 'range':[307,410], + 'stop': 0 + } } 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) + print("drive init start") + v = self.escMapping['turbine'] + for i in range(220, 250, 1): + drive.setPwm(v['slot'], 0, i) + sleep(0.04) + drive.setPwm(v['slot'], 0, 0) + print("drive init stop") #escSlots = [7, 6, 8, 9] def setSpeed(self, values): @@ -59,7 +68,14 @@ class MotorizedPlatform: self.convert_speed_to_esc(value, self.escMapping[esc_name]['range']) ) - # équivalent de la fonction map() de arduino + def set_raw_esc(self, esc_name, value): + drive.setPwm( + self.escMapping[esc_name]['slot'], + 0, + value + ) + + # équivalent de la fonction map() de arduino def mappyt(self, x, inMin, inMax, outMin, outMax): return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin @@ -88,21 +104,19 @@ class MotorizedPlatform: 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 + 'frontLeft': speed, + 'frontRight':speed, + 'backLeft': speed, + 'backRight': speed }) 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) + a = speed + s = 0 self.setSpeed({ 'frontLeft': a, 'frontRight': s, @@ -123,9 +137,18 @@ class MotorizedPlatform: def southEastTranslation(self, speed): self.northWestTranslation(-speed) + + def setTurbine(self, speed): + self.setSpeed({ + 'turbine':0 + }) def stop(self): - self.init() + if drive != None: + for k,v in self.escMapping.items(): + if k == 'turbine': + continue + drive.setPwm(v['slot'], 0, v['range'][0]) """ realescMapping = { 'frontLeft': { diff --git a/ServoDriver.py b/ServoDriver.py new file mode 100644 index 0000000..ec2c9c4 --- /dev/null +++ b/ServoDriver.py @@ -0,0 +1,40 @@ +from Adafruit_PCA9685 import PCA9685 + +class ServoDriver: + def __init__(self): + self.driver = PCA9685() + self.driver.set_pwm_freq(50) + self.servoProfiles = { + 'default': {'range': 180, 'min': 65, 'max': 530}, + # telescope 2023-04-08 + # deployé: 180 deg + # rangé: 40 deg + 'scope': {'range': 180, 'min': 65, 'max': 530}, + } + + def setAngle(self, slot, angle, profileName = 'default'): + profile = None + if type(profileName) is dict: + profile = profileName + if profile == None: + if profileName not in self.servoProfiles: + profileName = 'default' + profile = self.servoProfiles[profileName] + if 'range' not in profile: + profile['range'] = 180 + if angle < 0 or angle > profile['range']: + print('PWMDriver: Invalid range passed ' + angle + ' but range is ' + profile['range']) + if slot > 15 or slot < 0: + print('> ERR!!! Invalid slot passed to setAngle!') + return + pulse = int(self.mappyt(angle, 0, profile['range'], profile['min'], profile['max'])) + #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): + self.driver.set_pwm(slot, off, on) + + def mappyt(self, x, inMin, inMax, outMin, outMax): + return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin + + diff --git a/__pycache__/Robot.cpython-39.pyc b/__pycache__/Robot.cpython-39.pyc index 7dc4979..648878f 100644 Binary files a/__pycache__/Robot.cpython-39.pyc and b/__pycache__/Robot.cpython-39.pyc differ diff --git a/__pycache__/RobotCompletCode.cpython-39.pyc b/__pycache__/RobotCompletCode.cpython-39.pyc index 008c4fb..b9a2ca4 100644 Binary files a/__pycache__/RobotCompletCode.cpython-39.pyc and b/__pycache__/RobotCompletCode.cpython-39.pyc differ diff --git a/__pycache__/ServoDriver.cpython-39.pyc b/__pycache__/ServoDriver.cpython-39.pyc new file mode 100644 index 0000000..f1f902c Binary files /dev/null and b/__pycache__/ServoDriver.cpython-39.pyc differ diff --git a/server.py b/server.py index 9cd9795..4500c88 100644 --- a/server.py +++ b/server.py @@ -1,11 +1,15 @@ import Robot +import traceback import asyncio; import json import websockets; import sys; import Container from PWMDriver import PWMDriver +from ServoDriver import ServoDriver + robot = Robot.Robot() +servo_driver = ServoDriver() #robot = Robot.Robot() #t = robot.Translation #r = robot.Rotation @@ -48,6 +52,7 @@ class commandHandler: return 'initialised' def set_rotation(self, arg): try: + print(arg, self.speed); robot.rotation(arg,self.speed) except Exception as e: print(e) @@ -62,6 +67,27 @@ class commandHandler: def set_range(self, name:str, mini:int, maxi:int): robot.set_range(name, mini, maxi) + def turbine_souffler(self): + robot.turbine_souffler() + return 'Turbine souffler' + + def turbine_stop(self): + robot.turbine_stop() + return 'Turbine stopped' + + def turbine_aspirer(self): + robot.turbine_aspirer() + return 'Turbine aspirer' + + def telescope_deploy(self): + # slot 4 + servo_driver.setAngle(4, 180, profileName='scope') + return '' + + def telescope_zero(self): + servo_driver.setAngle(4, 40, profileName='scope') + # slot 4 + return '' cmd_handler = commandHandler() def commands(data): @@ -88,10 +114,21 @@ def commands(data): res=cmd_handler.init() elif(data['cmd']=='set_speed'): res=cmd_handler.set_speed(data["args"]["speed"]) + elif(data['cmd']=='turbine_aspirer'): + res=cmd_handler.turbine_aspirer() + elif(data['cmd']=='turbine_stop'): + res=cmd_handler.turbine_stop() + elif(data['cmd']=='turbine_souffler'): + res=cmd_handler.turbine_souffler() + elif(data['cmd']=='telescope_deploy'): + res=cmd_handler.telescope_deploy() + elif(data['cmd']=='telescope_zero'): + res=cmd_handler.telescope_zero() else: return 'NOPE!!!' except Exception as e: - return e + traceback.print_exc() + return str(e) return res async def handler(websocket, path): print('new handler created at path'+path) @@ -110,6 +147,9 @@ print('server running at 0.0.0.0 with port 1567') start_server = websockets.serve(handler, "0.0.0.0", 1567) +# init motors +robot.init() + asyncio.get_event_loop().run_until_complete(start_server) asyncio.get_event_loop().run_forever()