From ef00cd438de6a2d5c689cf5c5681a5688271a443 Mon Sep 17 00:00:00 2001 From: GZod01 Date: Sat, 25 Mar 2023 10:33:33 +0100 Subject: [PATCH] initial commit --- Container.py | 16 +++ PWMDriver.py | 38 ++++++ Robot.py | 48 ++++++++ RobotCompletCode.py | 130 ++++++++++++++++++++ __pycache__/Container.cpython-35.pyc | Bin 0 -> 846 bytes __pycache__/Driver.cpython-35.pyc | Bin 0 -> 1381 bytes __pycache__/PWMDriver.cpython-35.pyc | Bin 0 -> 1729 bytes __pycache__/Robot.cpython-35.pyc | Bin 0 -> 1900 bytes __pycache__/RobotCompletCode.cpython-35.pyc | Bin 0 -> 4235 bytes beta/Driver.py | 28 +++++ beta/oldRobot.py | 55 +++++++++ beta/robottest.py | 17 +++ oldserver.py | 79 ++++++++++++ pwm_manual.py | 4 + server.py | 109 ++++++++++++++++ test.py | 8 ++ testofrobot.py | 19 +++ 17 files changed, 551 insertions(+) create mode 100644 Container.py create mode 100644 PWMDriver.py create mode 100644 Robot.py create mode 100644 RobotCompletCode.py create mode 100644 __pycache__/Container.cpython-35.pyc create mode 100644 __pycache__/Driver.cpython-35.pyc create mode 100644 __pycache__/PWMDriver.cpython-35.pyc create mode 100644 __pycache__/Robot.cpython-35.pyc create mode 100644 __pycache__/RobotCompletCode.cpython-35.pyc create mode 100644 beta/Driver.py create mode 100644 beta/oldRobot.py create mode 100644 beta/robottest.py create mode 100644 oldserver.py create mode 100644 pwm_manual.py create mode 100644 server.py create mode 100644 test.py create mode 100644 testofrobot.py diff --git a/Container.py b/Container.py new file mode 100644 index 0000000..74accf1 --- /dev/null +++ b/Container.py @@ -0,0 +1,16 @@ +class Container: + instances = {} + + def set(self, name, instance): + self.instances[name] = instance + + def get(self, name): + if name not in self.instances: + return None + return self.instances[name] + + def count(self): + return len(self.instances) + + def getInstances(self): + return self.instances diff --git a/PWMDriver.py b/PWMDriver.py new file mode 100644 index 0000000..b01ef60 --- /dev/null +++ b/PWMDriver.py @@ -0,0 +1,38 @@ +from Adafruit_PCA9685 import PCA9685 + +class PWMDriver: + def __init__(self): + self.driver = PCA9685() + self.driver.set_pwm_freq(50) + self.servoProfiles = { + '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 + } + + 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/Robot.py b/Robot.py new file mode 100644 index 0000000..85763fe --- /dev/null +++ b/Robot.py @@ -0,0 +1,48 @@ +import RobotCompletCode +Controls = RobotCompletCode.MotorizedPlatform() +class Robot: + def init(self): + Controls.init() + def translation(self, direction, vitesse): + if direction == 'N': + Controls.northTranslation(vitesse) + elif direction == 'E': + Controls.eastTranslation(vitesse) + elif direction == 'W': + Controls.westTranslation(vitesse) + elif direction == 'S': + Controls.southTranslation(vitesse) + elif direction =='SE': + Controls.southEastTranslation(vitesse) + elif direction == 'SW': + Controls.southWestTranslation(vitesse) + elif direction == 'NE': + Controls.northEastTranslation(vitesse) + elif direction =='NW': + Controls.northWestTranslation(vitesse) + else: + print('error') + return 'error' + def rotation(self, direction, vitesse): + if direction == 'CW': + Controls.clockwiseRotation(vitesse) + elif direction == 'ACW': + Controls.antiClockwiseRotation(vitesse) + else: + print('error') + return 'error' + def stop(self): + Controls.stop() + def easy(self, mode, direction, vitesse): + if mode=='R': + self.rotation(direction, vitesse) + elif mode=='T': + self.translation(diretion, vitesse) + else: + print('error') + return 'error' + def easyjson(self, jsondata): + # Easy but from JSON datas + self.easy(jsondata["mode"], jsondata["direction"], jsondata["vitesse"]) + + diff --git a/RobotCompletCode.py b/RobotCompletCode.py new file mode 100644 index 0000000..f24b2d3 --- /dev/null +++ b/RobotCompletCode.py @@ -0,0 +1,130 @@ +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 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): + 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.setSpeed({ + 'frontLeft': 0, + 'frontRight': 0, + 'backLeft': 0, + 'backRight': 0 + }) diff --git a/__pycache__/Container.cpython-35.pyc b/__pycache__/Container.cpython-35.pyc new file mode 100644 index 0000000000000000000000000000000000000000..aecb1eb156655a2c58b495f562d75599aa8d8a97 GIT binary patch literal 846 zcmZuvu}RZth2noc((yaqgNemR)glDHhAU4?f4t~ii z1OLFpxwhH{39I+`*tgf`TsvFcZs+adGdmJQA5{19Qb0H--oF6h=0p20E%cN2z8@?CP2vtpa^G-LdvlW9dA&MU(@j>)Qcc<{_|j44UL*!*{~STl#LdNchXJN zf9>{f%9-llPx=o^mzQNV_%j&n0si)v^85FXRvJ}>tjfKTcB+%TLW)0`eLkmkQuT{mwuwxQE5&qCeHp3&j!ShnTi$w(s zxFA6IZ1G0D zn$+e>s{WVrB(hPO@*Ae9?WF0f%;!_)ahm?0kEcH4+R_ou(`|+|2JWPGrLJ4{s@XY* e=>THU3}P8KV(Iqr6}kDhLK_`;`G2k&#qr7l3;jDp7zu97$%JM^OL z!W*#T4R{Nlfu$F0SmhO1@t<*=gs67r%s&^O`R1IN>@*s+pNGGLZx+!XT6t{fwOXMy;LWIPg z;F!OKu-Kb#vqQLI?T~Uwm5hccld2e9B2^_-Ga8*6r0PajNHxH4C##-*IC-a|FQE>^ zd~L3I`D2u14<;p=5>1GtV0$j;+;B{>J3yUzR%h16JkQqrT~<=Yp9^AIAZ*qyjEacFlNIDp!HfX2?5Cpu;o zuar}WE8mTVC(+PU-7CKok5g`%x_h<8m6ZnF(das@W0?Z2xkSqc1H+6NTrUE(!(=8< z;LDHLB1(2g^O1-SU_D_rkFfE^^adMF=Q221#bhi@g1hK1Fjg{F3?q-~T=Q(J)6KHIxmcYnB^_-#+^q`t}X| zrn28s_8q0a#SH~rrQc?KWU7LYh|tIF!D1hUmR1ms{Apa2ru37OaPb=}^+}X04yu)Z z5p^|OlNm>WFBe}!8~uxhU32&>N_^KC#s0|xF)8|k)w&GNqD`z89_zk?uVY9^Drj3X&AKnmSEdpKq#---Q^TlZC=g9F zfS#@lhH|bGfGY%z>m=XgE%b8`g=nmi{Dw7*vPNTzveOLo@IPmc?qr=V` zJ)A2Jk5TXsgmtCuj}4MPQURcVm_|L2q_;fkF@ffwu0^oQeM3XlAdk^Ar24 zGD7P2Rz5{)gBl0PNgVD< z#zHnAoO)>#mswaEK{PBR$0^NBMoCfXENT-!%PtGapqJK?TTV@OcyDlg*95oY&~0WF z%Le`=bgkvaHM4094l>;;(PhW_suIqZ+YlsbqKnZITaazc4RKH00l^x=5cjp46k9p3 z*~Kj5q1bf?3kFAh82}+;500&_TnrpdbtBmjZx44O??SkGIvTlJn!H+V4G9m=CvU+f z=XZ_7tg}YwL3KD}2>Ibz$56H0!=fr-2e5+}JGYt68tvT0rg!9sfl9WzP;pV3QSu~8 z)Z|~3wRBqWR?Ys!E0xJU;=hgYnzAa~29CvVZvsp_uWWeUBn_r9uUnq?eCo&5iQGmI z$<@j?Sa94MxfFT~oaD}UKJXf?OCTe{K?9Nq041pY`)@ugV)T&PczI(}WWvi`6t M=Ml4B_0ZA&16HD5ivR!s literal 0 HcmV?d00001 diff --git a/__pycache__/Robot.cpython-35.pyc b/__pycache__/Robot.cpython-35.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0c93564d7d484aea87498726b5db0ab5f4d8395d GIT binary patch literal 1900 zcmb7FK~EDw6n-zRUysfODUN z@*0k`ic^R`0bPIqAOhGEaEh{o=b?nqf%gbIa4I2iNU!0$Kk*RMJ2VeIiNYF=^fgXs zBoJg4jo=A1gEr_i{#=$_2y+nTSw^7%p~rF_!XktvmT_|e!ZOPqgp=rb%H}rJwlW=o zUg)EhMh!iM?`n%-tfLNH(iYyvCQ8$Yt0$WZq%vVfkx*f!cX0wy7L|VuSrW%HSv8$I ze|b^8KzmmgRhF#Fiwcv)?MKyzU6>`yty@`Z2F}x3Q2y_R|1eIYO=Ysg z%(oL0*T%Q?B%R6_($sx=nkEYV950?evvL#nQ*tyUg@T{Tf*&aTS8MZqv73$R`+^fS z=3LLBwv2p1{hZ>qbTB#tpaW_yHaeNz2@Ik>H>`#6JMY7CrM)NAe0?Y`$ifOTF z#nyo4Yv@E*a0)!CD=MpuOE;@Fx2MABYDQgblOw9GA)B*UVzb2N*~3H%9S#|_Xr&_N z`0k)^785^DixjK`UL06Fb+AC_<#g-%I;xH?fdnt$hA;Vwfwz~cBy5}Z7~|F>-vr)A z?TmIgNb&%x`$g-uT1mSXwOY2+YGJ$wU6RYK)|-QNH$(W@Wu7!MM#Wn37 zLQSz!E}5CKD9iF(*~8gm#CL-Vms?Hs`rXLXkmg|Kl`=}l$5FU}&(Mz2JuWLgaG4M5 RJ;|=SO8-15RS!B6e*nqydo%z5 literal 0 HcmV?d00001 diff --git a/__pycache__/RobotCompletCode.cpython-35.pyc b/__pycache__/RobotCompletCode.cpython-35.pyc new file mode 100644 index 0000000000000000000000000000000000000000..4164b19eb6d51540ba6f6bf13cb03d4fe7c3eb7c GIT binary patch literal 4235 zcmb_f-EI>{6h5=Nb{xmX4&)co0)Y}1ngjwjl~9Eg5S2(GA{yeE?N<@Frna76y z6eVP#@vo>s)KpYaq_}QT!=k1|6^oQ-(J@iQrWe{K?a&u|cjy=&9nu-vaI+_vg!M@D8>DZF zULrkD`j+Te{Wj?zL?0)8hxCHzKysI~EBY1E_ekFt9jJaJy(s!5=_S(3q66UrF!nGW zUkjqZ_#IEbYSf~wz%=90Efch&m)=%%vI3IgywqR&+wY?|_pVmo;ZN}>Ye}OMHiGDc zNsTk6*4p;`N8r`}1LCBJ){`8IQHVO4VFCZcW6VZ-MW|a;e(-F}JMIR*M zp1r+AzghH2(I<=heX!{Wq$`?DV&&K-wL3?f7{B2<>n4xs3@;=wTqn+G4#7C@h4r;s zyY08ODsXqdX24E?ZoQipJb*n`tQZ%_d(NJZu`soo(bLs(aM9B zl|_h0%dfe*8Z_Gt4=?R4wGU&*Z~4(}3=FdnqzY=z>P!u|cZs{zE$)KH=@#+BLsSm# z(Gd(M-fGX@&WYi$9VUxMVEB>Edrs{ZyB4c;*D{weG%muLDvYd#ZO_xLVL4o7p z{+Xi;9XT{Jff(ZIHB_cIT(vICyV=C?>Y$SGTWfwxUbTZtHrR`}D(~7s3TvEgVpAPP zPq0iVajiNtgFur`eN3=zf^Y!}m|$lEC$hX813<`h@QZz)p<|V7aNgS|Ac0O;b1h_@ zV6UakJl9U6NA^Hvw7uV~N={0lHaF1uDM~kf;N3fepobCI3x2khlbV>odx!BOjfn|k z$W5?h4A9vihb7Jzn2>}|X5Dc9GaQYE9btGXR~cpt8=AMSxx=DAGUAd~3!^tMDMY^Z zgVuBO>1G^`lFrRRD^er+3}7R~0L~zZ0Svo`6WK+@R7sVQ`I5iRt-;6{7~3m%d^W*SNe2w;BTdP$0dOyFV+hR*VlU6)(W(7f_5q!>0awa5oZ1!)H7NhKLlOf244e3MH z+bb?6N~i%B$xSoK$p zUDw^|I9HWhTD2PItJP+p_ZlAbLbbZPS8MS7ty--VdpEEut~qk~s}#_-f!{^pOFxXv zEEbiNXs)xy_gpj2;sy&o3Jk}aoId6rYn+)3U#Sgm1H-=!43lIYvEaMmnN2Hp@@3wf zW|#08Y6U9^n}Tg4fJ#=0.1 and abs(vector_args[1])<=0.1: + v((vector_args[0]*10)) + if abs(vector_args[1])>=0.1 and abs(vector_args[0])<=0.1: + sd((vector_args[1]*10)) + if abs(vector_args[1])>=0.1 and abs(vector_args[0])>=0.1: + if vector_args[0]>0: + d((vector_args[0]+vector_args[1]//2)*10,True) + elif vector_args[0]<0: + d((vector_args[0]+vector_args[1]//2)*10,False) + if vector_type == 'rot': + r(vector_args[1]*10) + return f"{vector_type=},{vector_args=}" + """ + def set_direction(self,arg): + try: + return robot.translation(arg) + except Exception as e: + return 'error' + +cmd_handler = commandHandler() +async def handler(websocket, path): + print('new handler created with '+websocket+' at path'+path) + while True: + try : + data = await websocket.recv() + except websockets.exceptions.ConnectionClosedOK: + break + print(data) + if not data: + data = { + 'cmd':'ping', + 'args':{} + } + else: + data = json.loads(data) + if data['cmd'] =='STOP': + sys.exit() + try: + callable = getattr(cmd_handler, data['cmd']) + except AttributeError: + await websocket.send(json.dumps({"type":"err","message":"Invalid Command"})) + continue + + try: + res = callable(**data['args']) + except TypeError: + await websocket.send(json.dumps({"type":"err","message":"Invalid Args"})) + + continue + await websocket.send(json.dumps({"type":"success","message":res})) +print('server running at 0.0.0.0 with port 1567') +start_server = websockets.serve(handler, "0.0.0.0", 1567) + + +asyncio.get_event_loop().run_until_complete(start_server) + +asyncio.get_event_loop().run_forever() diff --git a/pwm_manual.py b/pwm_manual.py new file mode 100644 index 0000000..599f5b3 --- /dev/null +++ b/pwm_manual.py @@ -0,0 +1,4 @@ +from RobotCompletCode import MotorizedPlatform + +platform = MotorizedPlatform() + diff --git a/server.py b/server.py new file mode 100644 index 0000000..5ae9736 --- /dev/null +++ b/server.py @@ -0,0 +1,109 @@ +import Robot +import asyncio; +import json +import websockets; +import sys; +import Container +from PWMDriver import PWMDriver +robot = Robot.Robot() +#robot = Robot.Robot() +#t = robot.Translation +#r = robot.Rotation +#sd = t.sd +#v = t.droit +#d = t.trv +class commandHandler: + def ping(self): + return "Pong" + """def set_vector(self,vector_type:str,vector_args): + print(f"{vector_type=}, {vector_args=}") + for i in range(len(vector_args)): + vector_args[i]= float(vector_args[i]) + if vector_type =='trans': + if abs(vector_args[0])>=0.1 and abs(vector_args[1])<=0.1: + v((vector_args[0]*10)) + if abs(vector_args[1])>=0.1 and abs(vector_args[0])<=0.1: + sd((vector_args[1]*10)) + if abs(vector_args[1])>=0.1 and abs(vector_args[0])>=0.1: + if vector_args[0]>0: + d((vector_args[0]+vector_args[1]//2)*10,True) + elif vector_args[0]<0: + d((vector_args[0]+vector_args[1]//2)*10,False) + if vector_type == 'rot': + r(vector_args[1]*10) + return f"{vector_type=},{vector_args=}" + """ + speed=0 + def set_direction(self,arg): + try: + return robot.translation(arg,self.speed) + except Exception as e: + return 'error' + def stop(self): + robot.stop() + return 'stopped' + def init(): + robot.init() + return 'initialised' + def set_rotation(self, arg): + try: + robot.rotation(arg,self.speed) + except Exception as e: + return 'error' + def set_speed(self,v): + if v>=50: + v = 50 + else: + v=v + self.speed=v + return 'speed set to '+str(v) + + +cmd_handler = commandHandler() +def commands(data): + print('commands:') + print(data) + if not data: + data = { + } + else: + data = json.loads(data) + if 'STOP' in data: + if data['STOP']: + sys.exit() + try: + if(data['cmd']=='set_direction'): + res= cmd_handler.set_direction(data["args"]['direction']) + elif(data['cmd']=='set_rotation'): + res= cmd_handler.set_rotation(data["args"]['rotation']) + elif(data['cmd']=='stop_robot'): + res=cmd_handler.stop() + elif(data['cmd']=='init'): + res=cmd_handler.init() + elif(data['cmd']=='set_speed'): + res=cmd_handler.set_speed(data["args"]["speed"]) + else: + return 'NOPE!!!' + except Exception as e: + return e + return res +async def handler(websocket, path): + print('new handler created at path'+path) + while True: + try : + data = await websocket.recv() + except websockets.exceptions.ConnectionClosedOK: + break + try: + res= commands(data) + except Exception as e: + await websocket.send(json.dumps({"type":"error","message":e})) + print(e) + await websocket.send(json.dumps({"type":"success","message":res})) +print('server running at 0.0.0.0 with port 1567') +start_server = websockets.serve(handler, "0.0.0.0", 1567) + + +asyncio.get_event_loop().run_until_complete(start_server) + +asyncio.get_event_loop().run_forever() diff --git a/test.py b/test.py new file mode 100644 index 0000000..804d61e --- /dev/null +++ b/test.py @@ -0,0 +1,8 @@ +import Robot +import time +import PWMDriver +Robot.Robot().init() +e = Robot.Robot().easy +e('R','CW',30) +time.sleep(5) +Robot.Robot().stop() diff --git a/testofrobot.py b/testofrobot.py new file mode 100644 index 0000000..8c979fb --- /dev/null +++ b/testofrobot.py @@ -0,0 +1,19 @@ +import Robot +import Container +robot = Robot.MotorizedPlatform +def translation(arg): + try: + callable = getattr(robot,arg) + possible=True + except AttributeError: + print('ERROR') + possible=False + if possible: + callable() +x="" +while x!="q": + x=input('command : ') + if x=="help": + print(robot.__dict__) + else: + translation(x)