From c07003c2e00d53f07cff51774a44e0c7b0964870 Mon Sep 17 00:00:00 2001 From: Matthieu Bessat Date: Sat, 8 Apr 2023 12:30:15 +0000 Subject: [PATCH] Travail du 2023-04-08, ajout du telescope (servo) + controle turbine --- Robot-copy-of-eurobot-2020-main | 1 + Robot.py | 17 ++++-- RobotCompletCode.py | 55 ++++++++++++++------ ServoDriver.py | 40 ++++++++++++++ __pycache__/Robot.cpython-39.pyc | Bin 2199 -> 2580 bytes __pycache__/RobotCompletCode.cpython-39.pyc | Bin 4350 -> 4941 bytes __pycache__/ServoDriver.cpython-39.pyc | Bin 0 -> 1513 bytes server.py | 42 ++++++++++++++- 8 files changed, 135 insertions(+), 20 deletions(-) create mode 160000 Robot-copy-of-eurobot-2020-main create mode 100644 ServoDriver.py create mode 100644 __pycache__/ServoDriver.cpython-39.pyc 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 7dc497929844f60de2a570db17d727b19e9369a3..648878fda778bc095cd3317084ce958ffe3b4035 100644 GIT binary patch delta 719 zcmbVJJxc>Y5Z&3k+$|dOWfBz?KTt70gG7K$qHI( zC6?A!{sbEfu@)5nfrZYPMAS}qxwpfo*LPkc0JO!W_6HrV8D8>X8<0)ZIVQvP>rZ5LHq%Z)DVDXD$at@DTc61Ie5&{~e z$uju>ivf!;kU9B1i!vtf)C8J-!E5+&QQ>?pAvJ6W%tCQ&zu6KA*k(RAZB*`_TvYMLytg^*TIG_=B4 z66GNsC#sABbxs9}0zDQ!*hMeC>}~%*e+JP@(UVUFTC|6>+3oWV^-X1A3Vi1!IYU14 zydU$9N)r=lhGX|<*X66zjQyQ1`VRvaYdHOD5Ryq=Wi?*loODqYHBk_pIbxR;jJr%4 z(tOFJxho2$v>vit;s~Qyj)#_Cb({v2)cw!z-BIo{N0sSO|6$`|4X3}v8FN^HOI8q) zcYk8fDj4($X;p+w>-EAPo4E3 zOl$RlTX_II>tO7)6!G(k_v zMn@?UNeG2*EN7@m(lZbk5qOQ9smAEe6p3jPCn0i1Xx2+LCp2qr-GwYYcgrRobPm$V z-EQOb;ft&#T1G{**i+Wxb8x+za5~Iebc|e!9y;`H{ppnki#)~lJJ3&!TJBQJ} zfMC2iAEWzfw_lm4=o~bQ1WrAZM^FNw-$l@1E?kN^w!;GvdGfAthyCK*jtGo>v&Cfs z^GvqHfx%f|Nt?!Op6h=mllmExDSD>)NavdwNg=feUV}wHD5*eER3H77#48enE;18o zm0xAQxcj%}pKmoM2L{4)iO^~&w;rfD8li1!={{isbG^S^Q$z-Aeg;j*pM~7j3`b&t zZkhz+9ccau2*z!m<{3T>InA4=;~Y}=vZk22;~+<{aBBLNptSftJ76Fx`$+7F9izqe zIXTAxa?hqsBpmX5PK1^lI5j_HTB07Ob;yE-ooEN?# zU+XYyX|#%H97dX?>6NP0kMJx4%1`j-+(@9*uT0oF2k8{Qn%j?|w2)#qU}>}>%ixH_ zEqe;Aa7BZ{4%iJ~@7y!z*nxIX?R-{VCxOa;ENHWj_R1*e~7EvseDvMg)CD6?#ywal50J88mliFRy0FIaB3JdHZ3T3x z+tTwPio-+=ne;{QQ+%kC@Mhy+qfRkOyDoJO!vi=!oQPI#kSRebBwXph+U3aKX5=zQ z5?$3I1Vyg$w(3&7OIPjOV(I8Dq#E0`C>0L@R#cHZ#OZ6&;0v=~H!s8yk8-+!(52Kv zt67g{whyky+9%Cp&pc>n+e48*G!=bO=@pO`L`Abdlr~89W98NkVKllvG zijURW_3A%IAD>>p;2&`4ubOYi;SY_8A@6z!G&OHR86PRrLYEp@cRHnED4830C0 zbClViQ z6lTOM{krUqNIRomFfb4Dn`06D2|A7>II9Ej^Fg2dHF|gW9RX|9kQl5ky&cCy5r8a+ zvyWEw1H$#Dm<=Nahrs=Yz{PxX1We7|(BK%%W3hMlo7o#O@o$g8-q4U3EOOA>^d8#! zP*~GC=<-fZ&cPk2D2ROWjr^riNPHG;%zj5NixxR}-Pg^9c-Zth$2KBeW8T4Qun}P! zhFYVI$iC3k>1EW^h#)fXhEV4>r%@5<2%`EAk%W3rFW#t$Hm5r^pPqo;_UXu?py!o5G>Y`cYxO@fWUS{S)ipxx!b1nKMD3EsyAaV@cH zl;)n0R-c~e)02I=y=S&qk?h1}s{O;guC>znXfsP*t7s3bXje0|w^cf|5SW~Zc1~0a zK18hB)(5V=dK@;rsMEOr2Uu#nO*&aM2{Ve0rZ-5 jGjOr-pp2|2si-9q1Y#7r(7qY*`!N|@2j;jn&J*JQEk_?B literal 4350 zcma)9-*4O26~6b9lqtoIWk-pBwM~<>N!2!X>a5$arfq<9*d7uiO9liWS_rBR?QoVv z<&usQK|vpq*J1C2G1z_cztKN~uLTPDb;bHnY~8+dDJc@kV4=W!52<^}?|kPw=Uz7F z<|+cmUw^nlOJ{}no-Za33l}$WM%U1gAlVij=}F19YO9X&loY;tEiv`TBZKTmJao9L_b7M(}GKyT9p^ow+nE}=g| z@6fyGYjl~epg&9RQ62p`dY?W(zeFF>RrJfWP8;Y~=o)>5ew8*s-t}bRP8f$e==*fH z-Hf+G-8tC=LC3aN-qv9^zU^;}j(;-8|GM0i(bR1{gHH2ZeVUtjzH23Rx7qO%yAyN+w9O|$%f=t0gKbvI zEu7IgGy^rTcGN)pMGWK$RC5D*flF#jGaBfi;zzAJ&0a6)?$m7{^4r_Giof&%H;ZVF zg@z!-&sR4dh8=&S7i{eNI{Z3}H?M7OUPW_#;~rn$3Ol{FkB8ZYfFeF`%*KUXug@X#S6`ZKsnoFMi)QBneeyZfEGrE7dw?k4iFaXD?6#>58S z@w>1h1|_5`7ghgU-WeX&GQRRK92^k`r}5(=jDb8927L|a)iY}#4kbtF2-)_?W!%Sd ztFH74#wK%24kFZudcIE(g+W+)+-&dpkzT_P{RTH2pb4}trrqZmAL4#w1fr_4znaH8 z8aRcSxgTSG8yBpfLupV~pFvHB3W%jaTw%oeEeryGhE;ple;VoY=^caDF?dtB8Kg7p zV6)x+2*XpL^0e%)6awXOEygxF*ydz0+t@2 z6>+3kF-9-WmXXTNPw##Cx9|S-&(Bl!m_)JlGM=wjxiMbWT45K(GfpGZh(iw3P(q%| zA>%~Vmw3Xo&xV?haX;d`W+}PSma|gH+JE}zi#;Dppk?I8^Kh`u92oyu2LmW+Sqz+k zy)0^C08f|2ni$wiqZk<|_0mx2Pdq2+-U+&%)QP9@=XlX5 zotN`)(?PF}6H6tAqM&+SQlKQxT(pSKk@H-lIw5a4WlxA3w(1I1GTiHuzQR|-M0HG3 zc;()HCoMD?z?$>baFgb&lFR??U(O>gisV#2a6WT2e#x^q1lOmAC;M?=ADOin*^AoB z?>m0ieLH{@@O-2azMv|;3^K9 z4agC=G*M+z?S?vjIPq$|j?r0s)d+%^9JPf(qtIW>L(2}&VS5FMDV38XYA#&*DmMkd zH1C1_#8^gTm1OCcegUGan>j?U!$Tq}(364^|o&PI2k)ec*``$6R23uB`{kKwaygsh+- z*(gco_wW9te6Vw~r`%x@UW?mbV_;E?le5ik9Nfx6ZcOl$`V~OD*XL)uJ5oTBd#lxTo+WRq6bWQ z_1bj*4zDewyJnL2!L*(qG035w3t$yZ$-x?nnio(e8;dXsCe8_B^=}#1tMWZb8O(r- z85}RpTrI~+Er4m*%S_I}d@ZrwV3Y*~DM{JOgm}OJ{||eaDN|cuUio9+%^Nt)-@ch|oixYN)!%HnOj*;s&C5DE6Sj%n z_y_)rE~62a!h&Esx%NkQaY8Ev+sxsUw(4t%+b~-8l8~#+CJ>rM&r?5v(1~k zM#GyMZ_FCl_ZXPoEbiUMhh20#h~mVGVx3q)H`YsxoWqqm@(x?POE!C&v~Zh+hPT3I zh12}mqfLSQ4O=F6KVi%0@Kd&UGp28H^Kz4EEdFZI9Czk7x{bzFqa-Q0Ibu?(ek?8W zhmAYqIbD<{7KT=;6%Yi**m)YVnh_19frcf7jVAq?~L7i#Pf{lmiY zkPyDdtUdryq(nv&F%*J+BxNaiPD;+Kp{4966gpS1p->oa|H0n3Uw`w}h~?#R6YUPP z2@QoJ_zGp6(@?6evXyf#hPHB*2aThAwSv`E9YpV$?vq3x77ulLnCOvMipxBTrw5qT zU67h))KDZE`dwTg!6gdjayh{Zxx~3IEZ9j!NZ<{wcHw*V7n41Zblt*nq;YhdKTC|9 zq&Y}@99qUx$vB>7wQ;IZQ6{0SVM@kTZE>boC3RFDO`@?*o}1N*>)zMJIL(p@_hVN{ zHhxKZ4L<*Vvh!OpNp{L~=P1#|)1uz}e0TRVkS}+Z&U>dkF@6-Kd0I!24Zo`GoA5=? z>e!wCPW#Ko3q#FDBQ{?Q7t9J~dsTmfQFLb4)X15r#gufvk!N;eD>|_B-A3X9twW(d zRpPAW8@UC0Wi>Xm50!+ab&neK|A4Ar8|am_MF+MnYpI;Z+92g_&|JGgGqj-*$~&;D zKd{0_{|P&v_1uEmZs-DDiYq`I(3O^Uw&>ECi8BZFxECJ#HqND-dxhOWdrs})IGS;w z84#zz{x6T_VEHciDL;&}R0VA(gEFqFL7TYyGeyu{ii2xsCzOlS{2M3{Ou@-v#8gn!$icBYp zCWV@2%=e<``83YvJ9?c<`jE*-OgO<((zc!4NzI4kSuvu}v~K4eU$)u-{-3oy6_524 bFaP>qZ9kiK`%Te|e{WvSeSQY*VS3j82&hrn literal 0 HcmV?d00001 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()