From 1828a774bb5b06668073dda87a0c7453d807d1e6 Mon Sep 17 00:00:00 2001 From: zhanli <719901725@qq.com> Date: Tue, 23 Jan 2024 15:09:47 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0UPbot=E5=8F=AF=E8=A7=86?= =?UTF-8?q?=E5=8C=96=E9=85=8D=E7=BD=AE=E5=B7=A5=E5=85=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../__pycache__/dataholder.cpython-37.pyc | Bin 0 -> 7735 bytes .../__pycache__/params.cpython-37.pyc | Bin 0 -> 1463 bytes .../UPbot-Tools/__pycache__/pb.cpython-37.pyc | Bin 0 -> 17891 bytes .../__pycache__/transport.cpython-37.pyc | Bin 0 -> 5784 bytes Hardware/UPbot-Tools/app.py | 434 +++++ Hardware/UPbot-Tools/dataholder.py | 250 +++ Hardware/UPbot-Tools/main.py | 115 ++ Hardware/UPbot-Tools/params.py | 82 + Hardware/UPbot-Tools/pb.py | 749 ++++++++ Hardware/UPbot-Tools/pb.ui | 1613 +++++++++++++++++ Hardware/UPbot-Tools/pypibot/__init__.py | 17 + .../__pycache__/__init__.cpython-37.pyc | Bin 0 -> 655 bytes .../__pycache__/assistant.cpython-37.pyc | Bin 0 -> 7136 bytes .../pypibot/__pycache__/err.cpython-37.pyc | Bin 0 -> 1634 bytes .../pypibot/__pycache__/log.cpython-37.pyc | Bin 0 -> 8181 bytes Hardware/UPbot-Tools/pypibot/assistant.py | 234 +++ Hardware/UPbot-Tools/pypibot/configer.py | 56 + Hardware/UPbot-Tools/pypibot/daemon.py | 140 ++ Hardware/UPbot-Tools/pypibot/err.py | 58 + Hardware/UPbot-Tools/pypibot/log.py | 259 +++ Hardware/UPbot-Tools/pypibot/mapconvert.py | 35 + Hardware/UPbot-Tools/pypibot/proxy.py | 38 + Hardware/UPbot-Tools/set_default_params.py | 90 + Hardware/UPbot-Tools/test_motors.py | 122 ++ Hardware/UPbot-Tools/transport.py | 203 +++ 25 files changed, 4495 insertions(+) create mode 100644 Hardware/UPbot-Tools/__pycache__/dataholder.cpython-37.pyc create mode 100644 Hardware/UPbot-Tools/__pycache__/params.cpython-37.pyc create mode 100644 Hardware/UPbot-Tools/__pycache__/pb.cpython-37.pyc create mode 100644 Hardware/UPbot-Tools/__pycache__/transport.cpython-37.pyc create mode 100644 Hardware/UPbot-Tools/app.py create mode 100644 Hardware/UPbot-Tools/dataholder.py create mode 100644 Hardware/UPbot-Tools/main.py create mode 100644 Hardware/UPbot-Tools/params.py create mode 100644 Hardware/UPbot-Tools/pb.py create mode 100644 Hardware/UPbot-Tools/pb.ui create mode 100644 Hardware/UPbot-Tools/pypibot/__init__.py create mode 100644 Hardware/UPbot-Tools/pypibot/__pycache__/__init__.cpython-37.pyc create mode 100644 Hardware/UPbot-Tools/pypibot/__pycache__/assistant.cpython-37.pyc create mode 100644 Hardware/UPbot-Tools/pypibot/__pycache__/err.cpython-37.pyc create mode 100644 Hardware/UPbot-Tools/pypibot/__pycache__/log.cpython-37.pyc create mode 100644 Hardware/UPbot-Tools/pypibot/assistant.py create mode 100644 Hardware/UPbot-Tools/pypibot/configer.py create mode 100644 Hardware/UPbot-Tools/pypibot/daemon.py create mode 100644 Hardware/UPbot-Tools/pypibot/err.py create mode 100644 Hardware/UPbot-Tools/pypibot/log.py create mode 100644 Hardware/UPbot-Tools/pypibot/mapconvert.py create mode 100644 Hardware/UPbot-Tools/pypibot/proxy.py create mode 100644 Hardware/UPbot-Tools/set_default_params.py create mode 100644 Hardware/UPbot-Tools/test_motors.py create mode 100644 Hardware/UPbot-Tools/transport.py diff --git a/Hardware/UPbot-Tools/__pycache__/dataholder.cpython-37.pyc b/Hardware/UPbot-Tools/__pycache__/dataholder.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..56c3d1722428b7da8a2ec50e1efc970b24090732 GIT binary patch literal 7735 zcmcIp%X1t@8Sk0d$Lv$9)moMxc~l;dg-QxPk~qNxSr5yCXl3NJO%{jAT0ORRv(hR( zBimA~xhO}9%7H4lK#?jAq>2mw090|Kpo$9znBqo3l@6S8;}U+qo_%Yjm?fT7|E9n0 zp6;*v`}+I7Zp~#fNrC4t>5tuCpAzC9BsxzXnOP+7f+2(y(rAc^kY>eL64H{94~2~E znH3XMRK`HXKv|&T(gtO_)}B>~fKJFH=p<^Rpi(joDh(uwRH>Kal+_ZjAyBYmH?q>CS#+}gb6K-x# z)Do4G(zqsy&+KzOOZ!g&*O~1+Sk!QN&m-fCiXlbClt#tE5=W#}iONVNCZmVn8t zb^ZM*AYrf-fE_w>Wh#9*l^~&^f@k*!Rx|R=G zTS!ze&Cb`5*(vzq7lt%^vt{fVADfTNn_@F*Kp_m6+-ss`-ZH_kg#_CGb|a-q?96K( z{w|k`(IDpeYJ1(+OC3aS@A$5#og6e--!CC~lv!vG z6w5|4|14@oGvBf2m_$6thC8S!-3F(jxaE~L)-H1UcL;v~l#2H7gr-(t1m6y)boM6f zqYBTVaCN1$e4wQ+obKzBpRX*=Un*T+8Mz)WgRQS3c?=$U_EE8XU`-}b zFpK2TbadAwrtKj;9qnN}n9wV6wW{EXd~Cnamwkp$)@dF1&=7Nh=8?Rg0$O_iwzw&b z7Pp1jis%gz#PPX<(|A0j677V5T(z%(imCiPj`A zCyhsW$uTA$KIiA9u5R{nKiqW*&)+(e9|H|D2Q9At6x#>P!zlTp<=T4(~H<;;gd!Qx`{=uvpL=~F{sVz#>bG+KF zTP%c**tZhLhZ5E!*SRn@ze z=I9)f_Z5K1$6(Tq{zXuq@0h%vI(E%>sW_=s^9zEr;7SpSk)aK)$a>#&dX|W`j-Plm z?sn)OjzGos13S`0!7S3_#3}hD*B{!Nqa7;EfG@C3LRV^(;7F5-L5^OVlP zeU}`OH3ToG4vyNDz<>i~#G~!O50MGor!;+YMobIe>bOxj&U{PMBd`WU*aaZ$0uXiq z2)h7;T>!!^0AUw^unR!g1t9DK5Ox6wy8sOPgbhHxpz)Z->9%v;&0Rcq<;>g#q%&8} z&Ali+#W25IJg#2EJM@xQn4@1&Zxf!_-e~)O^<|Fo5qA^M4Cn-(vT);5y7>$!N8t=64;1F&sYgoQZ=`9Cp?Y^4G>!PxWn#qnG^%t=vJI zu2vo#tf!SU_eNV4r$&l8HB!{6k)lqGqz@wI@8%yF{uK4z5PqS4Ok(w>#T}z%{M`JJcx1K^ zMi#BoLCxHM9-J1ZpRf|b2;davhw9H*vtKI%^I%NMHZ7#ke1DGVreRN-HsaRZ<0B-c zk3ibKphLbXF)bykWc#4iJ_4Dcr_!5c#6I~kKIkaLi4j9}_Q-G)8rpPD>hKVG1+@>^ ze;8-lmOZ0bwvN<&$o?#n_djeETbuD4Zez6lW^^`iAYyQ3wvR>H+ZC-K(F-rS*_|MD z-|2=GM{FgAY@v10f^45q!O-YUWTCkh&vVF7y^*+$%)1tp+^RW`(%BO0Rv zx)d9dPhdO@jGe&RsXjPDg+AfD!t-e_jN8q38SH6NM>^CZ-g#+FnnmU@PT4#-=u}bbO>#`HLwnfK@}}Zzc{Aw$e0hU2+|GfsNNLx)c%cEQ zMuw&OJu(|!p&T7T{hq82+@P4a4+?L}I!uo#zY4nkExT{a&r&QITYjNGz_rL2vf~6H zI}1n1BgFP6GOIUY=QM|Ry67fZ$# zS9r0x7+w?1H@7$aG2})|2)V`E=l3II_cwChmR)$8*+29X5aRifLUhRR_K6T*y62=rOynn&JF2e{-DnybFW#?&!fdG5UMx_Mhy!9b#E_M6s-RDqIJBsOK8$ z|7GYqZhcp*FGux#OnCO>5m*QJIzZFzSle_(OQEP>x?4p*s(7eWlLTIRC`v{hBQSDT z#|bllAT!sjDoLljP{V%%dcm#E@m4IbFNXi;dy6vn&H`*RVrA3WcrKHTX5-nHVrK4i T?wQUko7zVy+Zp>o zN$qOGU>N_-ca1S%zzaPB#sLw)SQk13MG!(FB*RjF$2mhEtoo4h z>_cRiiKx^;-_Y#$G6-Si5r+swAqFiFhXm@jKoU~uTcx%hgEkphZ`nx#dKd^|%`T2%ZSa1(C*>lDp5bv|tX zPZ9b425S5y^wj(J--3oyFlm1O`G@uX51XEUA|LoumQH*0zR#q&8=tpUt@2c{lHDvx z%#$k5SRf08h>%gT3I$7qopQ0S_vO7j+viM#4I3(X)AB}8#`{d>HW~V^yeDY#U!xcu zQ`$r38yP`2hB=xoEcALYt?Bj){k5z(-XPWeq#MCC1Ek|k6XysyBBX#NZ-0^Gz@W??0}JU6~Fx%^FWHGNn!o0*w%yBdi72xi6~jm#`$nu-Z0!B5QYrZLDI z_rqXLwY;*5-M*@na%IaZIJwfMLUWT6r@WK(F`B`-TiSFZ4~rI53UUbd#hfiP5gy@j h9uMmre;Q>GJ)*TDMsWyzW8B$*|2vNHKQ;do{|3EHXTbmf literal 0 HcmV?d00001 diff --git a/Hardware/UPbot-Tools/__pycache__/pb.cpython-37.pyc b/Hardware/UPbot-Tools/__pycache__/pb.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..dce2291822cbf0b67f6167c012896f901b40e551 GIT binary patch literal 17891 zcmdU0S9es$mTu*MBElHk7=!_Xunp=~1cNb11Q{d{66pYLw{#Ao6}p?#Es$jSI)JYu z*f?hk=bUr4@8o%(hx0m5{Du26^X*-A>V#@@XVzVwwFa%;s`~2tcGa%hReiczY^|=Y zjNsqFtKYNU{xTBzPpY{8icz=~uk30VGh!BuM=HPHBIxD{Wg&Nq=s~WYVeV&mByfBUUauOa%8|AquzRm0b*D zMT`P7VicMMX5rb0QDhdG#rR!pmcVhTQ?fr9&kPNdg#OjzRfbm=UfKUx9*N8r%wd!l z3ks+N7iQ7L!h%SyaIR<;=xiijJR2D;2B(-fz)K`v3cPf#ELS*MGFr}hs#kEmQq?17 zi7Xe-%_D_LKV@dw#S->bCB0Rnts1?sC-hQtF@nBpfz|@8kbT!tokvxVQaxI@C-CPp zzX50i(0P(C&;>vj%w1ttnbq`ju^6!u5;1EeZz1Z!@0BQBi5Ao(i$;B=z?T*gY@ z*GQQ3CL&!1WSJJ}GK}F0$@3z;4&3X|vQYN5jMwKn$;%fhWx}$##k0kd<3+k0Ez9AH z=ZRS~Ys)X=t3>*8vt!wuKT@hKQjtj=6K*t%RbZa#K>(I6iyrlvE>tyYEp0QPw zSH68HqG*P)S&O0>z8bY&qToi+;;uw9=TB7fyeMMe#X?cUpyN#A{wVsG=D!7K3wWGG ziq|5eXy$xbA@_oasX27Eh?rWTZPj9Gh3A!8pKai_q2+qnODo6JCV5^=?clYCVrqwO zRY2FS`R{C>rcS6jwU|2Lxk>ADJ-F-9!g<0v>T|v1 zc`RnyD`AKS=PkLbcz_nsSo~2&OvvU#U6H7!`s6f z!Q2?y!y7|;cq4kgN%mFGBitxQDE9Cs_}Byv?;pHR+=9B;U7JzbJa?<3>oMQJO1xj8P@q6=sjP&b3cDbWqI8!OZjc+cquuT}cpru=S4X**gt&saw* zv^|u~9)AZ~cYx3NrT9DibBstm{!Vaqg74;8H_y{f@E7?`0FU1Teh>Jaa$W_$2mFS- z{M*344SdciHJ;nR-;kHT3!GixyE!8K?*hLiFMl`qyTRv`RsMH_zbP-j7o1-3ZwZXQ z7yQjGAB--}!EVXf%C#<8+r%@3RWI<~V>VGuJ!$m8Lm&FMRia*A?Lo^Pv}}%jYnRrG0j`0T-O}%2 zj>3>UFN!1J9SKEo1iIdU?uh39D0D}`>kIIX%9@DcF!EWC;uw_2v?z|j7qWwM*jw@2 z!Mz&=nc}&dmb`qv%A~I{$(cc2L~|!fcfuF%0%p-@@4Sq! za%L@_E!TX_qGcAocxNbIvzPHzA$?V7zV1THUGU}Z9uebRm+@68eN_^N%S&-?*2_1%5VwBgy%*S0y(wm`Ls?3t2dsh;~2MpSF2Ns09qw@`Nj zyBjgOdzWsZ?&ci32c>(^;_gm5P3B`fDhWZD^^XMnmp zi{7CH|13&p=kDiQfIrJ|o&}Tl600Z^&Ps2zBZck(lpYA}$OoWv_tqKC#s{R%>+eD6 z9t`#OAauMl&?a;bN?pDzrtzG?j+m$Ueh4iO!IwMV!q-C@?_uyB4vpbq=y>m_F+5B< zu2BZLt9^zus7kIq)w~R%T7$XB*XCsqwc+!JY`c%&%g)G=i8XozeLaFv%xL3%6fKX2 z#``Fkygyk*bMmOvd24hI=(*4uor8||qSBqy`a6%d^P&FEL&y70>CQ`CzBQ_1*5cVJ z&G%zyc?`b(L@mnKV;b*qv^^dg!{g8)|9!g0Rn5Ofj|A3;YTg=A&08a?d22+q@EScL zM<&+j0!DQKqqs*K?-OWwA~fD7z{GCh@m}El@PyQPYxE>^Plnd$N$Bni=$_R2dkVUz zLj64j9p@B}l5*lHsmr%UbSHiR`>0y;{WMyhhOe{KqI^BA@t#53Godj&1Ks@r-7~7@ zU!x0{>oU{bM^y9Hh-%&%QO#Q;s)g6+f*hGxqh~RyXE6#s&2Tn)XU}tJc`h{G=fLDs zid8gc&q;6I8a)s6`Oq3Y4;`OUO830fdHubBwiiPEy#O7bNJ{sD)a6^F8fGn?t9yPh^Sr-Z}@}9DKNE61}(1@q6nE42l zcf8WQrRm;A+uI@C+t9f?{xzQAw@C-)#kofECB2Bbdk4rnP`J6`&B429c{k+yT`+lf zvWn*JU2RP70evr|dk;F^ol5tfrh6Z4?}v2nL&v*Q>E73LAE51nknRKMcqb~|2b%6f zw0#)TeF&Yq6W`&v`;c^Cin)77dJ%K?5%7X{ zt>W)BsxdeOMUhByNns@7 zRE^k4b9Z7oJ(-KgoYL`R%8Iwy1^6?dOJtmiTw+N4t&vlLV!XwvJ7w9qy6KT^Lzk~rs zE|n9m%iCRfi&HB_$%)B$ZaQOKLQD}EMob%O%&GLkb?U<7a^1E&^Rweg)3Py^T-uJ? ziCi-6)QcheA$@2qP6^YTE2M|hCoIHiCKD4@&a#~a!buRlRF^-_R(C+HPK|VDrG_y* zwmTSEjoVf>&pgFkeVYnMbeyvj!(*-wKP1+_#iNZC^JdNpsV`e^0%x#Xgd76w1j%D&` zTHNMnr>9A~;8;432HxFx);kR{o=nn=CsRn}Q;BhBz6dvfc{3IZ%vio-0x`=TVlk&$ z_Hb%CK9QJms)TP@(`OEGxZ0?vMQ-`DqLxRyUTIrAZ6cr*wLIE(rEONTGCX0%#}nCH zE;(Vjvl76>#>%@_);kON5TlKeoy=s?c21muPHko~dt&QkE|*Tl$I}VZsqq0>D;FTx z76=h>NVjUcwB47u{Nsirb)9?D_V|QmN|qo zCYr}_r|Y;g>^K&3?BzMmilF0I9>?ol$Lqt6ai9f8>}>{|IYGyH)70D8IFf>sfulL# zy4B5&C=R@!>wK#pjXHJyMa?Db;Id|O>5Omf+O?JK$hUJ>zy0sgQNGoU*P~LDuuRhf{Y~t3BGHroRRG6rL?SwbTc& zbOw-j3gtVgqrs!e+tHowu``hvNyVqtSUg6nGfzxWVt6<{KuIV;LnocjGzFsPUm@`m|9xFqs;&Qbt9>G{rNYQQ1G}KL6SD zL}!%sixzvH==K1PE%6{|R|#aNhg1Wx>EXthkFL7#glMI5+1`Xbl1ybW!rk-`Xx9tf zKqfJ4**!UH0?`a0vc2i?V2boSX>&a{& zvysduGMmY4A+wcC7nyD{+sJGuvxCe|GCgE&BeRRlZZf@O`pE1dvzJUinSEpi$PAL% zPv!ubgJceoIZVbNbA-%MGRMf=P9{#~4l)TcLu7`@m}D$6$H|P4IYA~#W|YhrnQ<5- z(O&iTX-^QGB9o@JQsfU3+dfH%O(shwM`n`DDHx-Q=6^gnlA1sY81uN;g`#EKr-?L0 zW}3_lnLEkMlDUh_pUB)z<{mQllDUt}88T%Aeo2AJWS>hGLMouN9H`4 z$H+WR<^q`~$UI5rDKbx!d4|lhWS%4QJee2Byh!FHGB1;Ph0LpD=Ez(m^BS4g$-F`4 zO)_thd7I2TWZot79+~&ad_d+yG9Quon9L_+J|*)R3?DxG(x>INbgHpC?L;bz47Tj)OTgpcdB^hs4>s0#Qc>Of2Gx5Y4caw{gn=XrPE(oj~r41 zh&Bgm(LgO0sI>%Yt?x$c?-9*ko6QYJGs&TJEI&*mTF5m*Yw)7ql?C%i-)bjL z4SbV#V(VV1mD;Gs8jQg2G1&I^v@YZxf+L|G=|_*1*fbmeGb>_rXH9tXW-S=Q<>LEe z+T589uI2y5sKmVZ*FKytijpP@yo^kzh#S&{Qh`oFVT4Zh~HvH>d1E?#kBBm{>)VKe|cOhPek=!AB%7( z(tAZ09g!cPl#7fOj27ZQ@D<=cJM#aZJX0tytBdi6goG3+q*Nls3MrFFi9*UHQmT*& ziIgd%Qsc~%NV(!vX`E__R47i3#;L_0nbDXl6{lWu<|$;pM5+|hAdzZ?ERaZzLavZV ztwI(`q)s7MN~B&PizG5%Ay>%}HYnt3$yuO~Yb4^$;bMtgp*YuSoFx)js5nbCPNPJw zRGeiR=Q@cjQk>-)XN8R7D#ckTIae#>dQG)TBG)L+YK_w*k;RI$M&sNdk!uy_MvZfm zM3yMdT8S)G$U2EMDx`TXiZ5)M=VJ7=4Ze7#Tv~=!@!w5bWz^Rxew*~LTp{fe@z$ktXH2Pvh*-ki8nxuOa&+;^qHX?Syt|4&^xkEz| z8Zx9I!y4k>eWu)f>o8jEKK??lxsN~S%+&7f*}7-2anQ=<8tDVTOl2>9kJd;ZiDt@s zdiOUD;_ER3D8A9!$6sg_G#lmJ@{1{>Y@7II$*4R?-&hUOH$*cF_gVNNC~Gx#CvAKN z!yQN?E*?f^%GgV^(J19YO#W_>zgvxR=`E^CF;!|&rB?Zvz4V!UX;${EG`a0Nb7g3tAh zQKN)MCI=Lqsgnb0+%9jXosulywHDI_>r4g4(b#PbO^z5PJ#QMd}V~MPu-8Ye4BJInZiZe zU@|vu@wFF!J{z>Aa^iAoDIP(@h0<~=tt7LW%o;K`l37b83S*SxK}L9LC#-|a6Bpxtf@W6`TkJcy3k!q_%4O7Q9?H;e0dVOH{mOj&}~WRqU3k4M}GHa zsbJ`rg&o^Ld})`M~-_{B)MFf6&MN;{dse5;KD_&bU!$6(-n*(KETZXBcUlRk6@# z)vbnIv2~jb+^IPIez)T4xtU5v*Ivcbb+(d4?bQ9oNM)pvtK=}=t>l*&&v5Sv+tjQVHU#;M-3z=vB-Qwty8r85~TtTgeI_)_keb8{s_ zS@q^Rs%#j?wKxf*q-3h>n<9?Gb+J6J^4FW8%jKou=DaHG<8Q7k1)zH5&ccm_<+}^P?S--$-T!#|+KpS+ zmKIcgzv0Hsg&Vhi&EzDu)!?(&_@(=;+FCOS)?_VOZ@2Cz&1OBm-`du#_m>3vUR>2} zi43E-)s)FxYg=ZpL;)oxE%Xe#|LXQCosu3pBOUPuN_+=^8#_kAdd41;=O16~vU&Dk za>wi%UGp`zV{{E}UPZk%agRl39~X814D}~w*W5J@jk$P_bxFEbIpyr^z}c!rNsFnuG3S3Zl@ zDs}BYw9i$PxCJoKgJ-$PttX@nHg}-@Hg|ajwZlE0MeTB*kD$)*9M7Znc!7_i&hpcI zoKIk;&rk44w2bhRdaNJ^J>6jc6>#8Fn(ui>17Bg0~@el?|?kZ7!kB z(9K&ywC=Q{2x_CHE=RGO%1codwMQb*O0P0fJPI1|y7CC&Fe{Z!9!e2%OjGvlW_6Rg zwt_lg&cY)RCCYz4tR;aU9FntRQGZX_q+k;`LJWdhgh{~*bnY<#X1MzGH1v(m z#DHUS`-2CHN(xIwbB#1Mp|cz1k|D{LqWUVTqnqZ}MM9q9X0y(Pd=VpJVm&iD(+7JU z8gpnv7en=i-vhF}n7}`G*#-o)VR2Z(hZe*EmqMaS+>H#{J#K$UBFqBM_Iz%UN7Oh( z25)Dr0MeV}xU)-s9!^M$F;iau`OoP2^BXXduVF)Z9zbQ1T0=D3$tql;n}ib7cE;ao ziKw_DWG$>07o==TQUN#C4HY--kk8Q?MZHGQZ+f1Z&QX)Qs%5M)!EHEM5miP5_(9SO z+3(55#5>VwM#_PzK(+~QB!>5F%|Md6RbK4#>W^=f97%^#Hn9nW)}izABDFZxdNUTsQ>*Ni zq}jTRVKK>=G!gn&I3SR^579Spl3&5_$7ewilYl*~5It)Tp-)0FQYyF1d#O;NpMCmH z^sR1@j6keVyopmfZL&2r+6t?i2yoY;u)dABcRbzl73_WurBAj84tK^6$nFcXs+AnW z$TS9&_h3H>yv;7W3_|=EH&^h*jadc}c@m&}m`u~b?qckt_fQ@PFPyP90o90J`T*cSOK26-*vD4F`=L~p&zFi|3N9}WW zmhEEA9XpyC^w8Oddb&7`{FvH$6Ev4jF`(ckT8QpZz3)#n<(d8nmnZ}8PS9N1h1Ss{ zJQ~5XD@>n2k0W`M{4`iDcD(gwQhW&2==g-eLls1vUuahU= zxb-)^B847{`?G8ci9Oj*jF);RK?`LgtI#*Pn`JSwg=`SzvHjN*V9B!6-mF%?pk zE_*PxgEYDacd=*OrL9bzx0@aJ2bV5fe7u7&tlamSvJq0UZq8j$M)ioGKg(WT7m;|} zl2?963#Yqu&L0TqhzFdzjEp0!$5-YC6AzI{zD<)znIA};Oi4_Tq!NdZC)1SQ!f0&( zB~#ua{5FBf0iYaHz6r34(x+0fryO4$m#pv{mm~C%r(CI6i(qfalsYE_#o=N*qGO<; z*xFn#6^4v%9+Ogg(&m?185e79WLX={TBOZPCo>f{T}3%-+XxfNl_CymT;&jv1K_P@ zgurHL6RTuv5v6bFmp5sLBPXE*vx5>-TxDb$W&yBhOqnP!9tzFAIc1n;XZ8za@lTq} z*x%ncl*N+OXp4CQ1X`!B4dkzTxDf0a6iIqE2c~^E>p41tQ`BKSm!gGyhctPEBFk-z z_0q_))dHU7HqC=iCfqr04eXb$i1mX0ZLZvzNK>6+3|~~#GI@TnV!m~&Gra`&*p8s0 zaF@Ke&&TIGGk3&RTf|9`{AbiW=vgiK^2<1y%7oRec1OUs3j47hx>a zxXy@S0s{fb6%3HqX_TYs=u1qQ5Y^YI@35$7EltOd(bpH34?jjP`8aq#GY6lCFF^ND z-0#mE#C?*?z@L2|AK zhzLp2NdqxC0f~P@On#dH<*o7_KwnklyTFxIZSW(yq6hsFopBC;K__6+<{8Ar;VfHo zVxSYW7wvolC7uEpwpiGbw%PPfWyuoyDT5ZFR5qzLgrP53$0)A+E(ZUGRwY3bF`ZLG z;-H)2uw4vP1L^3&MVS6n91x^$L`a^^ln@J!?+X&*Rm}S>idF>N>!uEN9`=?4E>OBp zWNI#~0ilc-u^B%+U>o!!LubG17V=?x!@%Xv`i1!;wqtkgt~t$ioCL8RKS#=4C&8V( zi%ZRtn3;u53ycl}+LNsJ1-7!oNd_iOURmRROPOdI$o; z3>#Kbx>!@lK=mD}MkQ&Q%oCth^krJk5k@yZ`Aq_^5TJxkQrMCA36MH#4Ur@&Nu&&s zH%Xs5Hj)Q-8h+qe2Kt00-V4%0F+OR zeW-sNNYWTBu2fLuTD3$pJ(@GhZLHCMNzDyqi^sJ@pI;T0=Yyq%)!^>Jot5Rc%F1ZT zQ#AcG0>nR+!JV5(RxTGLUgfW~YjqybjrKTJDerRn1M&)4P<$T1(-|@HV#qXo#}Fk7 R882V>fIUZ&)~}ZQzX7+?4nzO| literal 0 HcmV?d00001 diff --git a/Hardware/UPbot-Tools/app.py b/Hardware/UPbot-Tools/app.py new file mode 100644 index 0000000..2e0aaac --- /dev/null +++ b/Hardware/UPbot-Tools/app.py @@ -0,0 +1,434 @@ +# coding:utf-8 + +import sys +sys.path.append("..") +from pypibot import log +import pypibot + +import params +import dataholder +from dataholder import MessageID +from transport import Transport +from PyQt5.QtWidgets import QApplication, QDialog +from PyQt5.QtCore import QObject,pyqtSignal +import pb +import threading + +port = "COM7" + +pypibot.assistant.enableGlobalExcept() +# log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log") +log.setLevel("i") + + +class MainDialog(QDialog): + encoder_signal = pyqtSignal(list) + imu_signal = pyqtSignal(list) + pid_debug_signal = pyqtSignal(list) + + def __init__(self, parent=None): + super(QDialog, self).__init__(parent) + self.ui = pb.Ui_pb() + self.ui.setupUi(self) + self.model_type_list = {"2wd-diff": dataholder.RobotModelType.MODEL_TYPE_2WD_DIFF, + "4wd-diff": dataholder.RobotModelType.MODEL_TYPE_4WD_DIFF, + "3wd-omni": dataholder.RobotModelType.MODEL_TYPE_3WD_OMNI, + "4wd-omni": dataholder.RobotModelType.MODEL_TYPE_4WD_OMNI, + "4wd-mecanum": dataholder.RobotModelType.MODEL_TYPE_4WD_MECANUM} + + self.model_value_list = {0: dataholder.RobotModelType.MODEL_TYPE_2WD_DIFF, + 1: dataholder.RobotModelType.MODEL_TYPE_4WD_DIFF, + 2: dataholder.RobotModelType.MODEL_TYPE_3WD_OMNI, + 3: dataholder.RobotModelType.MODEL_TYPE_4WD_OMNI, + 3: dataholder.RobotModelType.MODEL_TYPE_4WD_MECANUM} + + self.model_index_list = {dataholder.RobotModelType.MODEL_TYPE_2WD_DIFF: 0, + dataholder.RobotModelType.MODEL_TYPE_4WD_DIFF: 1, + dataholder.RobotModelType.MODEL_TYPE_3WD_OMNI: 2, + dataholder.RobotModelType.MODEL_TYPE_4WD_OMNI: 3, + dataholder.RobotModelType.MODEL_TYPE_4WD_MECANUM: 4} + + self.imu_type_list = {"gy65": 0, "gy85": 1, "gy87": 2} + self.imu_value_list = {49: "gy65", 69: "gy85", 71: "gy87"} + self.imu_index_list = {0: 49, 1: 69, 2: 71} + + self.init_ui() + self.mboard = None + + self.init_dev() + + self.encoder_signal.connect(self.update_encoder) + self.imu_signal.connect(self.update_imu) + self.pid_debug_signal.connect(self.update_pid_debug) + + self._KeepRunning = True + self.encoder_thread = threading.Thread(target=self._read_encoder) + self.encoder_thread.start() + + def closeEvent(self, event): + self._KeepRunning = False + + def init_ui(self): + for model_type in self.model_type_list.keys(): + self.ui.combox_model.addItem(model_type) + + for imu_type in self.imu_type_list.keys(): + self.ui.combox_imu_type.addItem(imu_type) + + self.ui.slider_wheel_diameter.setMinimum(10) + self.ui.slider_wheel_diameter.setMaximum(500) + + self.ui.slider_wheel_track.setMinimum(50) + self.ui.slider_wheel_track.setMaximum(1000) + + self.ui.slider_encoder.setMinimum(1) + self.ui.slider_encoder.setMaximum(500) + + self.ui.slider_motor_ratio.setMinimum(1) + self.ui.slider_motor_ratio.setMaximum(1000) + + self.ui.slider_pid_interval.setMinimum(1) + self.ui.slider_pid_interval.setMaximum(80) + self.ui.slider_kp.setMinimum(0) + self.ui.slider_kp.setMaximum(10000) + self.ui.slider_ki.setMinimum(0) + self.ui.slider_ki.setMaximum(32000) + self.ui.slider_kd.setMinimum(0) + self.ui.slider_kd.setMaximum(1000) + self.ui.slider_ko.setMinimum(0) + self.ui.slider_ko.setMaximum(1000) + + self.ui.slider_cmd_lasttime.setMinimum(0) + self.ui.slider_cmd_lasttime.setMaximum(1000) + self.ui.slider_vx_max.setMinimum(0) + self.ui.slider_vx_max.setMaximum(500) + self.ui.slider_vy_max.setMinimum(0) + self.ui.slider_vy_max.setMaximum(500) + self.ui.slider_va_max.setMinimum(0) + self.ui.slider_va_max.setMaximum(2000) + + self.ui.comboBox_support_model.setVisible(False) + self.ui.pushButton_load.setVisible(False) + + self.ui.pushButton_read.clicked.connect(self.read_params) + self.ui.pushButton_set.clicked.connect(self.write_params) + self.ui.pushButton_start.clicked.connect(self.start_motor) + self.ui.pushButton_stop.clicked.connect(self.stop_motor) + + self.ui.pushButton_start_2.clicked.connect(self.start_control) + self.ui.pushButton_stop_2.clicked.connect(self.stop_control) + + self.ui.slider_set_pwm1.setMinimum(-5000) + self.ui.slider_set_pwm1.setMaximum(5000) + self.ui.slider_set_pwm2.setMinimum(-5000) + self.ui.slider_set_pwm2.setMaximum(5000) + self.ui.slider_set_pwm3.setMinimum(-5000) + self.ui.slider_set_pwm3.setMaximum(5000) + self.ui.slider_set_pwm4.setMinimum(-5000) + self.ui.slider_set_pwm4.setMaximum(5000) + self.ui.tabWidget.setTabText(0, '1.参数配置') + self.ui.tabWidget.setTabText(1, '2.电机测试') + self.ui.tabWidget.setCurrentIndex(0) + + def update_param(self, param): + log.i("type:%d %d" % (param.model_type, param.imu_type)) + try: + self.ui.combox_model.setCurrentIndex(self.model_index_list[param.model_type]) + except Exception as e: + print("model type err") + + try: + self.ui.combox_imu_type.setCurrentIndex( + self.imu_type_list[self.imu_value_list[param.imu_type]]) + except Exception as e: + print("imu type err") + + try: + self.ui.slider_wheel_diameter.setSliderPosition( + param.wheel_diameter) + self.ui.slider_wheel_track.setSliderPosition(param.wheel_track) + self.ui.slider_encoder.setSliderPosition(param.encoder_resolution) + self.ui.slider_motor_ratio.setSliderPosition(param.motor_ratio) + + self.ui.checkBox_motor1.setChecked( + param.motor_nonexchange_flag & 0x1) + self.ui.checkBox_motor2.setChecked( + param.motor_nonexchange_flag & 0x2) + self.ui.checkBox_motor3.setChecked( + param.motor_nonexchange_flag & 0x4) + self.ui.checkBox_motor4.setChecked( + param.motor_nonexchange_flag & 0x8) + + self.ui.checkBox_encoder1.setChecked( + param.encoder_nonexchange_flag & 0x1) + self.ui.checkBox_encoder2.setChecked( + param.encoder_nonexchange_flag & 0x2) + self.ui.checkBox_encoder3.setChecked( + param.encoder_nonexchange_flag & 0x4) + self.ui.checkBox_encoder4.setChecked( + param.encoder_nonexchange_flag & 0x8) + except Exception as e: + print("motor dir param err") + + try: + self.ui.slider_cmd_lasttime.setSliderPosition(param.cmd_last_time) + self.ui.slider_vx_max.setSliderPosition(param.max_v_liner_x) + self.ui.slider_vy_max.setSliderPosition(param.max_v_liner_y) + self.ui.slider_va_max.setSliderPosition(param.max_v_angular_z) + except Exception as e: + print("pid param err") + + try: + self.ui.slider_pid_interval.setSliderPosition( + param.do_pid_interval) + self.ui.slider_kp.setSliderPosition(param.kp) + self.ui.slider_ki.setSliderPosition(param.ki) + self.ui.slider_kd.setSliderPosition(param.kd) + self.ui.slider_ko.setSliderPosition(param.ko) + except Exception as e: + print("velocity limit param err") + + def read_params(self): + # get robot parameter + robotParam = self.DataHolder[MessageID.ID_GET_ROBOT_PARAMETER] + p = self.mboard.request(MessageID.ID_GET_ROBOT_PARAMETER) + if p: + log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" + % (robotParam.param.model_type, + robotParam.param.wheel_diameter, + robotParam.param.wheel_track, + robotParam.param.encoder_resolution + )) + + log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" + % (robotParam.param.do_pid_interval, + robotParam.param.kp, + robotParam.param.ki, + robotParam.param.kd, + robotParam.param.ko)) + + log.info("cmd_last_time:%d imu_type:%d" + % (robotParam.param.cmd_last_time, + robotParam.param.imu_type + )) + + log.info("max_v:%d %d %d" + % (robotParam.param.max_v_liner_x, + robotParam.param.max_v_liner_y, + robotParam.param.max_v_angular_z + )) + + log.info("motor flag:%d encoder flag: %d" + % (robotParam.param.motor_nonexchange_flag, + robotParam.param.encoder_nonexchange_flag + )) + else: + log.error('get params err') + return False + + self.update_param(robotParam.param) + + def get_input_param(self): + params = dataholder.RobotParameters() + + params.wheel_diameter = self.ui.slider_wheel_diameter.sliderPosition() + params.wheel_track = self.ui.slider_wheel_track.sliderPosition() + params.encoder_resolution = self.ui.slider_encoder.sliderPosition() + params.do_pid_interval = self.ui.slider_pid_interval.sliderPosition() + params.kp = self.ui.slider_kp.sliderPosition() + params.ki = self.ui.slider_ki.sliderPosition() + params.kd = self.ui.slider_kd.sliderPosition() + params.ko = self.ui.slider_ko.sliderPosition() + params.cmd_last_time = self.ui.slider_cmd_lasttime.sliderPosition() + params.max_v_liner_x = self.ui.slider_vx_max.sliderPosition() + params.max_v_liner_y = self.ui.slider_vy_max.sliderPosition() + params.max_v_angular_z = self.ui.slider_va_max.sliderPosition() + params.motor_ratio = self.ui.slider_motor_ratio.sliderPosition() + params.imu_type = self.imu_index_list[self.ui.combox_imu_type.currentIndex( + )] + + params.model_type = self.model_value_list[self.ui.combox_model.currentIndex()] + + params.motor_nonexchange_flag = 0 + + if self.ui.checkBox_motor1.isChecked(): + params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x1 + else: + params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xfe + + if self.ui.checkBox_motor2.isChecked(): + params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x2 + else: + params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xfd + + if self.ui.checkBox_motor3.isChecked(): + params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x4 + else: + params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xfb + + if self.ui.checkBox_motor4.isChecked(): + params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x8 + else: + params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xf7 + + params.encoder_nonexchange_flag = 0 + + if self.ui.checkBox_encoder1.isChecked(): + params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x1 + else: + params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xfe + + if self.ui.checkBox_encoder2.isChecked(): + params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x2 + else: + params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xfd + + if self.ui.checkBox_encoder3.isChecked(): + params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x4 + else: + params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xfb + + if self.ui.checkBox_encoder4.isChecked(): + params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x8 + else: + params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xf7 + + return params + + def write_params(self): + self.DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = self.get_input_param() + p = self.mboard.request(MessageID.ID_SET_ROBOT_PARAMETER) + if p: + log.info('set parameter success') + else: + log.error('set parameter err') + quit(1) + + def update_pid_debug(self, pid_data): + self.ui.label_input_1.setText(str(pid_data[0])) + self.ui.label_input_2.setText(str(pid_data[1])) + self.ui.label_input_3.setText(str(pid_data[2])) + self.ui.label_input_4.setText(str(pid_data[3])) + self.ui.label_output_1.setText(str(pid_data[4])) + self.ui.label_output_2.setText(str(pid_data[5])) + self.ui.label_output_3.setText(str(pid_data[6])) + self.ui.label_output_4.setText(str(pid_data[7])) + + def update_imu(self, imu): + #log.info('imu: %s'%(('\t\t').join([str(x) for x in imu]))) + self.ui.label_acc_x.setText(str(round(imu[0], 6))) + self.ui.label_acc_y.setText(str(round(imu[1], 6))) + self.ui.label_acc_z.setText(str(round(imu[2], 6))) + self.ui.label_gyro_x.setText(str(round(imu[3], 6))) + self.ui.label_gyro_y.setText(str(round(imu[4], 6))) + self.ui.label_gyro_z.setText(str(round(imu[5], 6))) + self.ui.label_magn_x.setText(str(round(imu[6], 6))) + self.ui.label_magn_y.setText(str(round(imu[7], 6))) + self.ui.label_magn_z.setText(str(round(imu[8], 6))) + + def update_encoder(self, encoder): + log.debug('encoder count: %s'%(('\t\t').join([str(x) for x in encoder]))) + self.ui.label_feedback1.setText(str(encoder[0])) + self.ui.label_feedback2.setText(str(encoder[1])) + self.ui.label_feedback3.setText(str(encoder[2])) + self.ui.label_feedback4.setText(str(encoder[3])) + + + def _read_encoder(self): + while self._KeepRunning: + robot_encoder = self.DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder + p = self.mboard.request(MessageID.ID_GET_ENCODER_COUNT) + if p: + # log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robot_encoder]))) + self.encoder_signal.emit([int(x) for x in robot_encoder]) + + robot_imu = self.DataHolder[MessageID.ID_GET_IMU].imu + p = self.mboard.request(MessageID.ID_GET_IMU) + if p: + # log.info('imu: %s'%(('\t\t').join([str(x) for x in robot_imu]))) + self.imu_signal.emit([x for x in robot_imu]) + + pid_data = self.DataHolder[MessageID.ID_GET_PID_DEBUG].pid_data + p = self.mboard.request(MessageID.ID_GET_PID_DEBUG) + if p: + # log.info('imu: %s'%(('\t\t').join([str(x) for x in robot_imu]))) + self.pid_debug_signal.emit([x for x in pid_data]) + import time + time.sleep(0.1) + + def start_motor(self): + self.DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [self.ui.slider_set_pwm1.sliderPosition(), + self.ui.slider_set_pwm2.sliderPosition(), + self.ui.slider_set_pwm3.sliderPosition(), + self.ui.slider_set_pwm4.sliderPosition()] + p = self.mboard.request(MessageID.ID_SET_MOTOR_PWM) + if p: + log.info('set pwm success') + else: + log.error('set pwm err') + + def stop_motor(self): + self.DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4 + p = self.mboard.request(MessageID.ID_SET_MOTOR_PWM) + if p: + log.info('set pwm success') + else: + log.error('set pwm err') + + def start_control(self): + self.DataHolder[MessageID.ID_SET_VEL].v_liner_x = 200 + self.DataHolder[MessageID.ID_SET_VEL].v_liner_y = 0 + self.DataHolder[MessageID.ID_SET_VEL].v_angular_z = 10 + p = self.mboard.request(MessageID.ID_SET_VEL) + if p: + log.info('set vel success') + else: + log.error('set vel err') + + def stop_control(self): + self.DataHolder[MessageID.ID_SET_VEL].v_liner_x = 0 + self.DataHolder[MessageID.ID_SET_VEL].v_liner_y = 0 + self.DataHolder[MessageID.ID_SET_VEL].v_angular_z = 0 + p = self.mboard.request(MessageID.ID_SET_VEL) + if p: + log.info('set vel success') + else: + log.error('set vel err') + + def init_dev(self): + self.mboard = Transport(port, params.pibotBaud) + if not self.mboard.start(): + log.error("can not open %s" % port) + return False + + self.DataHolder = self.mboard.getDataHolder() + + for num in range(0, 3): + log.info("****************get robot version*****************") + boardVersion = self.DataHolder[MessageID.ID_GET_VERSION] + p = self.mboard.request(MessageID.ID_GET_VERSION) + if p: + log.info("firmware version:%s buildtime:%s" % ( + boardVersion.version.decode(), boardVersion.build_time.decode())) + break + else: + log.error('read firmware version err') + import time + time.sleep(1) + if num == 2: + log.error('please check connection or baudrate') + return False + + return self.read_params() + +if __name__ == '__main__': + app = QApplication(sys.argv) + + import qdarkstyle + app.setStyleSheet(qdarkstyle.load_stylesheet(qt_api='pyqt5')) + + # from qt_material import apply_stylesheet + # apply_stylesheet(app, theme='light_teal.xml') + myDlg = MainDialog() + myDlg.show() + sys.exit(app.exec_()) diff --git a/Hardware/UPbot-Tools/dataholder.py b/Hardware/UPbot-Tools/dataholder.py new file mode 100644 index 0000000..a9e80c5 --- /dev/null +++ b/Hardware/UPbot-Tools/dataholder.py @@ -0,0 +1,250 @@ +import struct + +params_size=29 + +# main board +class MessageID: + ID_GET_VERSION = 0 + ID_SET_ROBOT_PARAMETER = 1 + ID_GET_ROBOT_PARAMETER = 2 + ID_INIT_ODOM = 3 + ID_SET_VEL = 4 + ID_GET_ODOM = 5 + ID_GET_PID_DEBUG = 6 + ID_GET_IMU = 7 + ID_GET_ENCODER_COUNT = 8 + ID_SET_MOTOR_PWM = 9 + +class RobotMessage: + def pack(self): + return b'' + + def unpack(self): + return True + +class RobotFirmwareInfo(RobotMessage): + def __init__(self): + self.version = '' + self.build_time = '' + + def unpack(self, data): + try: + upk = struct.unpack('16s16s', bytes(data)) + except: + return False + [self.version, self.build_time] = upk + return True + +class RobotImuType: + IMU_TYPE_GY65 = 49 + IMU_TYPE_GY85 = 69 + IMU_TYPE_GY87 = 71 + +class RobotModelType: + MODEL_TYPE_2WD_DIFF = 1 + MODEL_TYPE_4WD_DIFF = 2 + MODEL_TYPE_3WD_OMNI = 101 + MODEL_TYPE_4WD_OMNI = 102 + MODEL_TYPE_4WD_MECANUM = 201 + +class RobotParameters(): + def __init__(self, wheel_diameter=0, \ + wheel_track=0, \ + encoder_resolution=0, \ + do_pid_interval=0, \ + kp=0, \ + ki=0, \ + kd=0, \ + ko=0, \ + cmd_last_time=0, \ + max_v_liner_x=0, \ + max_v_liner_y=0, \ + max_v_angular_z=0, \ + imu_type=0, \ + motor_ratio=0, \ + model_type=0, \ + motor_nonexchange_flag=255, \ + encoder_nonexchange_flag=255, \ + ): + self.wheel_diameter = wheel_diameter + self.wheel_track = wheel_track + self.encoder_resolution = encoder_resolution + self.do_pid_interval = do_pid_interval + self.kp = kp + self.ki = ki + self.kd = kd + self.ko = ko + self.cmd_last_time = cmd_last_time + self.max_v_liner_x = max_v_liner_x + self.max_v_liner_y = max_v_liner_y + self.max_v_angular_z = max_v_angular_z + self.imu_type = imu_type + self.motor_ratio = motor_ratio + self.model_type = model_type + self.motor_nonexchange_flag = motor_nonexchange_flag + self.encoder_nonexchange_flag = encoder_nonexchange_flag + reserve=b'\xff' + self.reserve=b'' + for i in range(64-params_size): + self.reserve+=reserve +robotParam = RobotParameters() + +class GetRobotParameters(RobotMessage): + def __init__(self): + self.param = robotParam + + def unpack(self, data): + #print(bytes(data), len(bytes(data))) + upk = struct.unpack('<3H1B8H1B1H3B%ds'%(64-params_size), bytes(data)) + + [self.param.wheel_diameter, + self.param.wheel_track, + self.param.encoder_resolution, + self.param.do_pid_interval, + self.param.kp, + self.param.ki, + self.param.kd, + self.param.ko, + self.param.cmd_last_time, + self.param.max_v_liner_x, + self.param.max_v_liner_y, + self.param.max_v_angular_z, + self.param.imu_type, + self.param.motor_ratio, + self.param.model_type, + self.param.motor_nonexchange_flag, + self.param.encoder_nonexchange_flag, + self.param.reserve] = upk + return True + +class SetRobotParameters(RobotMessage): + def __init__(self): + self.param = robotParam + + def pack(self): + data = [self.param.wheel_diameter, + self.param.wheel_track, + self.param.encoder_resolution, + self.param.do_pid_interval, + self.param.kp, + self.param.ki, + self.param.kd, + self.param.ko, + self.param.cmd_last_time, + self.param.max_v_liner_x, + self.param.max_v_liner_y, + self.param.max_v_angular_z, + self.param.imu_type, + self.param.motor_ratio, + self.param.model_type, + self.param.motor_nonexchange_flag, + self.param.encoder_nonexchange_flag, + self.param.reserve] + + print(data) + pk = struct.pack('<3H1B8H1B1H3B%ds'%(64-(3*2+1+8*2+1+2+3)), *data) + return pk + + def unpack(self, data): + return True + +class RobotVel(RobotMessage): + def __init__(self): + self.v_liner_x = 0 + self.v_liner_y = 0 + self.v_angular_z = 0 + + def pack(self): + data = [self.v_liner_x, + self.v_liner_y, + self.v_angular_z] + pk = struct.pack('3h', *data) + return pk + + def unpack(self, data): + return True + +#todo the rest of the message classes +class RobotOdom(RobotMessage): + def __init__(self): + self.v_liner_x = 0 + self.v_liner_y = 0 + self.v_angular_z = 0 + self.x = 0 + self.y = 0 + self.yaw = 0 + + def unpack(self, data): + try: + upk = struct.unpack('<3H2l1H', bytes(data)) + except: + return False + [self.v_liner_x, + self.v_liner_y, + self.v_angular_z, + self.x, + self.y, + self.yaw] = upk + return True + +class RobotPIDData(RobotMessage): + def __init__(self): + self.pid_data = [0]*8 + + def unpack(self, data): + try: + upk = struct.unpack('<8l', bytes(data)) + except: + return False + + self.pid_data = upk + return True + +class RobotIMU(RobotMessage): + def __init__(self): + self.imu = [0]*9 + + def unpack(self, data): + try: + upk = struct.unpack('<9f', bytes(data)) + except: + return False + + self.imu = upk + return True + +class RobotEncoderCount(RobotMessage): + def __init__(self): + self.encoder = [0]*4 + + def unpack(self, data): + try: + upk = struct.unpack('<4f', bytes(data)) + except: + return False + + self.encoder = upk + return True + +class RobotMotorPWM(RobotMessage): + def __init__(self): + self.pwm = [0]*4 + + def pack(self): + pk = struct.pack('4h', *self.pwm) + return pk + + def unpack(self, data): + return True + +BoardDataDict = {MessageID.ID_GET_VERSION:RobotFirmwareInfo(), + MessageID.ID_GET_ROBOT_PARAMETER:GetRobotParameters(), + MessageID.ID_SET_ROBOT_PARAMETER:SetRobotParameters(), + MessageID.ID_SET_VEL:RobotVel(), + MessageID.ID_GET_ODOM:RobotOdom(), + MessageID.ID_GET_PID_DEBUG: RobotPIDData(), + MessageID.ID_GET_IMU: RobotIMU(), + MessageID.ID_GET_ENCODER_COUNT: RobotEncoderCount(), + MessageID.ID_SET_MOTOR_PWM: RobotMotorPWM(), + } + diff --git a/Hardware/UPbot-Tools/main.py b/Hardware/UPbot-Tools/main.py new file mode 100644 index 0000000..5eaeb3a --- /dev/null +++ b/Hardware/UPbot-Tools/main.py @@ -0,0 +1,115 @@ +import platform +import sys +sys.path.append("..") +import pypibot +from pypibot import log +from transport import Transport +from dataholder import MessageID +import params +import time +import signal + +#for linux +#port="/dev/pibot" + +#for windows +port="COM7" + +pypibot.assistant.enableGlobalExcept() +#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log") +log.setLevel("i") + +run_flag = True + +def exit(signum, frame): + global run_flag + run_flag = False + +if __name__ == '__main__': + signal.signal(signal.SIGINT, exit) + + mboard = Transport(port, params.pibotBaud) + if not mboard.start(): + log.error("can not open %s"%port) + sys.exit() + + DataHolder = mboard.getDataHolder() + + for num in range(0,3): + log.info("****************get robot version*****************") + boardVersion = DataHolder[MessageID.ID_GET_VERSION] + p = mboard.request(MessageID.ID_GET_VERSION) + if p: + log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode())) + break + else: + log.error('read firmware version err\r\n') + import time + time.sleep(1) + if num == 2: + log.error('please check connection or baudrate\r\n') + sys.exit() + + # get robot parameter + robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER] + p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER) + if p: + log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \ + %(robotParam.param.model_type, \ + robotParam.param.wheel_diameter, \ + robotParam.param.wheel_track, \ + robotParam.param.encoder_resolution + )) + + log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \ + %(robotParam.param.do_pid_interval, \ + robotParam.param.kp, \ + robotParam.param.ki, \ + robotParam.param.kd, \ + robotParam.param.ko)) + + log.info("cmd_last_time:%d imu_type:%d" \ + %(robotParam.param.cmd_last_time,\ + robotParam.param.imu_type + )) + + log.info("max_v:%d %d %d\r\n" \ + %(robotParam.param.max_v_liner_x,\ + robotParam.param.max_v_liner_y, \ + robotParam.param.max_v_angular_z + )) + + log.info("motor flag:%d encoder flag: %d\r\n" \ + %(robotParam.param.motor_nonexchange_flag,\ + robotParam.param.encoder_nonexchange_flag + )) + else: + log.error('get params err\r\n') + quit(1) + + log.info("****************get odom&imu*****************") + while run_flag: + robotOdom = DataHolder[MessageID.ID_GET_ODOM] + p = mboard.request(MessageID.ID_GET_ODOM) + if p: + log.info('request get odom success, vx=%d vy=%d vangular=%d, x=%d y=%d yaw=%d'%(robotOdom.v_liner_x, \ + robotOdom.v_liner_y, \ + robotOdom.v_angular_z, \ + robotOdom.x, \ + robotOdom.y, \ + robotOdom.yaw)) + else: + log.error('get odom err') + quit(1) + + robotIMU = DataHolder[MessageID.ID_GET_IMU].imu + p = mboard.request(MessageID.ID_GET_IMU) + if p: + log.info('get imu success, imu=[%f %f %f %f %f %f %f %f %f]'%(robotIMU[0], robotIMU[1], robotIMU[2], \ + robotIMU[3], robotIMU[4], robotIMU[5], \ + robotIMU[6], robotIMU[7], robotIMU[8])) + else: + log.error('get imu err') + quit(1) + + time.sleep(0.1) diff --git a/Hardware/UPbot-Tools/params.py b/Hardware/UPbot-Tools/params.py new file mode 100644 index 0000000..086ab3a --- /dev/null +++ b/Hardware/UPbot-Tools/params.py @@ -0,0 +1,82 @@ +import dataholder +import os +from dataholder import RobotImuType +from dataholder import RobotModelType + +# for linux +# pibotModel = os.environ['PIBOT_MODEL'] +# boardType = os.environ['PIBOT_BOARD'] +# pibotBaud = os.environ['PIBOT_DRIVER_BAUDRATE'] +# for windwos +pibotModel = "apollo" +boardType = "stm32f1" +pibotBaud = 115200 + +print(pibotModel) +print(boardType) +print(pibotBaud) + + + +pibotParam = dataholder.RobotParameters() + +if pibotModel == "apollo" and boardType == "arduino": + pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \ + 75, 2500, 0, 10, \ + 250, 40, 0, 200, \ + RobotImuType.IMU_TYPE_GY85, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) +elif pibotModel == "apollo" and boardType == "stm32f1": + pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \ + 320, 2700, 0, 10, \ + 250, 50, 0, 200, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) +elif pibotModel == "apollo" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \ + 320, 2700, 0, 10, \ + 250, 40, 0, 200, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) +elif pibotModel == "zeus" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(58, 230, 44, 10, \ + 320, 2700, 0, 10, \ + 250, 50, 50, 250, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_3WD_OMNI) +elif pibotModel == "hades" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(76, 470, 44, 10, \ + 320, 2700, 0, 10, \ + 250, 50, 50, 250, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_4WD_MECANUM) +elif pibotModel == "hadesX" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(150, 565, 44, 10, \ + 250, 2750, 0, 10, \ + 250, 50, 50, 250, \ + RobotImuType.IMU_TYPE_GY87, 72, \ + RobotModelType.MODEL_TYPE_4WD_MECANUM) +elif pibotModel == "hera" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(82, 338, 44, 10, \ + 320, 2700, 0, 10, \ + 250, 50, 50, 250, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_4WD_DIFF) +elif pibotModel == "apolloX" and boardType == "arduino": + pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \ + 75, 2500, 0, 10, \ + 250, 40, 0, 200, \ + RobotImuType.IMU_TYPE_GY85, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) +elif pibotModel == "apolloX" and boardType == "stm32f1": + pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \ + 250, 1200, 0, 10, \ + 250, 50, 0, 200, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) +elif pibotModel == "apolloX" and boardType == "stm32f4": + pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \ + 250, 1200, 0, 10, \ + 250, 50, 0, 200, \ + RobotImuType.IMU_TYPE_GY87, 90, \ + RobotModelType.MODEL_TYPE_2WD_DIFF) \ No newline at end of file diff --git a/Hardware/UPbot-Tools/pb.py b/Hardware/UPbot-Tools/pb.py new file mode 100644 index 0000000..d989af2 --- /dev/null +++ b/Hardware/UPbot-Tools/pb.py @@ -0,0 +1,749 @@ +# -*- coding: utf-8 -*- + +# Form implementation generated from reading ui file 'pb.ui' +# +# Created by: PyQt5 UI code generator 5.15.9 +# +# WARNING: Any manual changes made to this file will be lost when pyuic5 is +# run again. Do not edit this file unless you know what you are doing. + + +from PyQt5 import QtCore, QtGui, QtWidgets + + +class Ui_pb(object): + def setupUi(self, pb): + pb.setObjectName("pb") + pb.resize(866, 542) + self.gridLayout_2 = QtWidgets.QGridLayout(pb) + self.gridLayout_2.setObjectName("gridLayout_2") + self.line_5 = QtWidgets.QFrame(pb) + self.line_5.setFrameShape(QtWidgets.QFrame.HLine) + self.line_5.setFrameShadow(QtWidgets.QFrame.Sunken) + self.line_5.setObjectName("line_5") + self.gridLayout_2.addWidget(self.line_5, 0, 0, 1, 1) + self.tabWidget = QtWidgets.QTabWidget(pb) + self.tabWidget.setObjectName("tabWidget") + self.tab_3 = QtWidgets.QWidget() + self.tab_3.setObjectName("tab_3") + self.verticalLayout_3 = QtWidgets.QVBoxLayout(self.tab_3) + self.verticalLayout_3.setObjectName("verticalLayout_3") + self.horizontalLayout_25 = QtWidgets.QHBoxLayout() + self.horizontalLayout_25.setObjectName("horizontalLayout_25") + self.horizontalLayout_5 = QtWidgets.QHBoxLayout() + self.horizontalLayout_5.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_5.setObjectName("horizontalLayout_5") + self.label_2 = QtWidgets.QLabel(self.tab_3) + self.label_2.setObjectName("label_2") + self.horizontalLayout_5.addWidget(self.label_2) + spacerItem = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_5.addItem(spacerItem) + self.combox_model = QtWidgets.QComboBox(self.tab_3) + self.combox_model.setObjectName("combox_model") + self.horizontalLayout_5.addWidget(self.combox_model) + spacerItem1 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_5.addItem(spacerItem1) + self.horizontalLayout_5.setStretch(1, 1) + self.horizontalLayout_5.setStretch(3, 8) + self.horizontalLayout_25.addLayout(self.horizontalLayout_5) + self.horizontalLayout_6 = QtWidgets.QHBoxLayout() + self.horizontalLayout_6.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_6.setObjectName("horizontalLayout_6") + self.label_3 = QtWidgets.QLabel(self.tab_3) + self.label_3.setObjectName("label_3") + self.horizontalLayout_6.addWidget(self.label_3) + spacerItem2 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_6.addItem(spacerItem2) + self.combox_imu_type = QtWidgets.QComboBox(self.tab_3) + self.combox_imu_type.setObjectName("combox_imu_type") + self.horizontalLayout_6.addWidget(self.combox_imu_type) + spacerItem3 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_6.addItem(spacerItem3) + self.horizontalLayout_6.setStretch(1, 1) + self.horizontalLayout_6.setStretch(2, 8) + self.horizontalLayout_6.setStretch(3, 8) + self.horizontalLayout_25.addLayout(self.horizontalLayout_6) + self.verticalLayout_3.addLayout(self.horizontalLayout_25) + self.line_2 = QtWidgets.QFrame(self.tab_3) + self.line_2.setFrameShape(QtWidgets.QFrame.HLine) + self.line_2.setFrameShadow(QtWidgets.QFrame.Sunken) + self.line_2.setObjectName("line_2") + self.verticalLayout_3.addWidget(self.line_2) + self.verticalLayout_2 = QtWidgets.QVBoxLayout() + self.verticalLayout_2.setObjectName("verticalLayout_2") + self.gridLayout = QtWidgets.QGridLayout() + self.gridLayout.setObjectName("gridLayout") + self.horizontalLayout = QtWidgets.QHBoxLayout() + self.horizontalLayout.setContentsMargins(10, -1, 10, -1) + self.horizontalLayout.setObjectName("horizontalLayout") + self.label_6 = QtWidgets.QLabel(self.tab_3) + self.label_6.setObjectName("label_6") + self.horizontalLayout.addWidget(self.label_6) + self.slider_motor_ratio = QtWidgets.QSlider(self.tab_3) + self.slider_motor_ratio.setOrientation(QtCore.Qt.Horizontal) + self.slider_motor_ratio.setObjectName("slider_motor_ratio") + self.horizontalLayout.addWidget(self.slider_motor_ratio) + self.label_motor_ratio = QtWidgets.QLabel(self.tab_3) + self.label_motor_ratio.setObjectName("label_motor_ratio") + self.horizontalLayout.addWidget(self.label_motor_ratio) + self.gridLayout.addLayout(self.horizontalLayout, 1, 1, 1, 1) + self.horizontalLayout_3 = QtWidgets.QHBoxLayout() + self.horizontalLayout_3.setContentsMargins(10, -1, 10, -1) + self.horizontalLayout_3.setObjectName("horizontalLayout_3") + self.label = QtWidgets.QLabel(self.tab_3) + self.label.setObjectName("label") + self.horizontalLayout_3.addWidget(self.label) + self.slider_wheel_diameter = QtWidgets.QSlider(self.tab_3) + self.slider_wheel_diameter.setOrientation(QtCore.Qt.Horizontal) + self.slider_wheel_diameter.setObjectName("slider_wheel_diameter") + self.horizontalLayout_3.addWidget(self.slider_wheel_diameter) + self.label_wheel_diameter = QtWidgets.QLabel(self.tab_3) + self.label_wheel_diameter.setObjectName("label_wheel_diameter") + self.horizontalLayout_3.addWidget(self.label_wheel_diameter) + self.horizontalLayout_3.setStretch(1, 16) + self.gridLayout.addLayout(self.horizontalLayout_3, 0, 0, 1, 1) + self.horizontalLayout_4 = QtWidgets.QHBoxLayout() + self.horizontalLayout_4.setContentsMargins(10, -1, 10, -1) + self.horizontalLayout_4.setObjectName("horizontalLayout_4") + self.label_4 = QtWidgets.QLabel(self.tab_3) + self.label_4.setObjectName("label_4") + self.horizontalLayout_4.addWidget(self.label_4) + self.slider_encoder = QtWidgets.QSlider(self.tab_3) + self.slider_encoder.setOrientation(QtCore.Qt.Horizontal) + self.slider_encoder.setObjectName("slider_encoder") + self.horizontalLayout_4.addWidget(self.slider_encoder) + self.label_encoder_res = QtWidgets.QLabel(self.tab_3) + self.label_encoder_res.setObjectName("label_encoder_res") + self.horizontalLayout_4.addWidget(self.label_encoder_res) + self.horizontalLayout_4.setStretch(1, 16) + self.gridLayout.addLayout(self.horizontalLayout_4, 1, 0, 1, 1) + self.horizontalLayout_2 = QtWidgets.QHBoxLayout() + self.horizontalLayout_2.setContentsMargins(10, -1, 10, -1) + self.horizontalLayout_2.setObjectName("horizontalLayout_2") + self.label_5 = QtWidgets.QLabel(self.tab_3) + self.label_5.setObjectName("label_5") + self.horizontalLayout_2.addWidget(self.label_5) + self.slider_wheel_track = QtWidgets.QSlider(self.tab_3) + self.slider_wheel_track.setOrientation(QtCore.Qt.Horizontal) + self.slider_wheel_track.setObjectName("slider_wheel_track") + self.horizontalLayout_2.addWidget(self.slider_wheel_track) + self.label_wheel_track = QtWidgets.QLabel(self.tab_3) + self.label_wheel_track.setObjectName("label_wheel_track") + self.horizontalLayout_2.addWidget(self.label_wheel_track) + self.gridLayout.addLayout(self.horizontalLayout_2, 0, 1, 1, 1) + self.verticalLayout_2.addLayout(self.gridLayout) + self.verticalLayout_3.addLayout(self.verticalLayout_2) + self.line_3 = QtWidgets.QFrame(self.tab_3) + self.line_3.setFrameShape(QtWidgets.QFrame.HLine) + self.line_3.setFrameShadow(QtWidgets.QFrame.Sunken) + self.line_3.setObjectName("line_3") + self.verticalLayout_3.addWidget(self.line_3) + self.verticalLayout = QtWidgets.QVBoxLayout() + self.verticalLayout.setObjectName("verticalLayout") + self.groupBox = QtWidgets.QGroupBox(self.tab_3) + self.groupBox.setAutoFillBackground(False) + self.groupBox.setFlat(False) + self.groupBox.setObjectName("groupBox") + self.verticalLayout_4 = QtWidgets.QVBoxLayout(self.groupBox) + self.verticalLayout_4.setObjectName("verticalLayout_4") + self.horizontalLayout_26 = QtWidgets.QHBoxLayout() + self.horizontalLayout_26.setObjectName("horizontalLayout_26") + self.gridLayout_4 = QtWidgets.QGridLayout() + self.gridLayout_4.setContentsMargins(5, -1, -1, -1) + self.gridLayout_4.setObjectName("gridLayout_4") + self.checkBox_motor1 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_motor1.setObjectName("checkBox_motor1") + self.gridLayout_4.addWidget(self.checkBox_motor1, 0, 0, 1, 1) + self.checkBox_motor2 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_motor2.setObjectName("checkBox_motor2") + self.gridLayout_4.addWidget(self.checkBox_motor2, 0, 1, 1, 1) + self.checkBox_motor3 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_motor3.setObjectName("checkBox_motor3") + self.gridLayout_4.addWidget(self.checkBox_motor3, 0, 2, 1, 1) + self.checkBox_motor4 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_motor4.setObjectName("checkBox_motor4") + self.gridLayout_4.addWidget(self.checkBox_motor4, 0, 3, 1, 1) + self.checkBox_encoder1 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_encoder1.setObjectName("checkBox_encoder1") + self.gridLayout_4.addWidget(self.checkBox_encoder1, 0, 4, 1, 1) + self.checkBox_encoder2 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_encoder2.setObjectName("checkBox_encoder2") + self.gridLayout_4.addWidget(self.checkBox_encoder2, 0, 5, 1, 1) + self.checkBox_encoder3 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_encoder3.setObjectName("checkBox_encoder3") + self.gridLayout_4.addWidget(self.checkBox_encoder3, 0, 6, 1, 1) + self.checkBox_encoder4 = QtWidgets.QCheckBox(self.groupBox) + self.checkBox_encoder4.setObjectName("checkBox_encoder4") + self.gridLayout_4.addWidget(self.checkBox_encoder4, 0, 7, 1, 1) + self.horizontalLayout_26.addLayout(self.gridLayout_4) + self.verticalLayout_4.addLayout(self.horizontalLayout_26) + self.verticalLayout.addWidget(self.groupBox) + self.verticalLayout_3.addLayout(self.verticalLayout) + self.horizontalLayout_24 = QtWidgets.QHBoxLayout() + self.horizontalLayout_24.setObjectName("horizontalLayout_24") + self.groupBox_3 = QtWidgets.QGroupBox(self.tab_3) + self.groupBox_3.setObjectName("groupBox_3") + self.gridLayout_5 = QtWidgets.QGridLayout(self.groupBox_3) + self.gridLayout_5.setObjectName("gridLayout_5") + self.horizontalLayout_20 = QtWidgets.QHBoxLayout() + self.horizontalLayout_20.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_20.setObjectName("horizontalLayout_20") + self.label_16 = QtWidgets.QLabel(self.groupBox_3) + self.label_16.setObjectName("label_16") + self.horizontalLayout_20.addWidget(self.label_16) + self.slider_ki = QtWidgets.QSlider(self.groupBox_3) + self.slider_ki.setOrientation(QtCore.Qt.Horizontal) + self.slider_ki.setObjectName("slider_ki") + self.horizontalLayout_20.addWidget(self.slider_ki) + self.label_ki = QtWidgets.QLabel(self.groupBox_3) + self.label_ki.setObjectName("label_ki") + self.horizontalLayout_20.addWidget(self.label_ki) + self.gridLayout_5.addLayout(self.horizontalLayout_20, 2, 0, 1, 1) + self.horizontalLayout_21 = QtWidgets.QHBoxLayout() + self.horizontalLayout_21.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_21.setObjectName("horizontalLayout_21") + self.label_17 = QtWidgets.QLabel(self.groupBox_3) + self.label_17.setObjectName("label_17") + self.horizontalLayout_21.addWidget(self.label_17) + self.slider_kd = QtWidgets.QSlider(self.groupBox_3) + self.slider_kd.setOrientation(QtCore.Qt.Horizontal) + self.slider_kd.setObjectName("slider_kd") + self.horizontalLayout_21.addWidget(self.slider_kd) + self.label_kd = QtWidgets.QLabel(self.groupBox_3) + self.label_kd.setObjectName("label_kd") + self.horizontalLayout_21.addWidget(self.label_kd) + self.gridLayout_5.addLayout(self.horizontalLayout_21, 3, 0, 1, 1) + self.horizontalLayout_22 = QtWidgets.QHBoxLayout() + self.horizontalLayout_22.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_22.setObjectName("horizontalLayout_22") + self.label_15 = QtWidgets.QLabel(self.groupBox_3) + self.label_15.setObjectName("label_15") + self.horizontalLayout_22.addWidget(self.label_15) + self.slider_kp = QtWidgets.QSlider(self.groupBox_3) + self.slider_kp.setOrientation(QtCore.Qt.Horizontal) + self.slider_kp.setObjectName("slider_kp") + self.horizontalLayout_22.addWidget(self.slider_kp) + self.label_kp = QtWidgets.QLabel(self.groupBox_3) + self.label_kp.setObjectName("label_kp") + self.horizontalLayout_22.addWidget(self.label_kp) + self.gridLayout_5.addLayout(self.horizontalLayout_22, 1, 0, 1, 1) + self.horizontalLayout_23 = QtWidgets.QHBoxLayout() + self.horizontalLayout_23.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_23.setObjectName("horizontalLayout_23") + self.label_18 = QtWidgets.QLabel(self.groupBox_3) + self.label_18.setObjectName("label_18") + self.horizontalLayout_23.addWidget(self.label_18) + self.slider_ko = QtWidgets.QSlider(self.groupBox_3) + self.slider_ko.setOrientation(QtCore.Qt.Horizontal) + self.slider_ko.setObjectName("slider_ko") + self.horizontalLayout_23.addWidget(self.slider_ko) + self.label_ko = QtWidgets.QLabel(self.groupBox_3) + self.label_ko.setObjectName("label_ko") + self.horizontalLayout_23.addWidget(self.label_ko) + self.gridLayout_5.addLayout(self.horizontalLayout_23, 4, 0, 1, 1) + self.horizontalLayout_15 = QtWidgets.QHBoxLayout() + self.horizontalLayout_15.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_15.setObjectName("horizontalLayout_15") + self.label_19 = QtWidgets.QLabel(self.groupBox_3) + self.label_19.setObjectName("label_19") + self.horizontalLayout_15.addWidget(self.label_19) + self.slider_pid_interval = QtWidgets.QSlider(self.groupBox_3) + self.slider_pid_interval.setOrientation(QtCore.Qt.Horizontal) + self.slider_pid_interval.setObjectName("slider_pid_interval") + self.horizontalLayout_15.addWidget(self.slider_pid_interval) + self.label_pid_interval = QtWidgets.QLabel(self.groupBox_3) + self.label_pid_interval.setObjectName("label_pid_interval") + self.horizontalLayout_15.addWidget(self.label_pid_interval) + self.gridLayout_5.addLayout(self.horizontalLayout_15, 0, 0, 1, 1) + self.horizontalLayout_24.addWidget(self.groupBox_3) + self.groupBox_2 = QtWidgets.QGroupBox(self.tab_3) + self.groupBox_2.setObjectName("groupBox_2") + self.gridLayout_3 = QtWidgets.QGridLayout(self.groupBox_2) + self.gridLayout_3.setObjectName("gridLayout_3") + self.horizontalLayout_17 = QtWidgets.QHBoxLayout() + self.horizontalLayout_17.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_17.setObjectName("horizontalLayout_17") + self.label_22 = QtWidgets.QLabel(self.groupBox_2) + self.label_22.setObjectName("label_22") + self.horizontalLayout_17.addWidget(self.label_22) + self.slider_vy_max = QtWidgets.QSlider(self.groupBox_2) + self.slider_vy_max.setOrientation(QtCore.Qt.Horizontal) + self.slider_vy_max.setObjectName("slider_vy_max") + self.horizontalLayout_17.addWidget(self.slider_vy_max) + self.label_vy_max = QtWidgets.QLabel(self.groupBox_2) + self.label_vy_max.setObjectName("label_vy_max") + self.horizontalLayout_17.addWidget(self.label_vy_max) + self.gridLayout_3.addLayout(self.horizontalLayout_17, 3, 0, 1, 1) + self.horizontalLayout_18 = QtWidgets.QHBoxLayout() + self.horizontalLayout_18.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_18.setObjectName("horizontalLayout_18") + self.label_21 = QtWidgets.QLabel(self.groupBox_2) + self.label_21.setObjectName("label_21") + self.horizontalLayout_18.addWidget(self.label_21) + self.slider_vx_max = QtWidgets.QSlider(self.groupBox_2) + self.slider_vx_max.setOrientation(QtCore.Qt.Horizontal) + self.slider_vx_max.setObjectName("slider_vx_max") + self.horizontalLayout_18.addWidget(self.slider_vx_max) + self.label_vx_max = QtWidgets.QLabel(self.groupBox_2) + self.label_vx_max.setObjectName("label_vx_max") + self.horizontalLayout_18.addWidget(self.label_vx_max) + self.gridLayout_3.addLayout(self.horizontalLayout_18, 2, 0, 1, 1) + self.horizontalLayout_19 = QtWidgets.QHBoxLayout() + self.horizontalLayout_19.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_19.setObjectName("horizontalLayout_19") + self.label_23 = QtWidgets.QLabel(self.groupBox_2) + self.label_23.setObjectName("label_23") + self.horizontalLayout_19.addWidget(self.label_23) + self.slider_va_max = QtWidgets.QSlider(self.groupBox_2) + self.slider_va_max.setOrientation(QtCore.Qt.Horizontal) + self.slider_va_max.setObjectName("slider_va_max") + self.horizontalLayout_19.addWidget(self.slider_va_max) + self.label_va_max = QtWidgets.QLabel(self.groupBox_2) + self.label_va_max.setObjectName("label_va_max") + self.horizontalLayout_19.addWidget(self.label_va_max) + self.gridLayout_3.addLayout(self.horizontalLayout_19, 4, 0, 1, 1) + self.horizontalLayout_16 = QtWidgets.QHBoxLayout() + self.horizontalLayout_16.setContentsMargins(10, -1, -1, -1) + self.horizontalLayout_16.setObjectName("horizontalLayout_16") + self.label_20 = QtWidgets.QLabel(self.groupBox_2) + self.label_20.setObjectName("label_20") + self.horizontalLayout_16.addWidget(self.label_20) + self.slider_cmd_lasttime = QtWidgets.QSlider(self.groupBox_2) + self.slider_cmd_lasttime.setOrientation(QtCore.Qt.Horizontal) + self.slider_cmd_lasttime.setObjectName("slider_cmd_lasttime") + self.horizontalLayout_16.addWidget(self.slider_cmd_lasttime) + self.label_cmd_lasttime = QtWidgets.QLabel(self.groupBox_2) + self.label_cmd_lasttime.setObjectName("label_cmd_lasttime") + self.horizontalLayout_16.addWidget(self.label_cmd_lasttime) + self.gridLayout_3.addLayout(self.horizontalLayout_16, 1, 0, 1, 1) + self.horizontalLayout_24.addWidget(self.groupBox_2) + self.verticalLayout_3.addLayout(self.horizontalLayout_24) + self.line_4 = QtWidgets.QFrame(self.tab_3) + self.line_4.setFrameShape(QtWidgets.QFrame.HLine) + self.line_4.setFrameShadow(QtWidgets.QFrame.Sunken) + self.line_4.setObjectName("line_4") + self.verticalLayout_3.addWidget(self.line_4) + self.horizontalLayout_29 = QtWidgets.QHBoxLayout() + self.horizontalLayout_29.setObjectName("horizontalLayout_29") + spacerItem4 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_29.addItem(spacerItem4) + self.comboBox_support_model = QtWidgets.QComboBox(self.tab_3) + self.comboBox_support_model.setObjectName("comboBox_support_model") + self.horizontalLayout_29.addWidget(self.comboBox_support_model) + spacerItem5 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_29.addItem(spacerItem5) + self.pushButton_load = QtWidgets.QPushButton(self.tab_3) + self.pushButton_load.setObjectName("pushButton_load") + self.horizontalLayout_29.addWidget(self.pushButton_load) + spacerItem6 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_29.addItem(spacerItem6) + self.pushButton_set = QtWidgets.QPushButton(self.tab_3) + self.pushButton_set.setObjectName("pushButton_set") + self.horizontalLayout_29.addWidget(self.pushButton_set) + spacerItem7 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_29.addItem(spacerItem7) + self.pushButton_read = QtWidgets.QPushButton(self.tab_3) + self.pushButton_read.setObjectName("pushButton_read") + self.horizontalLayout_29.addWidget(self.pushButton_read) + spacerItem8 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_29.addItem(spacerItem8) + self.horizontalLayout_29.setStretch(0, 20) + self.horizontalLayout_29.setStretch(1, 3) + self.horizontalLayout_29.setStretch(2, 1) + self.horizontalLayout_29.setStretch(3, 3) + self.horizontalLayout_29.setStretch(4, 1) + self.horizontalLayout_29.setStretch(5, 3) + self.horizontalLayout_29.setStretch(6, 1) + self.horizontalLayout_29.setStretch(7, 3) + self.horizontalLayout_29.setStretch(8, 1) + self.verticalLayout_3.addLayout(self.horizontalLayout_29) + self.tabWidget.addTab(self.tab_3, "") + self.tab_4 = QtWidgets.QWidget() + self.tab_4.setObjectName("tab_4") + self.verticalLayout_6 = QtWidgets.QVBoxLayout(self.tab_4) + self.verticalLayout_6.setObjectName("verticalLayout_6") + self.groupBox_4 = QtWidgets.QGroupBox(self.tab_4) + self.groupBox_4.setObjectName("groupBox_4") + self.verticalLayout_5 = QtWidgets.QVBoxLayout(self.groupBox_4) + self.verticalLayout_5.setObjectName("verticalLayout_5") + self.horizontalLayout_7 = QtWidgets.QHBoxLayout() + self.horizontalLayout_7.setObjectName("horizontalLayout_7") + self.label_7 = QtWidgets.QLabel(self.groupBox_4) + self.label_7.setAlignment(QtCore.Qt.AlignCenter) + self.label_7.setObjectName("label_7") + self.horizontalLayout_7.addWidget(self.label_7) + self.slider_set_pwm1 = QtWidgets.QSlider(self.groupBox_4) + self.slider_set_pwm1.setOrientation(QtCore.Qt.Horizontal) + self.slider_set_pwm1.setObjectName("slider_set_pwm1") + self.horizontalLayout_7.addWidget(self.slider_set_pwm1) + self.label_set_pwm1 = QtWidgets.QLabel(self.groupBox_4) + self.label_set_pwm1.setAlignment(QtCore.Qt.AlignCenter) + self.label_set_pwm1.setObjectName("label_set_pwm1") + self.horizontalLayout_7.addWidget(self.label_set_pwm1) + spacerItem9 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_7.addItem(spacerItem9) + self.label_10 = QtWidgets.QLabel(self.groupBox_4) + self.label_10.setObjectName("label_10") + self.horizontalLayout_7.addWidget(self.label_10) + self.label_feedback1 = QtWidgets.QLabel(self.groupBox_4) + self.label_feedback1.setAlignment(QtCore.Qt.AlignCenter) + self.label_feedback1.setObjectName("label_feedback1") + self.horizontalLayout_7.addWidget(self.label_feedback1) + self.horizontalLayout_7.setStretch(0, 1) + self.horizontalLayout_7.setStretch(1, 10) + self.horizontalLayout_7.setStretch(2, 1) + self.horizontalLayout_7.setStretch(3, 2) + self.horizontalLayout_7.setStretch(4, 1) + self.horizontalLayout_7.setStretch(5, 1) + self.verticalLayout_5.addLayout(self.horizontalLayout_7) + self.horizontalLayout_8 = QtWidgets.QHBoxLayout() + self.horizontalLayout_8.setObjectName("horizontalLayout_8") + self.label_8 = QtWidgets.QLabel(self.groupBox_4) + self.label_8.setAlignment(QtCore.Qt.AlignCenter) + self.label_8.setObjectName("label_8") + self.horizontalLayout_8.addWidget(self.label_8) + self.slider_set_pwm2 = QtWidgets.QSlider(self.groupBox_4) + self.slider_set_pwm2.setOrientation(QtCore.Qt.Horizontal) + self.slider_set_pwm2.setObjectName("slider_set_pwm2") + self.horizontalLayout_8.addWidget(self.slider_set_pwm2) + self.label_set_pwm2 = QtWidgets.QLabel(self.groupBox_4) + self.label_set_pwm2.setAlignment(QtCore.Qt.AlignCenter) + self.label_set_pwm2.setObjectName("label_set_pwm2") + self.horizontalLayout_8.addWidget(self.label_set_pwm2) + spacerItem10 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_8.addItem(spacerItem10) + self.label_11 = QtWidgets.QLabel(self.groupBox_4) + self.label_11.setObjectName("label_11") + self.horizontalLayout_8.addWidget(self.label_11) + self.label_feedback2 = QtWidgets.QLabel(self.groupBox_4) + self.label_feedback2.setAlignment(QtCore.Qt.AlignCenter) + self.label_feedback2.setObjectName("label_feedback2") + self.horizontalLayout_8.addWidget(self.label_feedback2) + self.horizontalLayout_8.setStretch(0, 1) + self.horizontalLayout_8.setStretch(1, 10) + self.horizontalLayout_8.setStretch(2, 1) + self.horizontalLayout_8.setStretch(3, 2) + self.horizontalLayout_8.setStretch(4, 1) + self.horizontalLayout_8.setStretch(5, 1) + self.verticalLayout_5.addLayout(self.horizontalLayout_8) + self.horizontalLayout_9 = QtWidgets.QHBoxLayout() + self.horizontalLayout_9.setObjectName("horizontalLayout_9") + self.label_9 = QtWidgets.QLabel(self.groupBox_4) + self.label_9.setAlignment(QtCore.Qt.AlignCenter) + self.label_9.setObjectName("label_9") + self.horizontalLayout_9.addWidget(self.label_9) + self.slider_set_pwm3 = QtWidgets.QSlider(self.groupBox_4) + self.slider_set_pwm3.setOrientation(QtCore.Qt.Horizontal) + self.slider_set_pwm3.setObjectName("slider_set_pwm3") + self.horizontalLayout_9.addWidget(self.slider_set_pwm3) + self.label_set_pwm3 = QtWidgets.QLabel(self.groupBox_4) + self.label_set_pwm3.setAlignment(QtCore.Qt.AlignCenter) + self.label_set_pwm3.setObjectName("label_set_pwm3") + self.horizontalLayout_9.addWidget(self.label_set_pwm3) + spacerItem11 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_9.addItem(spacerItem11) + self.label_12 = QtWidgets.QLabel(self.groupBox_4) + self.label_12.setObjectName("label_12") + self.horizontalLayout_9.addWidget(self.label_12) + self.label_feedback3 = QtWidgets.QLabel(self.groupBox_4) + self.label_feedback3.setAlignment(QtCore.Qt.AlignCenter) + self.label_feedback3.setObjectName("label_feedback3") + self.horizontalLayout_9.addWidget(self.label_feedback3) + self.horizontalLayout_9.setStretch(0, 1) + self.horizontalLayout_9.setStretch(1, 10) + self.horizontalLayout_9.setStretch(2, 1) + self.horizontalLayout_9.setStretch(3, 2) + self.horizontalLayout_9.setStretch(4, 1) + self.horizontalLayout_9.setStretch(5, 1) + self.verticalLayout_5.addLayout(self.horizontalLayout_9) + self.horizontalLayout_10 = QtWidgets.QHBoxLayout() + self.horizontalLayout_10.setObjectName("horizontalLayout_10") + self.label_13 = QtWidgets.QLabel(self.groupBox_4) + self.label_13.setAlignment(QtCore.Qt.AlignCenter) + self.label_13.setObjectName("label_13") + self.horizontalLayout_10.addWidget(self.label_13) + self.slider_set_pwm4 = QtWidgets.QSlider(self.groupBox_4) + self.slider_set_pwm4.setOrientation(QtCore.Qt.Horizontal) + self.slider_set_pwm4.setObjectName("slider_set_pwm4") + self.horizontalLayout_10.addWidget(self.slider_set_pwm4) + self.label_set_pwm4 = QtWidgets.QLabel(self.groupBox_4) + self.label_set_pwm4.setAlignment(QtCore.Qt.AlignCenter) + self.label_set_pwm4.setObjectName("label_set_pwm4") + self.horizontalLayout_10.addWidget(self.label_set_pwm4) + spacerItem12 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_10.addItem(spacerItem12) + self.label_14 = QtWidgets.QLabel(self.groupBox_4) + self.label_14.setObjectName("label_14") + self.horizontalLayout_10.addWidget(self.label_14) + self.label_feedback4 = QtWidgets.QLabel(self.groupBox_4) + self.label_feedback4.setAlignment(QtCore.Qt.AlignCenter) + self.label_feedback4.setObjectName("label_feedback4") + self.horizontalLayout_10.addWidget(self.label_feedback4) + self.horizontalLayout_10.setStretch(1, 10) + self.horizontalLayout_10.setStretch(2, 1) + self.horizontalLayout_10.setStretch(3, 2) + self.horizontalLayout_10.setStretch(4, 1) + self.horizontalLayout_10.setStretch(5, 1) + self.verticalLayout_5.addLayout(self.horizontalLayout_10) + self.horizontalLayout_11 = QtWidgets.QHBoxLayout() + self.horizontalLayout_11.setObjectName("horizontalLayout_11") + spacerItem13 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_11.addItem(spacerItem13) + self.pushButton_start = QtWidgets.QPushButton(self.groupBox_4) + self.pushButton_start.setObjectName("pushButton_start") + self.horizontalLayout_11.addWidget(self.pushButton_start) + self.pushButton_stop = QtWidgets.QPushButton(self.groupBox_4) + self.pushButton_stop.setObjectName("pushButton_stop") + self.horizontalLayout_11.addWidget(self.pushButton_stop) + self.verticalLayout_5.addLayout(self.horizontalLayout_11) + self.verticalLayout_6.addWidget(self.groupBox_4) + self.groupBox_6 = QtWidgets.QGroupBox(self.tab_4) + self.groupBox_6.setObjectName("groupBox_6") + self.verticalLayout_7 = QtWidgets.QVBoxLayout(self.groupBox_6) + self.verticalLayout_7.setObjectName("verticalLayout_7") + self.horizontalLayout_13 = QtWidgets.QHBoxLayout() + self.horizontalLayout_13.setObjectName("horizontalLayout_13") + self.label_25 = QtWidgets.QLabel(self.groupBox_6) + self.label_25.setAlignment(QtCore.Qt.AlignCenter) + self.label_25.setObjectName("label_25") + self.horizontalLayout_13.addWidget(self.label_25) + self.label_input_1 = QtWidgets.QLabel(self.groupBox_6) + self.label_input_1.setAlignment(QtCore.Qt.AlignCenter) + self.label_input_1.setObjectName("label_input_1") + self.horizontalLayout_13.addWidget(self.label_input_1) + self.label_input_2 = QtWidgets.QLabel(self.groupBox_6) + self.label_input_2.setAlignment(QtCore.Qt.AlignCenter) + self.label_input_2.setObjectName("label_input_2") + self.horizontalLayout_13.addWidget(self.label_input_2) + self.label_input_3 = QtWidgets.QLabel(self.groupBox_6) + self.label_input_3.setAlignment(QtCore.Qt.AlignCenter) + self.label_input_3.setObjectName("label_input_3") + self.horizontalLayout_13.addWidget(self.label_input_3) + self.label_input_4 = QtWidgets.QLabel(self.groupBox_6) + self.label_input_4.setAlignment(QtCore.Qt.AlignCenter) + self.label_input_4.setObjectName("label_input_4") + self.horizontalLayout_13.addWidget(self.label_input_4) + self.verticalLayout_7.addLayout(self.horizontalLayout_13) + self.horizontalLayout_14 = QtWidgets.QHBoxLayout() + self.horizontalLayout_14.setObjectName("horizontalLayout_14") + self.label_27 = QtWidgets.QLabel(self.groupBox_6) + self.label_27.setAlignment(QtCore.Qt.AlignCenter) + self.label_27.setObjectName("label_27") + self.horizontalLayout_14.addWidget(self.label_27) + self.label_output_1 = QtWidgets.QLabel(self.groupBox_6) + self.label_output_1.setAlignment(QtCore.Qt.AlignCenter) + self.label_output_1.setObjectName("label_output_1") + self.horizontalLayout_14.addWidget(self.label_output_1) + self.label_output_2 = QtWidgets.QLabel(self.groupBox_6) + self.label_output_2.setAlignment(QtCore.Qt.AlignCenter) + self.label_output_2.setObjectName("label_output_2") + self.horizontalLayout_14.addWidget(self.label_output_2) + self.label_output_3 = QtWidgets.QLabel(self.groupBox_6) + self.label_output_3.setAlignment(QtCore.Qt.AlignCenter) + self.label_output_3.setObjectName("label_output_3") + self.horizontalLayout_14.addWidget(self.label_output_3) + self.label_output_4 = QtWidgets.QLabel(self.groupBox_6) + self.label_output_4.setAlignment(QtCore.Qt.AlignCenter) + self.label_output_4.setObjectName("label_output_4") + self.horizontalLayout_14.addWidget(self.label_output_4) + self.verticalLayout_7.addLayout(self.horizontalLayout_14) + self.horizontalLayout_27 = QtWidgets.QHBoxLayout() + self.horizontalLayout_27.setObjectName("horizontalLayout_27") + self.label_29 = QtWidgets.QLabel(self.groupBox_6) + self.label_29.setAlignment(QtCore.Qt.AlignCenter) + self.label_29.setObjectName("label_29") + self.horizontalLayout_27.addWidget(self.label_29) + self.label_30 = QtWidgets.QLabel(self.groupBox_6) + self.label_30.setAlignment(QtCore.Qt.AlignCenter) + self.label_30.setObjectName("label_30") + self.horizontalLayout_27.addWidget(self.label_30) + self.label_31 = QtWidgets.QLabel(self.groupBox_6) + self.label_31.setAlignment(QtCore.Qt.AlignCenter) + self.label_31.setObjectName("label_31") + self.horizontalLayout_27.addWidget(self.label_31) + self.pushButton_start_2 = QtWidgets.QPushButton(self.groupBox_6) + self.pushButton_start_2.setObjectName("pushButton_start_2") + self.horizontalLayout_27.addWidget(self.pushButton_start_2) + self.pushButton_stop_2 = QtWidgets.QPushButton(self.groupBox_6) + self.pushButton_stop_2.setObjectName("pushButton_stop_2") + self.horizontalLayout_27.addWidget(self.pushButton_stop_2) + self.verticalLayout_7.addLayout(self.horizontalLayout_27) + self.verticalLayout_6.addWidget(self.groupBox_6) + self.groupBox_5 = QtWidgets.QGroupBox(self.tab_4) + self.groupBox_5.setObjectName("groupBox_5") + self.horizontalLayout_12 = QtWidgets.QHBoxLayout(self.groupBox_5) + self.horizontalLayout_12.setObjectName("horizontalLayout_12") + self.gridLayout_6 = QtWidgets.QGridLayout() + self.gridLayout_6.setObjectName("gridLayout_6") + self.label_28 = QtWidgets.QLabel(self.groupBox_5) + self.label_28.setAlignment(QtCore.Qt.AlignCenter) + self.label_28.setObjectName("label_28") + self.gridLayout_6.addWidget(self.label_28, 2, 0, 1, 1) + self.label_magn_y = QtWidgets.QLabel(self.groupBox_5) + self.label_magn_y.setAlignment(QtCore.Qt.AlignCenter) + self.label_magn_y.setObjectName("label_magn_y") + self.gridLayout_6.addWidget(self.label_magn_y, 2, 2, 1, 1) + self.label_26 = QtWidgets.QLabel(self.groupBox_5) + self.label_26.setAlignment(QtCore.Qt.AlignCenter) + self.label_26.setObjectName("label_26") + self.gridLayout_6.addWidget(self.label_26, 1, 0, 1, 1) + self.label_24 = QtWidgets.QLabel(self.groupBox_5) + self.label_24.setAlignment(QtCore.Qt.AlignCenter) + self.label_24.setObjectName("label_24") + self.gridLayout_6.addWidget(self.label_24, 0, 0, 1, 1) + self.label_acc_z = QtWidgets.QLabel(self.groupBox_5) + self.label_acc_z.setAlignment(QtCore.Qt.AlignCenter) + self.label_acc_z.setObjectName("label_acc_z") + self.gridLayout_6.addWidget(self.label_acc_z, 0, 3, 1, 1) + self.label_acc_y = QtWidgets.QLabel(self.groupBox_5) + self.label_acc_y.setAlignment(QtCore.Qt.AlignCenter) + self.label_acc_y.setObjectName("label_acc_y") + self.gridLayout_6.addWidget(self.label_acc_y, 0, 2, 1, 1) + self.label_gyro_x = QtWidgets.QLabel(self.groupBox_5) + self.label_gyro_x.setAlignment(QtCore.Qt.AlignCenter) + self.label_gyro_x.setObjectName("label_gyro_x") + self.gridLayout_6.addWidget(self.label_gyro_x, 1, 1, 1, 1) + self.label_gyro_z = QtWidgets.QLabel(self.groupBox_5) + self.label_gyro_z.setAlignment(QtCore.Qt.AlignCenter) + self.label_gyro_z.setObjectName("label_gyro_z") + self.gridLayout_6.addWidget(self.label_gyro_z, 1, 3, 1, 1) + self.label_gyro_y = QtWidgets.QLabel(self.groupBox_5) + self.label_gyro_y.setAlignment(QtCore.Qt.AlignCenter) + self.label_gyro_y.setObjectName("label_gyro_y") + self.gridLayout_6.addWidget(self.label_gyro_y, 1, 2, 1, 1) + self.label_magn_z = QtWidgets.QLabel(self.groupBox_5) + self.label_magn_z.setAlignment(QtCore.Qt.AlignCenter) + self.label_magn_z.setObjectName("label_magn_z") + self.gridLayout_6.addWidget(self.label_magn_z, 2, 3, 1, 1) + self.label_acc_x = QtWidgets.QLabel(self.groupBox_5) + self.label_acc_x.setAlignment(QtCore.Qt.AlignCenter) + self.label_acc_x.setObjectName("label_acc_x") + self.gridLayout_6.addWidget(self.label_acc_x, 0, 1, 1, 1) + self.label_magn_x = QtWidgets.QLabel(self.groupBox_5) + self.label_magn_x.setAlignment(QtCore.Qt.AlignCenter) + self.label_magn_x.setObjectName("label_magn_x") + self.gridLayout_6.addWidget(self.label_magn_x, 2, 1, 1, 1) + self.horizontalLayout_12.addLayout(self.gridLayout_6) + spacerItem14 = QtWidgets.QSpacerItem(615, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.horizontalLayout_12.addItem(spacerItem14) + self.verticalLayout_6.addWidget(self.groupBox_5) + self.tabWidget.addTab(self.tab_4, "") + self.gridLayout_2.addWidget(self.tabWidget, 1, 0, 1, 1) + self.gridLayout_2.setRowStretch(0, 1) + + self.retranslateUi(pb) + self.tabWidget.setCurrentIndex(1) + self.slider_wheel_diameter.valueChanged['int'].connect(self.label_wheel_diameter.setNum) # type: ignore + self.slider_wheel_track.valueChanged['int'].connect(self.label_wheel_track.setNum) # type: ignore + self.slider_encoder.valueChanged['int'].connect(self.label_encoder_res.setNum) # type: ignore + self.slider_motor_ratio.valueChanged['int'].connect(self.label_motor_ratio.setNum) # type: ignore + self.slider_pid_interval.valueChanged['int'].connect(self.label_pid_interval.setNum) # type: ignore + self.slider_kd.valueChanged['int'].connect(self.label_kd.setNum) # type: ignore + self.slider_kp.valueChanged['int'].connect(self.label_kp.setNum) # type: ignore + self.slider_ko.valueChanged['int'].connect(self.label_ko.setNum) # type: ignore + self.slider_ki.valueChanged['int'].connect(self.label_ki.setNum) # type: ignore + self.slider_cmd_lasttime.valueChanged['int'].connect(self.label_cmd_lasttime.setNum) # type: ignore + self.slider_va_max.valueChanged['int'].connect(self.label_va_max.setNum) # type: ignore + self.slider_vy_max.valueChanged['int'].connect(self.label_vy_max.setNum) # type: ignore + self.slider_vx_max.valueChanged['int'].connect(self.label_vx_max.setNum) # type: ignore + self.slider_set_pwm1.valueChanged['int'].connect(self.label_set_pwm1.setNum) # type: ignore + self.slider_set_pwm2.valueChanged['int'].connect(self.label_set_pwm2.setNum) # type: ignore + self.slider_set_pwm3.valueChanged['int'].connect(self.label_set_pwm3.setNum) # type: ignore + self.slider_set_pwm4.valueChanged['int'].connect(self.label_set_pwm4.setNum) # type: ignore + QtCore.QMetaObject.connectSlotsByName(pb) + + def retranslateUi(self, pb): + _translate = QtCore.QCoreApplication.translate + pb.setWindowTitle(_translate("pb", "PIBOT Test Tool")) + self.label_2.setText(_translate("pb", "Model Name")) + self.label_3.setText(_translate("pb", "IMU Type")) + self.label_6.setText(_translate("pb", "MotorRatio")) + self.label_motor_ratio.setText(_translate("pb", "0")) + self.label.setText(_translate("pb", "Diameter")) + self.label_wheel_diameter.setText(_translate("pb", "0")) + self.label_4.setText(_translate("pb", "Encoder")) + self.label_encoder_res.setText(_translate("pb", "0")) + self.label_5.setText(_translate("pb", "WheelTrack")) + self.label_wheel_track.setText(_translate("pb", "0")) + self.groupBox.setTitle(_translate("pb", "Reverse Direction Flag")) + self.checkBox_motor1.setText(_translate("pb", "Motor1 ")) + self.checkBox_motor2.setText(_translate("pb", "Motor2")) + self.checkBox_motor3.setText(_translate("pb", "Motor3")) + self.checkBox_motor4.setText(_translate("pb", "Motor4")) + self.checkBox_encoder1.setText(_translate("pb", "Encoder1")) + self.checkBox_encoder2.setText(_translate("pb", "Encoder2")) + self.checkBox_encoder3.setText(_translate("pb", "Encoder3")) + self.checkBox_encoder4.setText(_translate("pb", "Encoder4")) + self.groupBox_3.setTitle(_translate("pb", "PID")) + self.label_16.setText(_translate("pb", "KI")) + self.label_ki.setText(_translate("pb", "0")) + self.label_17.setText(_translate("pb", "KD")) + self.label_kd.setText(_translate("pb", "0")) + self.label_15.setText(_translate("pb", "KP")) + self.label_kp.setText(_translate("pb", "0")) + self.label_18.setText(_translate("pb", "KO")) + self.label_ko.setText(_translate("pb", "0")) + self.label_19.setText(_translate("pb", "Interval")) + self.label_pid_interval.setText(_translate("pb", "0")) + self.groupBox_2.setTitle(_translate("pb", "Speed Limit")) + self.label_22.setText(_translate("pb", "VY")) + self.label_vy_max.setText(_translate("pb", "0")) + self.label_21.setText(_translate("pb", "VX")) + self.label_vx_max.setText(_translate("pb", "0")) + self.label_23.setText(_translate("pb", "VA")) + self.label_va_max.setText(_translate("pb", "0")) + self.label_20.setText(_translate("pb", "CMD Time")) + self.label_cmd_lasttime.setText(_translate("pb", "0")) + self.pushButton_load.setText(_translate("pb", "Load")) + self.pushButton_set.setText(_translate("pb", "Set")) + self.pushButton_read.setText(_translate("pb", "Read")) + self.tabWidget.setTabText(self.tabWidget.indexOf(self.tab_3), _translate("pb", "Tab 1")) + self.groupBox_4.setTitle(_translate("pb", "Motor")) + self.label_7.setText(_translate("pb", "Motor1")) + self.label_set_pwm1.setText(_translate("pb", "0")) + self.label_10.setText(_translate("pb", "Encoder Feedback")) + self.label_feedback1.setText(_translate("pb", "0")) + self.label_8.setText(_translate("pb", "Motor2")) + self.label_set_pwm2.setText(_translate("pb", "0")) + self.label_11.setText(_translate("pb", "Encoder Feedback")) + self.label_feedback2.setText(_translate("pb", "0")) + self.label_9.setText(_translate("pb", "Motor3")) + self.label_set_pwm3.setText(_translate("pb", "0")) + self.label_12.setText(_translate("pb", "Encoder Feedback")) + self.label_feedback3.setText(_translate("pb", "0")) + self.label_13.setText(_translate("pb", "Motor4")) + self.label_set_pwm4.setText(_translate("pb", "0")) + self.label_14.setText(_translate("pb", "Encoder Feedback")) + self.label_feedback4.setText(_translate("pb", "0")) + self.pushButton_start.setText(_translate("pb", "start")) + self.pushButton_stop.setText(_translate("pb", "stop")) + self.groupBox_6.setTitle(_translate("pb", "PID Debug")) + self.label_25.setText(_translate("pb", "Input")) + self.label_input_1.setText(_translate("pb", "0")) + self.label_input_2.setText(_translate("pb", "0")) + self.label_input_3.setText(_translate("pb", "0")) + self.label_input_4.setText(_translate("pb", "0")) + self.label_27.setText(_translate("pb", "Feedback")) + self.label_output_1.setText(_translate("pb", "0")) + self.label_output_2.setText(_translate("pb", "0")) + self.label_output_3.setText(_translate("pb", "0")) + self.label_output_4.setText(_translate("pb", "0")) + self.label_29.setText(_translate("pb", "VY")) + self.label_30.setText(_translate("pb", "VY")) + self.label_31.setText(_translate("pb", "VA")) + self.pushButton_start_2.setText(_translate("pb", "start")) + self.pushButton_stop_2.setText(_translate("pb", "stop")) + self.groupBox_5.setTitle(_translate("pb", "IMU")) + self.label_28.setText(_translate("pb", "MAGN")) + self.label_magn_y.setText(_translate("pb", "0.000")) + self.label_26.setText(_translate("pb", "GYRO")) + self.label_24.setText(_translate("pb", "ACC")) + self.label_acc_z.setText(_translate("pb", "0.000")) + self.label_acc_y.setText(_translate("pb", "0.000")) + self.label_gyro_x.setText(_translate("pb", "0.000")) + self.label_gyro_z.setText(_translate("pb", "0.000")) + self.label_gyro_y.setText(_translate("pb", "0.000")) + self.label_magn_z.setText(_translate("pb", "0.000")) + self.label_acc_x.setText(_translate("pb", "0.000")) + self.label_magn_x.setText(_translate("pb", "0.000")) + self.tabWidget.setTabText(self.tabWidget.indexOf(self.tab_4), _translate("pb", "Tab 2")) diff --git a/Hardware/UPbot-Tools/pb.ui b/Hardware/UPbot-Tools/pb.ui new file mode 100644 index 0000000..cbca814 --- /dev/null +++ b/Hardware/UPbot-Tools/pb.ui @@ -0,0 +1,1613 @@ + + + pb + + + + 0 + 0 + 866 + 542 + + + + UPBot 图形化配置工具 + + + + + + Qt::Horizontal + + + + + + + 1 + + + + Tab 1 + + + + + + + + 10 + + + + + Model Name + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + 10 + + + + + IMU Type + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + Qt::Horizontal + + + + + + + + + + + 10 + + + 10 + + + + + MotorRatio + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + 10 + + + + + Diameter + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + 10 + + + + + Encoder + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + 10 + + + + + WheelTrack + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + + + + + Qt::Horizontal + + + + + + + + + false + + + Reverse Direction Flag + + + false + + + + + + + + 5 + + + + + Motor1 + + + + + + + Motor2 + + + + + + + Motor3 + + + + + + + Motor4 + + + + + + + Encoder1 + + + + + + + Encoder2 + + + + + + + Encoder3 + + + + + + + Encoder4 + + + + + + + + + + + + + + + + + + PID + + + + + + 10 + + + + + KI + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + KD + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + KP + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + KO + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + Interval + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + + + + Speed Limit + + + + + + 10 + + + + + VY + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + VX + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + VA + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + 10 + + + + + CMD Time + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + + + + + + + + + + + Qt::Horizontal + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Load + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Set + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Read + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Tab 2 + + + + + + Motor + + + + + + + + Motor1 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Feedback + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + Motor2 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Feedback + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + Motor3 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Feedback + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + Motor4 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Feedback + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + start + + + + + + + stop + + + + + + + + + + + + PID Debug + + + + + + + + Input + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + Feedback + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + 0 + + + Qt::AlignCenter + + + + + + + + + + + VY + + + Qt::AlignCenter + + + + + + + VY + + + Qt::AlignCenter + + + + + + + VA + + + Qt::AlignCenter + + + + + + + start + + + + + + + stop + + + + + + + + + + + + IMU + + + + + + + + MAGN + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + GYRO + + + Qt::AlignCenter + + + + + + + ACC + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + 0.000 + + + Qt::AlignCenter + + + + + + + + + Qt::Horizontal + + + + 615 + 20 + + + + + + + + + + + + + + + + + slider_wheel_diameter + valueChanged(int) + label_wheel_diameter + setNum(int) + + + 218 + 98 + + + 395 + 98 + + + + + slider_wheel_track + valueChanged(int) + label_wheel_track + setNum(int) + + + 651 + 98 + + + 820 + 98 + + + + + slider_encoder + valueChanged(int) + label_encoder_res + setNum(int) + + + 214 + 142 + + + 395 + 142 + + + + + slider_motor_ratio + valueChanged(int) + label_motor_ratio + setNum(int) + + + 651 + 142 + + + 820 + 142 + + + + + slider_pid_interval + valueChanged(int) + label_pid_interval + setNum(int) + + + 246 + 330 + + + 412 + 330 + + + + + slider_kd + valueChanged(int) + label_kd + setNum(int) + + + 230 + 418 + + + 412 + 418 + + + + + slider_kp + valueChanged(int) + label_kp + setNum(int) + + + 229 + 359 + + + 412 + 359 + + + + + slider_ko + valueChanged(int) + label_ko + setNum(int) + + + 230 + 447 + + + 412 + 447 + + + + + slider_ki + valueChanged(int) + label_ki + setNum(int) + + + 226 + 388 + + + 412 + 388 + + + + + slider_cmd_lasttime + valueChanged(int) + label_cmd_lasttime + setNum(int) + + + 681 + 334 + + + 838 + 334 + + + + + slider_va_max + valueChanged(int) + label_va_max + setNum(int) + + + 655 + 443 + + + 838 + 443 + + + + + slider_vy_max + valueChanged(int) + label_vy_max + setNum(int) + + + 655 + 407 + + + 838 + 407 + + + + + slider_vx_max + valueChanged(int) + label_vx_max + setNum(int) + + + 656 + 370 + + + 838 + 370 + + + + + slider_set_pwm1 + valueChanged(int) + label_set_pwm1 + setNum(int) + + + 313 + 142 + + + 525 + 142 + + + + + slider_set_pwm2 + valueChanged(int) + label_set_pwm2 + setNum(int) + + + 322 + 165 + + + 528 + 165 + + + + + slider_set_pwm3 + valueChanged(int) + label_set_pwm3 + setNum(int) + + + 322 + 199 + + + 528 + 199 + + + + + slider_set_pwm4 + valueChanged(int) + label_set_pwm4 + setNum(int) + + + 322 + 232 + + + 528 + 232 + + + + + diff --git a/Hardware/UPbot-Tools/pypibot/__init__.py b/Hardware/UPbot-Tools/pypibot/__init__.py new file mode 100644 index 0000000..aa21836 --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/__init__.py @@ -0,0 +1,17 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import pypibot.assistant +import pypibot.log as Logger +import pypibot.err +log=Logger.log +import sys +def createNamedLog(name): + return Logger.NamedLog(name) +class Object(): + pass +isDebug="-d" in sys.argv + +import platform +isWindows=False +if platform.system()=='Windows': + isWindows=True diff --git a/Hardware/UPbot-Tools/pypibot/__pycache__/__init__.cpython-37.pyc b/Hardware/UPbot-Tools/pypibot/__pycache__/__init__.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e76284f85703b15020580d0f21ab163a1d814b99 GIT binary patch literal 655 zcmY*W%}yIJ5VpM=ud~@i>Vlv7D$^oV$Jfn+QPk4O&w63!%5bUD1GJPmatK9UK~)aYrvL6Y4PeItiF zllT|P(>IuhrVHQ`#J`@)cTnT$TJ(e;(8kx|X-*vd(UqmroK1^HNpN!P;uh>@q+_W6 zcOs8xeKqf_nCq%7SN+U(U2SIlIz(p|3UY^+pJGB(ZDoZx>ep_v(5kR%-=29Q1OQ=8 z0TJnR!%Axx znY9V4h5jxsba}^<%ABeBs&uTc3wzV)h7T*Vo9ZZhVpa2juRp!^q#gUZ-SEW!Msonb RaE2MCpo~mNN*InI@*fEjo{#_l literal 0 HcmV?d00001 diff --git a/Hardware/UPbot-Tools/pypibot/__pycache__/assistant.cpython-37.pyc b/Hardware/UPbot-Tools/pypibot/__pycache__/assistant.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..c2b3710a80c27fb67a28f234f11e84cbf4cf2b4d GIT binary patch literal 7136 zcma)B%WoUU8K2n~mlQ?QvaHB?l}@e1Od8cqf+S7za2~evaFifYY=?EgP@I*t(uzyZ zt`do$jG%HG_mCoo6li;?)Kd%O&_mHfP_#J|D0=FlMN#abDAG&vFZ9y>zHe5PsK`Jm z?CduJY^~ShG-O&qg=dw<*5SRMdhm^-X&F1BY2Mlext0)Y83U+KrW9q zD(X&kL{)Fd#*yI2mXJc-rN-1Tw5u0{8dt|}2z7kRY8*vtLfwYeZD@_DNp%8q98oqe#WF+_qACSt@x#?8r^iPBN$4N|!QU2d#)}cCwLVUYIO|QIHi>s5`AyR9f>14;R8t(hb^a<|b*Yy^`5U z(5+jV_SqO|x1uP}P5Sg#LRZlDpJ&t)SG(bSoHplm*jZfbUQOdTO0IS{xcBOX0Aml% z4fVB>B*g5Ubh^8dO|;jv4mv5%+5|sXJk}dzP{|kyA;)AHe-*1L$7NNNrLLm4P5Y!D zapvKr)LZtp3p-*H`o>y?zO(HsAKKep zub5N*{s3i>DNFk(M{>$94p5FTWqBXvsGhS}#qfA@bN1N{#wC@adJk7k?hwb(4A%YRz+CXQuSySd{cLEi-tJtm+@mwS(Nm|Bwhp@~*C(6>;uza)fV(@Z&XtoUI3KZlAHEr(J4z{suXXDj&%ior)=eXpD@hn z+wePEkEKrE*&-iw^cZXnzSMUXtQ{%EwXw?rGfX=o^;Syy7gB7xQuL~60oLeFU z8+U^az^xmFX=aBV6tJ<(Yjx4Dvf?@ZB8)-Pg$C2QmyLAyX3U%@>;w&OAq*mwWOg@H zncZGhnG6mpwld7i%iu4$8->W#Qz@Zp81< z2XKyXvD1ngh1r)byzU*hinu-HnGqP{x2F4SL2?CDd zku$(~NSgXV^bKT50mH|`=KcgK2&r<#(q(1M3K%xa5CH?Sf?1A3P8Q7aj4^Pcxq^@} zE0jah=$)=TzZA4rYUg8JOP2y{8uk`++CeQ|sI_Wqop8Md@OU?oqZpxJIbefPbm!!+Qnt#y{5--z}| z-UQ#MrEx8a7kO)S2@i@FXL`rp%;{8(_x40>D$z&54ckn;pwFP56;lmJokwcHdd-~W zIP7Fq1A@)L3c5hV4y2ma-9`cZa~s{D;iGb?6|DuCmCo~o`dhTZ0r{28JWFxBat*@? zsa;rQn8lq^KZ^R?kX2AbVXGkc?F(H_MJk)3jW<=W&{~Vq3sF4Z ziq5fQ>ayVloz^^Zqyr3%neP3>lrSa)!Gs6Cr5~rc>9aPelYS%uiaMm`pYR13!`QlR4lCMtfrz2mXK&u>%_n08T(sX8eW9brdQUxQaAs5tXpiTn&2Ymoz>Y3Q3*{(7}GSWAOB=4SfI zC`RI&JUzYl88Tu*5d9q#z^vS|nVh1&XTHpeNszwK3RWqa@Fw8}gYWdW@o}TjT1(?M z))0Xd6Y3|a*G`udGhBC=%$WM#rzUgQ&*cq#W`F`487okh;0Ryp=g>b;G3snlXYo$R z-~vHsFr2@RuoX$1z+C0w#N()f^6_?6mCshN?EG9+P!-tF(afbxN*k$?>9pCFQ6aM( zc+?AM^l|8E!VFQ$K9Hx0n1#{?0*NM$dMI5*bw1`rg;jskBUA0WThv>~E#~|#-1sv% zv8apjV!g=H%-n@gCu!z&10BYS1(ua45z|cy-B|(Byd3Kl%FyEO`KYyM(9{Wi)A<(GNdahoQGQEw;NT|AhczN+zug$T+M~6U4pA&yoY>>P8`bW=Ru2eC3#)$ zy4w0YD14szQ`Cydr}_Lim3(#nYmI#V-~K=IUu#?u*Ba#Bum;fvwj%KSzHui9*2M$$`mz zYkXof^S$N!O6MvLx3@o6~g4%D10|CyOv7RX7y84Q2aKA z_Zrn+q~aMW&Y}R!u{?&u3lDvQ;T!;bIw-&v$MNos7y&T+d`Y3CP4-7WY7Fe~4K#8& z%&;>`wm(WCSsr<&?!f*o!afiZdSsuJIIt9Qptr8$9Iy}Q?U}jWD0y9tR3ERUCjo)4 zV=c2|-rYmg9N2)tahvxE&oy5iY5)$+`x|_m1GtYZ=UOlAZaVG`shzgYYOkUK_C;Kw za$D|@86(D9M&QKZEo4nYbd{B`7-etNER`#C27t}VAtPhJ(zG7tA_3R^49G!IsoBg* z&E{&X)*`Bxo6T!$t;me%SLj>T{#mNcQNc3XQ~Lmlc>0e)k&r5VSt|LXzF+dWzm2ZJ zkJNn@PjUst0Qiw90_JUoe}a7j|89T_HVOb+5|?g8RRWE#MpPN^q8e2dyi0tshFc+Z zRE?osR>vU2aRk4xy%Old&Z|;n!L=jSwhVY7r64RIjG}8V zuDs~@jvjjwpf(PT&LU$Yis8QfKpuX9>0rsknEo0SBpUr)6o>Pfc_g(!{}~h}skKUS z!XoEjcFpMEz(ULp5SZLQ#c^Z{_nv)g3uod@iDNB}Co4EGr}BHmEaOn$GB*diCgU(~ zS>@Rz2{f4upF4B|6Y7&Fpi)B1OtvyPe=vNMT1w3tOZy9m@>HDe8hb}`uJ)eEt;_xz&xy-gwUC5Xq2D z3~ZmEfxT^TmXR!_TPX9Wney2vG7E>DzthUO2~G~JyN_tFHmP=CkEC9*Hr!d@U~Z^uFxer-BM8KMM}Xz^KE=>SanR~BOAQ6yozBbm(@Q&$B-xx z6(}q4Wv`lZJ|G%!Oh`cwdk`hE#IeC8sfmYMND639cjC?FKS8rApglY?V>9(p{U*ls zB`Pjc!3a-4!r-3y$e574rW?GLZqY!S3PnXg#R3(3*0G2Ggyw(1!&#^=eaE+btL&M# z?`ayRx%fvaBxB%xl6T`sDI&fsrzmJb`NsHqh3<~%f*kiolY?tI3jVvvCk208=P8G1 z^5DNO^<)>toe>GOH_6FI?Y&l*A{(O113K4FPfwe>+#|U1f0w&zbTM?Y?wR!bc6@7q z=T)lRgCdjfAw-{{=3yzJPfMJ`-2Vc_zGTQkUN8n5&>{K&Vs;V9a4k$Vy?->~5q>ap&PLL7!WKUO~Ms=iug~uerJUadQVmbLtT) zj#Kdf6`cGr%vPz%cbavo(Ipn&_cW~8^X3ZTG3t7pisz^xZL*$uf4u6UT>4L%gyU%& kA0&iclW+_Ev;DIFh+jqGP;zX)=vgHXPqp$u<)mNyAAcv$+5i9m literal 0 HcmV?d00001 diff --git a/Hardware/UPbot-Tools/pypibot/__pycache__/err.cpython-37.pyc b/Hardware/UPbot-Tools/pypibot/__pycache__/err.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5eff6b8edb31762976b1dbbf542550c7c39bc4ab GIT binary patch literal 1634 zcmai!O^@3|7{|wU5o0p}O0&Re9+Z?F4QXy1^*RFM&+c#Xr6=(!{ zOvj)x=t=D#Jv|fCXCdPHFdWcJO#KZODSV5BZxf=t623zYHLxR2YGH3m#7W+j*jc_y zZRqNhozrNMWG6}`Dr8+ec(*Zs#MA>UTWc!siS-`~=U^OWrb3l+r|d3v5D$x5H68*_U8TH@~OU+$bi=ks(^dO;Ay zQ5FO!v(Q{yVYRGcym$NgjYP#Dn(_ zj&WiMiG5?7TH5zGvG%MO?jOH6*3XV<3iJG`cEvKT{L72d4Fa91Am~F*n><*>Xs-x4 zGS_!JVKI{;4xUr9v+B}Xr32~de^^>et>+h#-LEa#cPw3B&!!&NE}pMt5R{!DSSOUP zn7cvnYd&AqH_C+?lS9Q{bm^rkNu|m*s>>JLJ+~Hj@Rk;1_U2eau>**u+31R?S*LLPnWRlo%53WN13BYvB1EM91JHaKey47~ z>`5j=g!`&az3ZGhb?Q8-zgH-D48Nb{cY}Lp82dR@dOzy9h7|o3XG}1`>&#Dc?sHu? zd_(7^Z|dAyw|$$ZV;tWJ>`rEv$(y>*_1%W6OIbgwbI72` z{eo};wpv_sWFj)Cx!u|yi7aZKKO%B|N#y-eQSiq^(LW@ZKQ2c6!=fZcKV$xc7!!w( zPu^l;Tpa$4iNiaFKNU>vu-Y`*Cd4G#Ced~zI4Y*rj2B$~c|6@YCZ@%aU4t`}kBg(? z7?rp3!3lAEhyMY8$Qtt&3r?(g82_uB`6oGRmoT<0P6Q`u>_c!#aR21TCm*rjVh!UF zt#nG9+~s=B)BV!1e(89>RPL9)G$S}7$~(L^jn&TdN1g4LhR2^0r$LK}Q{WHS?3tHz znw6U>^Du0P=4Q00tc`g2{kDA}%>XaF-?lESJXupFz)KtBUs?oA0tNw`KMPrju_Y?e zk8knd@{yvtBrT`ds zd5KwHN^%l?uV(fQ^du~1U`C#f7-cYAP6X`yX9jynVL<8M=PIa=UU% z*$emY-@QM}g0!@us!gq)P zI7Dm=0%mB@)?}jm%NX-YA#$VcrMZU@kzRiobFcT+n8|P@UACuKx%>v&x(JmMXkPB$ zc`0q~vKY7b{t)iz{;v0rdNmDWeHx${`7RXZfCJ~yrwSV_QHz6{I5?o{lIH7!tkl+n z{lpbgv@{%tzetpklBTn1PwbJ2<1+mN-%zf zgbUo-@6!#iw?0gGrzjke*)@ba%|v#RZJT=LNMi1C`6J9MQ3*2%N0D$mHny#88$F|m zjXrH|8+i`a@}99BTOI2h+GVTbz>?7-V8Up>XR-LOHrl5-+jd~I$KweBx)W#Fm|#1= z0>h1O+xb1GXSnroiwECISe@&bUem+H+Z zkaJWvnvt?vxauk+LfL?*C_9W$RPK6hEkFVNgj3m3s~*OX_UBkWQjQGPo1X?M6K$>6 z!^WC&N&eyx3z&`OrZS-vRCeL{QqYRSCZ@Mws^Zy z*s-}|$Vy^$tQg{+*t@)w5fEGwz-Q<52p1z(X+#3AcF>o$q~jj4IGb2IeB0cD*eB)$ z+cqG6znzNc*6HWRG|usDJ27G}v3K|+{9cH@@$3f=!Jx(*1l3&gw1i7#CC&t+ zF<%zZu7}AJV~5ANH49hR!3>oD(3r-IIKP_j6yTL*&}!2faj`S9%P{VITJ5YG>vpbWR9}L74CO&Wv{WsSoW|-nBHvU5r` zI;TkuwI#}Du!H}J&#umN-buU!$|K25GCBkFedC28KSX{G^iDvn8FB(@ZI+2voUWX3Am`E1+T7yq1>+oh{l@7%pr)?U`ttKXT4+D5@^U;cO|`lLKF|DL!o zGk+-ss9q9R`e1Hm{<0{an}3c`k7l8*!v8kmzmvZke(?ovBI&KRb@b!;^>zmETbIAN zPSuOO>ZM-ca<71{<9+*5ZEnP^jkvrdYthqFT@^N$Ivpkyfbzm9Y>+a8Lw*amlv8U# z7YVo|XZYHKTg+H71_nQH3ps4P6sYA6C|QYUTF-lej(f|^wJI((<6@+M8U zo;Ek6a@NDfMjR+70ui7ww;nD@C|X^~hEX?Uk|zl5Ndjd8rwE)Ta2`NqYD>>HLK$cm z1Eg<3)kd)ZrYW^}+5EsKQs#QJqO6)+ zi6ohCWulnsS7L-RLZLX2wPdgyJ_pi`W#s#GFSwJQSqoEiq{g@hKiq?7Ag zc#x>+oc>}6wKqWhmS{=t-ne5jet$WHcCcd|3?C$Fvb}#jgnF=J@s?=ISnZ8F9>Ji$ z8A4m=Gn(T4gTzd3+20Of9&9*#OSGg=-n3z5ZPsMt$3u7rJEookM6LbYpA2Ch?0EAn z(USIj(~c!6gTEg_I}mF9Y;%yP$%*}kA=HB%2YsiLoKI3*+Ue{mE>c^FP0T^K$0aGQ zS!E)dsyLpfL5zkl~=7I^4X|UIbW?l+o;vk9yyP-Lbxs4GXH+T?AfBe?PEu)uz+ex$q@#2`<9KyK^-Ph746Y|&OiQ~e}XrJ7}JyqQR%&{P{R<@G{5L{{sk!_$zcXr^dcTtdV=ufXC2~B z-$au$1cw;m6#@!+JK!B7HusxrOOqmh7lWlfJ(~8$8&8t)uSnge4Ue~reX72~bRQ&1 zWYB-Mha_)6QvdA!KREQY4gJ6EA!op$uQ2s*$)T@B65U_5hok|AUgHA1B{^kad7})G zCi(Y0+vcSi@sgB;Z!SFg zplx2Bi4?!+TZs4LcIM*M8F79ldMPkqc1A8ClUE4bAn-1L8twkcHrIbBP2T|AR=pO} zTY`K@V;>Rl3EU%413*l?72$Of|K|kDwT*hbpd)YkUxdQyPhT6^`&Q*{WkLQXEi_5s z3;}W*QjLVwcgTiBzXMQ00PP|a zCrE*}dn66kDe%_Gpg_F)8I}QuCsX`=X5zW@9S`&JI*=dZCv@mdA%6PlKpy4)1$ZQM A_y7O^ literal 0 HcmV?d00001 diff --git a/Hardware/UPbot-Tools/pypibot/assistant.py b/Hardware/UPbot-Tools/pypibot/assistant.py new file mode 100644 index 0000000..94683b0 --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/assistant.py @@ -0,0 +1,234 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import os, sys, inspect +import datetime +import signal +import threading +import time +# function: get directory of current script, if script is built +# into an executable file, get directory of the excutable file +def current_file_directory(): + path = os.path.realpath(sys.path[0]) # interpreter starter's path + if os.path.isfile(path): # starter is excutable file + path = os.path.dirname(path) + path = os.path.abspath(path) # return excutable file's directory + else: # starter is python script + caller_file = inspect.stack()[0][1] # function caller's filename + path = os.path.abspath(os.path.dirname(caller_file))# return function caller's file's directory + if path[-1]!=os.path.sep:path+=os.path.sep + return path + +"""格式化字符串""" +def formatString(string,*argv): + string=string%argv + if string.find('$(scriptpath)')>=0: + string=string.replace('$(scriptpath)',current_file_directory()) + if string.find('$(filenumber2)')>=0: + i=0 + path="" + while True: + path=string.replace('$(scriptpath)',"%02d"%i) + if not os.path.lexists(path):break + i+=1 + string=path + #8位日期（20140404） + if string.find('$(Date8)')>=0: + now=datetime.datetime.now() + string=string.replace('$(Date8)', now.strftime("%Y%m%d")) + #6位日期（140404） + if string.find('$(Date6)')>=0: + now=datetime.datetime.now() + string=string.replace('$(Date6)', now.strftime("%y%m%d")) + #6位时间（121212） + if string.find('$(Time6)')>=0: + now=datetime.datetime.now() + string=string.replace('$(Time6)', now.strftime("%H%M%S")) + #4位时间（1212） + if string.find('$(Time4)')>=0: + now=datetime.datetime.now() + string=string.replace('$(Time4)', now.strftime("%H%M")) + #文件编号2位（必须在最后） + if string.find('$(filenumber2)')>=0: + i=0 + path="" + while True: + path=string.replace('$(filenumber2)',"%02d"%i) + if not os.path.lexists(path):break + i+=1 + string=path + #文件编号3位（必须在最后） + if string.find('$(filenumber3)')>=0: + i=0 + path="" + while True: + path=string.replace('$(filenumber3)',"%03d"%i) + if not os.path.lexists(path):break + i+=1 + string=path + return string + +""" + 取得进程列表 + 格式：(PID,cmd) +""" +def getProcessList(): + processList = [] + try: + for line in os.popen("ps xa"): + fields = line.split() + # spid = fields[0] + pid = 0 + try:pid = int(fields[0]) + except:None + cmd = line[27:-1] + # print "PS:%d,%s"%(npid,process) + if pid != 0 and len(cmd) > 0: + processList.append((pid, cmd)) + except Exception as e: + print("getProcessList except:%s" % (e)) + return processList +def killCommand(cmd): + try: + processList = getProcessList() + for p in processList: + if p[1].find(cmd) != -1: + pid = p[0] + os.kill(pid, signal.SIGKILL) + except Exception as e: + print("killCommand ‘%s’ except:%s" % (cmd,e)) + +def check_pid(pid): + """ Check For the existence of a unix pid. """ + if pid == 0:return False + try: + os.kill(pid, 0) + except OSError: + return False + else: + return True + +SF=formatString + +#全局异常捕获 +def excepthook(excType, excValue, tb): + '''''write the unhandle exception to log''' + from log import log + import traceback + log.e('Unhandled Error: %s',''.join(traceback.format_exception(excType, excValue, tb))) + sys.exit(-1) + #sys.__excepthook__(type, value, trace) + #sys.__excepthook__(excType, excValue, tb) + +_defaultGlobalExcept=sys.excepthook + +def enableGlobalExcept(enable=True): + if enable: + sys.excepthook = excepthook + else: + sys.excepthook=_defaultGlobalExcept +# 默认启动全局异常处理 +enableGlobalExcept() +#创建线程 +def createThread(name,target,args=(),autoRun=True,daemon=True): + from log import log + def threadProc(): + log.i("thread %s started!",name) + try: + target(*args) + log.i("thread %s ended!",name) + except Exception as e: + log.e("thread %s crash!err=%s",name,e) + thd=threading.Thread(name=name,target=threadProc) + thd.setDaemon(daemon) + if autoRun:thd.start() + return thd + + +#定时器 +class Timer(): + def __init__(self, timer_proc, args=(),first=0,period=0,name="Timer"): + self.name=name + self.first=first + self.period=period + self.args=args + self.timer_proc=timer_proc + self.thdWork=None + self.stopFlag=False + from log import NamedLog + self.log=NamedLog(name) + def run(self): + if self.first>0: + time.sleep(self.first) + try: + self.timer_proc(*self.args) + except Exception as e: + self.log.error("timer proc crash!err=%s",e) + while self.period>0 and not self.stopFlag: + time.sleep(self.period) + try: + self.timer_proc(*self.args) + except Exception as e: + self.log.error("timer proc crash!err=%s",e) + def start(self): + if self.isAlive(): + self.log.d("already running!") + return True + self.stopFlag=False + self.thdWork=threading.Thread(name=self.name,target=self.run) + self.thdWork.setDaemon(True) + self.thdWork.start() + def stop(self,timeout=3): + if self.isAlive(): + self.stopFlag=True + try: + self.thdWork.join(timeout) + except Exception as e: + self.log.e("stop timeout!") + + def isAlive(self): + return self.thdWork and self.thdWork.isAlive() +#计时器 +class Ticker(): + def __init__(self): + self.reset() + # 片段,可以判断时长是否在一个特定毫秒段内 + self.section=[] + def reset(self): + self.tick=time.time() + self.end=0 + def stop(self): + self.end=time.time() + @property + def ticker(self): + if self.end==0: + return int((time.time()-self.tick)*1000) + else: + return int((self.end-self.tick)*1000) + def addSection(self,a,b): + a,b=int(a),int(b) + assert a=a and tick<=b: + return True + return False + def __call__(self): + return self.ticker +def waitExit(): + import log + log.log.i("start waiting to exit...") + try: + while(True): + time.sleep(1) + except Exception as e: + log.log.w("recv exit sign!") + +def is_python3(): + return sys.hexversion > 0x03000000 diff --git a/Hardware/UPbot-Tools/pypibot/configer.py b/Hardware/UPbot-Tools/pypibot/configer.py new file mode 100644 index 0000000..7bb3b9f --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/configer.py @@ -0,0 +1,56 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- + +import ConfigParser +from log import PLOG +import os +def getdefaultfilename(): + pass +def openconfiger(filename): + return configer(filename) +class configer: + def __init__(self,fullfilepath): + self._filepath=fullfilepath + if not os.path.isdir(os.path.dirname(fullfilepath)): + os.makedirs(os.path.dirname(fullfilepath)) + self._conf=ConfigParser.ConfigParser() + if os.path.isfile(fullfilepath): + try: + self._conf.readfp(open(fullfilepath,"r")) + except Exception,e: + PLOG.error("配置文件'%s'打开失败,err=%s"%(self._filepath,e)) + def save(self): + try: + self._conf.write(open(self._filepath,"w")) + except Exception,e: + PLOG.error("配置文件'%s'保存失败,err=%s"%(self._filepath,e)) + + def changeConfValue(self,section,option,value): + if self._conf.has_section(section): + self._conf.set(section,option,value) + else: + self._conf.add_section(section) + self._conf.set(section,option,value) + + def _readvalue(self,fn,section,option,default): + result=default + if self._conf.has_section(section): + if self._conf.has_option(section,option): + result=fn(section,option) + PLOG.debug("Option[%s][%s]=%s"%(section,option,str(result))) + else: + self._conf.set(section,option,str(default)) + result=default + else: + self._conf.add_section(section) + self._conf.set(section,option,str(default)) + result=default + return result + def getstr(self,section,option,default=""): + return self._readvalue(self._conf.get, section, option, default) + def getint(self,section,option,default=0): + return self._readvalue(self._conf.getint, section, option, default) + def getfloat(self,section,option,default=0.0): + return self._readvalue(self._conf.getfloat, section, option, default) + def getboolean(self,section,option,default=False): + return self._readvalue(self._conf.getboolean, section, option, default) diff --git a/Hardware/UPbot-Tools/pypibot/daemon.py b/Hardware/UPbot-Tools/pypibot/daemon.py new file mode 100644 index 0000000..1ff78dc --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/daemon.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import sys, os, time, atexit +from signal import SIGTERM + +def check_pid(pid): + """ Check For the existence of a unix pid. """ + if pid == 0:return False + try: + os.kill(pid, 0) + except OSError: + return False + else: + return True + +class daemon: + """ + A generic daemon class. + + Usage: subclass the Daemon class and override the run() method + """ + def __init__(self, pidfile, stdin='/dev/null', stdout='/dev/null', stderr='/dev/null'): + self.stdin = stdin + self.stdout = stdout + self.stderr = stderr + self.pidfile = pidfile + + def daemonize(self): + """ + do the UNIX double-fork magic, see Stevens' "Advanced + Programming in the UNIX Environment" for details (ISBN 0201563177) + http://www.erlenstar.demon.co.uk/unix/faq_2.html#SEC16 + """ + try: + pid = os.fork() + if pid > 0: + # exit first parent + sys.exit(0) + except OSError, e: + sys.stderr.write("fork #1 failed: %d (%s)\n" % (e.errno, e.strerror)) + sys.exit(1) + + # decouple from parent environment + os.chdir("/") + os.setsid() + os.umask(0) + + # do second fork + try: + pid = os.fork() + if pid > 0: + # exit from second parent + sys.exit(0) + except OSError, e: + sys.stderr.write("fork #2 failed: %d (%s)\n" % (e.errno, e.strerror)) + sys.exit(1) + + # redirect standard file descriptors + sys.stdout.flush() + sys.stderr.flush() + si = file(self.stdin, 'r') + so = file(self.stdout, 'a+') + se = file(self.stderr, 'a+', 0) + os.dup2(si.fileno(), sys.stdin.fileno()) + os.dup2(so.fileno(), sys.stdout.fileno()) + os.dup2(se.fileno(), sys.stderr.fileno()) + + # write pidfile + atexit.register(self.delpid) + pid = str(os.getpid()) + file(self.pidfile, 'w+').write("%s\n" % pid) + + def delpid(self): + os.remove(self.pidfile) + + def start(self): + """ + Start the daemon + """ + # Check for a pidfile to see if the daemon already runs + try: + pf = file(self.pidfile, 'r') + pid = int(pf.read().strip()) + pf.close() + except IOError: + pid = None + + if pid and check_pid(pid): + message = "pidfile %s already exist. Daemon already running?\n" + sys.stderr.write(message % self.pidfile) + sys.exit(1) + print("daemon start...") + # Start the daemon + self.daemonize() + self.run() + + def stop(self): + """ + Stop the daemon + """ + # Get the pid from the pidfile + try: + pf = file(self.pidfile, 'r') + pid = int(pf.read().strip()) + pf.close() + except IOError: + pid = None + + if not pid: + message = "pidfile %s does not exist. Daemon not running?\n" + sys.stderr.write(message % self.pidfile) + return # not an error in a restart + + # Try killing the daemon process + try: + while 1: + os.kill(pid, SIGTERM) + time.sleep(0.1) + except OSError, err: + err = str(err) + if err.find("No such process") > 0: + if os.path.exists(self.pidfile): + os.remove(self.pidfile) + else: + print(str(err)) + sys.exit(1) + + def restart(self): + """ + Restart the daemon + """ + self.stop() + self.start() + + def run(self): + """ + You should override this method when you subclass Daemon. It will be called after the process has been + daemonized by start() or restart(). + """ diff --git a/Hardware/UPbot-Tools/pypibot/err.py b/Hardware/UPbot-Tools/pypibot/err.py new file mode 100644 index 0000000..d812de9 --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/err.py @@ -0,0 +1,58 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- + +# 异常类 +class PibotError(Exception): + def __init__(self, errcode, errmsg): + self.errcode = errcode + self.errmsg = errmsg + #Exception.__init__(self,self.__str__()) + + def msg(self, msg): + if not msg is None:return PibotError(self.errcode, msg) + return PibotError(8,"unknow error") + def __str__(self): + return "PibotError:%s(%d)"%(self.errmsg,self.errcode) + @property + def message(self): + return str(self) +# 声明 +# 成功 +success=PibotError(0,"null") +# 通用失败 +fail=PibotError(1,"fail") +# 参数无效 +invalidParameter=PibotError(2,"invalid parameter") +# 不支持 +noSupport=PibotError(3,"no support") +# 不存在 +noExist=PibotError(4,"no exist") +# 超时 +timeout=PibotError(5,"timeout") +# 繁忙 +busy=PibotError(6,"busy") +# 缺少参数 +missParameter=PibotError(7,"miss parameter") +# 系统错误(通用错误) +systemError=PibotError(8,"system error") +# 密码错误 +invalidPassword=PibotError(9,"invalid password") +# 编码失败 +encodeFailed=PibotError(10,"encode failed") +# 数据库操作失败 +dbOpertationFailed=PibotError(11,"db error") +# 已占用 +occupied=PibotError(12,"occupied") +# session不存在 +noSession = PibotError(13,'cannot find session') +#没有找到 +noFound = PibotError(14, "no found") +#已经存在 +existed = PibotError(15, "existed") +#已经锁定 +locked = PibotError(16, "locked") +#已经过期 +expired = PibotError(17, "is expired") +#无效的参数 +invalidParameter = PibotError(18, "invalid parameter") + diff --git a/Hardware/UPbot-Tools/pypibot/log.py b/Hardware/UPbot-Tools/pypibot/log.py new file mode 100644 index 0000000..c031d4a --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/log.py @@ -0,0 +1,259 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import sys,os +import datetime +import threading +import pypibot.assistant as assistant +import platform +if assistant.is_python3(): + import _thread +else: + import thread +import traceback +""" +%a Locale’s abbreviated weekday name. +%A Locale’s full weekday name. +%b Locale’s abbreviated month name. +%B Locale’s full month name. +%c Locale’s appropriate date and time representation. +%d Day of the month as a decimal number [01,31]. +%H Hour (24-hour clock) as a decimal number [00,23]. +%I Hour (12-hour clock) as a decimal number [01,12]. +%j Day of the year as a decimal number [001,366]. +%m Month as a decimal number [01,12]. +%M Minute as a decimal number [00,59]. +%p Locale’s equivalent of either AM or PM. (1) +%S Second as a decimal number [00,61]. (2) +%U Week number of the year (Sunday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Sunday are considered to be in week 0. (3) +%w Weekday as a decimal number [0(Sunday),6]. +%W Week number of the year (Monday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Monday are considered to be in week 0. (3) +%x Locale’s appropriate date representation. +%X Locale’s appropriate time representation. +%y Year without century as a decimal number [00,99]. +%Y Year with century as a decimal number. +%Z Time zone name (no characters if no time zone exists). +%% A literal '%' character. + +""" +isWindows=False +if platform.system()=='Windows': + isWindows=True +defaultEncodeing="utf8" +if "-utf8" in sys.argv: + defaultEncodeing="utf-8" +if "-gbk" in sys.argv: + defaultEncodeing="gbk" + +TRACE=5 +DEBUG=4 +INFORMATION=3 +WARNING=2 +ERROR=1 +NONE=0 + +MAX_MSG_SIZE = 4096 + +def getLevelFromString(level): + level=level.lower() + if level=='t' or level=='trace':return 5 + elif level=='d' or level=='debug':return 4 + elif level=='i' or level=='info':return 3 + elif level=='w' or level=='wran':return 2 + elif level=='e' or level=='error':return 1 + else :return 0 +def getLevelString(level): + if level==TRACE:return "T" + elif level==DEBUG:return "D" + elif level==INFORMATION:return "I" + elif level==WARNING:return "W" + elif level==ERROR:return "E" + else:return "N" +class PibotLog: + def __init__(self): + self.isEnableControlLog=True + self.fileTemple=None + self.filePath="" + self.level=5 + self._lock=threading.RLock() + self.fd=None + self.fd_day=-1 + def setLevel(self,level): + self.level=getLevelFromString(level) + def enableControllog(self,enable): + self.isEnableControlLog=enable + def enableFileLog(self,fileName): + self.fileTemple=fileName + self.updateFilelog() + def updateFilelog(self): + fn=assistant.SF(self.fileTemple) + if fn!=self.filePath: + self.i("new log file:%s",fn) + if self.fd: + self.fd.close() + self.fd=None + self.fd_day=-1 + self.filePath="" + try: + path=os.path.dirname(fn) + if path!="" and not os.path.isdir(path):os.makedirs(path) + self.fd=open(fn,mode="w") + try: + linkfn = fn.split(".log.", 1)[0] + ".INFO" + if os.path.exists(linkfn): + os.remove(linkfn) + (filepath, tempfilename) = os.path.split(fn) + os.symlink(tempfilename, linkfn) + except: + pass + self.fd_day=datetime.datetime.now().day + self.filePath=fn + except Exception as e: + print("open file fail!file=%s,err=%s"%(fn,e)) + def _output(self,level,msg,args): + if self.level0: + t=[] + for arg in args: + if isinstance(arg,Exception): + t.append(traceback.format_exc(arg).decode('utf-8')) + elif isinstance(arg,bytes) : + t.append(arg.decode('utf-8')) + else: + t.append(arg) + args=tuple(t) + try: + msg=msg%args + except: + try: + for arg in args: + msg=msg+str(arg)+" " + except Exception as e: + msg=msg+"==LOG ERROR==>%s"%(e) + if len(msg)>MAX_MSG_SIZE:msg=msg[0:MAX_MSG_SIZE] + now=datetime.datetime.now() + msg=msg+"\r\n" + if assistant.is_python3(): + id = _thread.get_ident() + else: + id = thread.get_ident() + s="[%s] %04d-%02d-%02d %02d:%02d:%02d.%03d (0x%04X):%s"%(getLevelString(level),now.year,now.month,now.day,now.hour,now.minute,now.second,now.microsecond/1000,(id>>8)&0xffff,msg) + prefix='' + suffix='' + if not isWindows: + suffix='\033[0m' + if level==TRACE: + prefix='\033[0;37m' + elif level==DEBUG: + prefix='\033[1m' + elif level==INFORMATION: + prefix='\033[0;32m' + elif level==WARNING: + prefix='\033[0;33m' + elif level==ERROR: + prefix='\033[0;31m' + else: + pass + self._lock.acquire() + try: + if self.isEnableControlLog: + sys.stdout.write((prefix+s+suffix)) + if self.fd: + if self.fd_day!=now.day: + self.updateFilelog() + if assistant.is_python3(): + self.fd.write(s) + else: + self.fd.write(s.encode('utf-8')) + self.fd.flush() + finally: + self._lock.release() + except Exception as e: + if assistant.is_python3(): + print(e) + else: + print("PibotLog._output crash!err=%s"%traceback.format_exc(e)) + + def trace(self,msg,*args): + self._output(TRACE,msg,args) + def t(self,msg,*args): + self._output(TRACE,msg,args) + def debug(self,msg,*args): + self._output(DEBUG, msg,args) + def d(self,msg,*args): + self._output(DEBUG, msg,args) + def info(self,msg,*args): + self._output(INFORMATION, msg,args) + def i(self,msg,*args): + self._output(INFORMATION, msg,args) + def warn(self,msg,*args): + self._output(WARNING, msg,args) + def w(self,msg,*args): + self._output(WARNING, msg,args) + def error(self,msg,*args): + self._output(ERROR, msg,args) + def e(self,msg,*args): + self._output(ERROR, msg,args) + def _log(self,level,msg,args): + self._output(level, msg,args) + def createNamedLog(self,name): + return NamedLog(name) +log=PibotLog() +class NamedLog: + def __init__(self,name=None): + global log + self.name='' + if name: + self.name='['+name+']' + self.log=log + def trace(self,msg,*args): + msg=self.name+msg + self.log._log(TRACE,msg,args) + def t(self,msg,*args): + msg=self.name+msg + self.log._log(TRACE,msg,args) + def debug(self,msg,*args): + msg=self.name+msg + self.log._log(DEBUG, msg,args) + def d(self,msg,*args): + msg=self.name+msg + self.log._log(DEBUG, msg,args) + def info(self,msg,*args): + msg=self.name+msg + self.log._log(INFORMATION, msg,args) + def i(self,msg,*args): + msg=self.name+msg + self.log._log(INFORMATION, msg,args) + def warn(self,msg,*args): + msg=self.name+msg + self.log._log(WARNING, msg,args) + def w(self,msg,*args): + msg=self.name+msg + self.log._log(WARNING, msg,args) + def error(self,msg,*args): + msg=self.name+msg + self.log._log(ERROR, msg,args) + def e(self,msg,*args): + msg=self.name+msg + self.log._log(ERROR, msg,args) + +if __name__ == "__main__": + log.trace("1%s","hello") + log.debug("2%d",12) + try: + raise Exception("EXC") + except Exception as e: + log.info("3%s",e) + log.warn("1") + log.error("1") + #log.enableFileLog("$(scriptpath)test_$(Date8)_$(filenumber2).log") + log.trace("1") + log.debug("1") + log.info("1") + log.warn("1") + log.error("1") + log=NamedLog("test") + log.d("1") + log.i("1") + log.warn("1") + log.error("1=%d,%s",100,e) diff --git a/Hardware/UPbot-Tools/pypibot/mapconvert.py b/Hardware/UPbot-Tools/pypibot/mapconvert.py new file mode 100644 index 0000000..0d27ec4 --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/mapconvert.py @@ -0,0 +1,35 @@ +import cv2 +import numpy as np + +def map_server_to_cv2(data): + # print(data.info.height, data.info.width) + data = np.reshape(data.data, (data.info.height, data.info.width)) + data = np.flipud(data) + + # -1 --> 205 + # 0 --> 255 + # 100--> 0 + image = np.where(data == -1, 205, 255 - (255.0 * data / 100)).astype(np.uint8) + + #cv2.imshow("cv2_map", data) + return image + + +def cv2_to_map_server(image): + image = np.flipud(image) + + # 205 --> -1 + # 255 --> 0 + # 0 --> 100 + data = np.where(image == 205, -1, (255 - image) * 100.0 / 255).astype(np.int8).flatten() + + return list(data) + # pub_data = OccupancyGrid() + # pub_data.header.frame_id = "map" + # pub_data.header.stamp = rospy.Time.now() + # pub_data.info = map_data.info + # pub_data.data = list(data) + + # print(len(pub_data.data), pub_data.info.height * pub_data.info.width) + + # map_publisher.publish(pub_data) \ No newline at end of file diff --git a/Hardware/UPbot-Tools/pypibot/proxy.py b/Hardware/UPbot-Tools/pypibot/proxy.py new file mode 100644 index 0000000..5187c22 --- /dev/null +++ b/Hardware/UPbot-Tools/pypibot/proxy.py @@ -0,0 +1,38 @@ +from pypibot import log + +import threading +import zmq + +class MqProxy: + def __init__(self, sub_addr, pub_addr): + self.thd = None + self.sub_addr = sub_addr + self.pub_addr = pub_addr + + def _run(self): + context = zmq.Context() + + frontend = context.socket(zmq.XSUB) + frontend.bind(self.sub_addr) + # frontend.bind("tcp://*:5556") + backend = context.socket(zmq.XPUB) + backend.bind(self.pub_addr) + # backend.bind("tcp://*:5557") + try: + zmq.proxy(frontend, backend) + except KeyboardInterrupt: + pass + + frontend.close() + backend.close() + context.term() + + def start(self): + if self.thd is None: + log.i("mq proxy starting...") + self.thd=threading.Thread(target=self._run, name="proxy") + self.thd.setDaemon(True) + self.thd.start() + + def stop(self): + pass diff --git a/Hardware/UPbot-Tools/set_default_params.py b/Hardware/UPbot-Tools/set_default_params.py new file mode 100644 index 0000000..096b17c --- /dev/null +++ b/Hardware/UPbot-Tools/set_default_params.py @@ -0,0 +1,90 @@ +import platform +import sys +sys.path.append("..") +import pypibot +from pypibot import log +from transport import Transport +from dataholder import MessageID +import params + +#for linux +port="/dev/pibot" + +#for windows +#port="com3" + +pypibot.assistant.enableGlobalExcept() +#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log") +log.setLevel("i") + +if __name__ == '__main__': + mboard = Transport(port, params.pibotBaud) + if not mboard.start(): + log.error("can not open %s"%port) + sys.exit() + + DataHolder = mboard.getDataHolder() + + for num in range(0,3): + log.info("****************get robot version*****************") + boardVersion = DataHolder[MessageID.ID_GET_VERSION] + p = mboard.request(MessageID.ID_GET_VERSION) + if p: + log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode())) + break + else: + log.error('read firmware version err\r\n') + import time + time.sleep(1) + if num == 2: + log.error('please check connection or baudrate\r\n') + sys.exit() + + # set robot parameter + log.info("****************set robot parameter*****************") + + DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = params.pibotParam + + p = mboard.request(MessageID.ID_SET_ROBOT_PARAMETER) + if p: + log.info('set parameter success') + else: + log.error('set parameter err') + quit(1) + + # get robot parameter + robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER] + p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER) + if p: + log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \ + %(robotParam.param.model_type, \ + robotParam.param.wheel_diameter, \ + robotParam.param.wheel_track, \ + robotParam.param.encoder_resolution + )) + + log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \ + %(robotParam.param.do_pid_interval, \ + robotParam.param.kp, \ + robotParam.param.ki, \ + robotParam.param.kd, \ + robotParam.param.ko)) + + log.info("cmd_last_time:%d imu_type:%d" \ + %(robotParam.param.cmd_last_time,\ + robotParam.param.imu_type + )) + + log.info("max_v:%d %d %d\r\n" \ + %(robotParam.param.max_v_liner_x,\ + robotParam.param.max_v_liner_y, \ + robotParam.param.max_v_angular_z + )) + + log.info("motor flag:%d encoder flag: %d\r\n" \ + %(robotParam.param.motor_nonexchange_flag,\ + robotParam.param.encoder_nonexchange_flag + )) + else: + log.error('get param err\r\n') + quit(1) \ No newline at end of file diff --git a/Hardware/UPbot-Tools/test_motors.py b/Hardware/UPbot-Tools/test_motors.py new file mode 100644 index 0000000..d5e0daa --- /dev/null +++ b/Hardware/UPbot-Tools/test_motors.py @@ -0,0 +1,122 @@ +import platform +import sys +sys.path.append("..") +import pypibot +from pypibot import log +from transport import Transport +from dataholder import MessageID +import params +import signal + +#for linux +port="/dev/pibot" + +#for windows +#port="com3" + +pypibot.assistant.enableGlobalExcept() +#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log") +log.setLevel("i") + +run_flag = True + +def exit(signum, frame): + global run_flag + run_flag = False + +if __name__ == '__main__': + signal.signal(signal.SIGINT, exit) + + mboard = Transport(port, params.pibotBaud) + if not mboard.start(): + log.error("can not open %s"%port) + sys.exit() + + pwm=[0]*4 + for i in range(len(sys.argv)-1): + pwm[i] = int(sys.argv[i+1]) + + DataHolder = mboard.getDataHolder() + + for num in range(0,3): + log.info("****************get robot version*****************") + boardVersion = DataHolder[MessageID.ID_GET_VERSION] + p = mboard.request(MessageID.ID_GET_VERSION) + if p: + log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode())) + break + else: + log.error('read firmware version err\r\n') + import time + time.sleep(1) + if num == 2: + log.error('please check connection or baudrate\r\n') + sys.exit() + + # get robot parameter + robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER] + p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER) + if p: + log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \ + %(robotParam.param.model_type, \ + robotParam.param.wheel_diameter, \ + robotParam.param.wheel_track, \ + robotParam.param.encoder_resolution + )) + + log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \ + %(robotParam.param.do_pid_interval, \ + robotParam.param.kp, \ + robotParam.param.ki, \ + robotParam.param.kd, \ + robotParam.param.ko)) + + log.info("cmd_last_time:%d imu_type:%d" \ + %(robotParam.param.cmd_last_time,\ + robotParam.param.imu_type + )) + + log.info("max_v:%d %d %d\r\n" \ + %(robotParam.param.max_v_liner_x,\ + robotParam.param.max_v_liner_y, \ + robotParam.param.max_v_angular_z + )) + + log.info("motor flag:%d encoder flag: %d\r\n" \ + %(robotParam.param.motor_nonexchange_flag,\ + robotParam.param.encoder_nonexchange_flag + )) + else: + log.error('get params err\r\n') + quit(1) + + DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = pwm + + p = mboard.request(MessageID.ID_SET_MOTOR_PWM) + if p: + log.info('set pwm success') + else: + log.error('set pwm err') + quit(1) + + log.info("****************get encoder count*****************") + while run_flag: + robotEncoder = DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder + p = mboard.request(MessageID.ID_GET_ENCODER_COUNT) + if p: + log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robotEncoder]))) + else: + log.error('get encoder count err') + quit(1) + + import time + time.sleep(0.5) + + DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4 + + p = mboard.request(MessageID.ID_SET_MOTOR_PWM) + if p: + log.info('set pwm success') + else: + log.error('set pwm err') + quit(1) \ No newline at end of file diff --git a/Hardware/UPbot-Tools/transport.py b/Hardware/UPbot-Tools/transport.py new file mode 100644 index 0000000..d3c63db --- /dev/null +++ b/Hardware/UPbot-Tools/transport.py @@ -0,0 +1,203 @@ +import sys +sys.path.append("..") +import pypibot +from pypibot import log +from pypibot import assistant + +import serial +import threading +import struct +import time +from dataholder import MessageID, BoardDataDict +FIX_HEAD = 0x5a + +class Recstate(): + WAITING_HD = 0 + WAITING_MSG_ID = 1 + RECEIVE_LEN = 2 + RECEIVE_PACKAGE = 3 + RECEIVE_CHECK = 4 + +def checksum(d): + sum = 0 + if assistant.is_python3(): + for i in d: + sum += i + sum = sum&0xff + else: + for i in d: + sum += ord(i) + sum = sum&0xff + return sum + + +class Transport: + def __init__(self, port, baudrate=921600): + self._Port = port + self._Baudrate = baudrate + self._KeepRunning = False + self.receive_state = Recstate.WAITING_HD + self.rev_msg = [] + self.rev_data = [] + self.req_lock = threading.Lock() + self.wait_event = threading.Event() + + def getDataHolder(self): + return BoardDataDict + + def start(self): + try: + self._Serial = serial.Serial(port=self._Port, baudrate=self._Baudrate, timeout=0.2) + self._KeepRunning = True + self._ReceiverThread = threading.Thread(target=self._Listen) + self._ReceiverThread.setDaemon(True) + self._ReceiverThread.start() + return True + except: + print("Open Serial Error") + return False + + def Stop(self): + self._KeepRunning = False + time.sleep(0.1) + self._Serial.close() + + def _Listen(self): + while self._KeepRunning: + if self.receiveFiniteStates(self._Serial.read()): + self.packageAnalysis() + + def receiveFiniteStates(self, s): + if len(s) == 0: + return False + val = s[0] + val_int = val + if not assistant.is_python3(): + val_int = ord(val) + + if self.receive_state == Recstate.WAITING_HD: + if val_int == FIX_HEAD: + log.trace('got head') + self.rev_msg = [] + self.rev_data =[] + self.rev_msg.append(val) + self.receive_state = Recstate.WAITING_MSG_ID + elif self.receive_state == Recstate.WAITING_MSG_ID: + log.trace('got msg id') + self.rev_msg.append(val) + self.receive_state = Recstate.RECEIVE_LEN + elif self.receive_state == Recstate.RECEIVE_LEN: + log.trace('got len:%d', val_int) + self.rev_msg.append(val) + if val_int == 0: + self.receive_state = Recstate.RECEIVE_CHECK + else: + self.receive_state = Recstate.RECEIVE_PACKAGE + elif self.receive_state == Recstate.RECEIVE_PACKAGE: + # if assistant.is_python3(): + # self.rev_data.append((chr(val)).encode('latin1')) + # else: + self.rev_data.append(val) + r = False + if assistant.is_python3(): + r = len(self.rev_data) == int(self.rev_msg[-1]) + else: + r = len(self.rev_data) == ord(self.rev_msg[-1]) + + if r: + self.rev_msg.extend(self.rev_data) + self.receive_state = Recstate.RECEIVE_CHECK + elif self.receive_state == Recstate.RECEIVE_CHECK: + log.trace('got check') + self.receive_state = Recstate.WAITING_HD + if val_int == checksum(self.rev_msg): + log.trace('got a complete message') + return True + else: + self.receive_state = Recstate.WAITING_HD + + # continue receiving + return False + + def packageAnalysis(self): + if assistant.is_python3(): + in_msg_id = int(self.rev_msg[1]) + else: + in_msg_id = ord(self.rev_msg[1]) + if assistant.is_python3(): + log.debug("recv body:" + " ".join("{:02x}".format(c) for c in self.rev_data)) + r = BoardDataDict[in_msg_id].unpack(bytes(self.rev_data)) + else: + log.debug("recv body:" + " ".join("{:02x}".format(ord(c)) for c in self.rev_data)) + r = BoardDataDict[in_msg_id].unpack(''.join(self.rev_data)) + if r: + self.res_id = in_msg_id + if in_msg_id<100: + self.set_response() + else:#notify + log.debug('msg %d'%self.rev_msg[4],'data incoming') + pass + else: + log.debug ('error unpacking pkg') + + def request(self, id, timeout=0.5): + self.req_lock.acquire() + if not self.write(id): + log.debug ('Serial send error!') + self.req_lock.release() + return False + if self.wait_for_response(timeout): + if id == self.res_id: + log.trace ('OK') + else: + log.error ('Got unmatched response!') + else: + log.error ('Request got no response!') + self.req_lock.release() + return False + # clear response + self.req_lock.release() + self.res_id = None + return True + + def write(self, id): + cmd = self.make_command(id) + if assistant.is_python3(): + log.d("write:" + " ".join("{:02x}".format(c) for c in cmd)) + else: + log.d("write:" + " ".join("{:02x}".format(ord(c)) for c in cmd)) + self._Serial.write(cmd) + return True + + def wait_for_response(self, timeout): + self.wait_event.clear() + return self.wait_event.wait(timeout) + + def set_response(self): + self.wait_event.set() + + def make_command(self, id): + #print(DataDict[id]) + data = BoardDataDict[id].pack() + l = [FIX_HEAD, id, len(data)] + head = struct.pack("3B", *l) + body = head + data + + if assistant.is_python3(): + return body + chr(checksum(body)).encode('latin1') + else: + return body + chr(checksum(body)) + + +if __name__ == '__main__': + + mboard = Transport('com10') + if not mboard.start(): + import sys + sys.exit() + + p = mboard.request(MessageID.ID_GET_VERSION) + log.i("result=%s"%p) + print('Version =[',mboard.getDataHolder()[MessageID.ID_GET_VERSION].version.decode(), mboard.getDataHolder()[MessageID.ID_GET_VERSION].build_time.decode(),"]\r\n") + +