From 038568c7e3b861656c24e02c43234dac57e109d8 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Fri, 3 Apr 2026 21:00:57 +0200 Subject: [PATCH] finished toid_interaction node --- toid_bt/behavior_trees/behavior1.xml | 23 + .../behavior_trees/calibration_routines.xml | 13 +- toid_bt/behavior_trees/toid_behaviors.btproj | 42 +- .../toid_bt/plugins/send_text_action.hpp | 43 ++ toid_bt/src/bt_executor.cpp | 9 + toid_interaction/setup.py | 3 +- .../__pycache__/__init__.cpython-312.pyc | Bin 0 -> 205 bytes .../interaction_node.cpython-312.pyc | Bin 0 -> 4581 bytes .../toid_interaction/interaction_node.py | 97 +++ .../__pycache__/__init__.cpython-312.pyc | Bin 0 -> 400 bytes .../group_sync_read.cpython-312.pyc | Bin 0 -> 6784 bytes .../group_sync_write.cpython-312.pyc | Bin 0 -> 3179 bytes .../__pycache__/port_handler.cpython-312.pyc | Bin 0 -> 5587 bytes .../protocol_packet_handler.cpython-312.pyc | Bin 0 -> 20947 bytes .../__pycache__/scscl.cpython-312.pyc | Bin 0 -> 6216 bytes .../__pycache__/sts.cpython-312.pyc | Bin 0 -> 7957 bytes .../__pycache__/stservo_def.cpython-312.pyc | Bin 0 -> 766 bytes .../__pycache__/__init__.cpython-312.pyc | Bin 0 -> 215 bytes .../__pycache__/sekvenca_2026.cpython-312.pyc | Bin 0 -> 2419 bytes .../__pycache__/zidovi_load.cpython-312.pyc | Bin 0 -> 10590 bytes .../__pycache__/zupcanik.cpython-312.pyc | Bin 0 -> 4610 bytes .../mechanism/sekvenca_2026.py | 63 +- .../toid_interaction/mechanism/zidovi_load.py | 582 ++++++++++-------- .../toid_interaction/mechanism/zuocanik.py | 105 ---- .../toid_interaction/mechanism/zupcanik.py | 130 ++++ toid_lidar/package.xml | 2 +- toid_msgs/CMakeLists.txt | 1 + toid_msgs/srv/SendString.srv | 2 + 28 files changed, 708 insertions(+), 407 deletions(-) create mode 100644 toid_bt/include/toid_bt/plugins/send_text_action.hpp create mode 100644 toid_interaction/toid_interaction/__pycache__/__init__.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/__pycache__/interaction_node.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/interaction_node.py create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/__init__.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_read.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_write.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/port_handler.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/protocol_packet_handler.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/scscl.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/sts.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/stservo_def.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/__pycache__/__init__.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/__pycache__/sekvenca_2026.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/__pycache__/zidovi_load.cpython-312.pyc create mode 100644 toid_interaction/toid_interaction/mechanism/__pycache__/zupcanik.cpython-312.pyc delete mode 100644 toid_interaction/toid_interaction/mechanism/zuocanik.py create mode 100644 toid_interaction/toid_interaction/mechanism/zupcanik.py create mode 100644 toid_msgs/srv/SendString.srv diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index 38e29f7..e2a1bdb 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -1,5 +1,14 @@ + + + + + + + + @@ -70,6 +79,20 @@ Action server name + + Service name + + + + Service name + + + Service name + diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml index e19211a..19b8c42 100644 --- a/toid_bt/behavior_trees/calibration_routines.xml +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -26,14 +26,14 @@ action_name=""/> - + - + - + - + @@ -102,6 +102,11 @@ + + + Service name diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj index 501f4ea..054a172 100644 --- a/toid_bt/behavior_trees/toid_behaviors.btproj +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -5,45 +5,55 @@ - + - Service name + Service name - - - Action server name + + + Action server name - - - Action server name + + + Action server name - - - Action server name + + + Action server name + + + Service name + + + + Service name + + + Service name - + - Service name + Service name - - Action server name + + Action server name - Service name + Service name diff --git a/toid_bt/include/toid_bt/plugins/send_text_action.hpp b/toid_bt/include/toid_bt/plugins/send_text_action.hpp new file mode 100644 index 0000000..bcb18fd --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/send_text_action.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" + +#include "toid_msgs/srv/send_string.hpp" + +namespace toid +{ + +class SendTextNode : public BT::RosServiceNode +{ +public: + SendTextNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosServiceNode(name, conf, params) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("text"), + }); + } + + bool setRequest(typename Request::SharedPtr &req) override { + std::string text = getInput("text").value_or(""); + req->text = text; + + return true; + } + + BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override + { + return BT::NodeStatus::SUCCESS; + } +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp index 5db62cf..889aede 100644 --- a/toid_bt/src/bt_executor.cpp +++ b/toid_bt/src/bt_executor.cpp @@ -9,6 +9,7 @@ #include "toid_bt/plugins/move_coords_action.hpp" #include "toid_bt/plugins/rotate_action.hpp" #include "toid_bt/plugins/rotate_towards_action.hpp" +#include "toid_bt/plugins/send_text_action.hpp" #include "toid_bt/plugins/stuck_detector_decorator.hpp" #include "toid_bt/plugins/translate_x_action.hpp" @@ -61,6 +62,14 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib")); + factory.registerNodeType("Seq1", BT::RosNodeParams(nh, "/sequence1")); + + BT::RosNodeParams service_params(nh, "/sequence2"); + service_params.server_timeout = std::chrono::seconds(15); + factory.registerNodeType("Seq2", service_params); + + factory.registerNodeType("Seq3", BT::RosNodeParams(nh, "/sequence3")); + factory.registerNodeType("DetectStuck", get_pose); std::cout << describeCustomNodes() << std::endl; diff --git a/toid_interaction/setup.py b/toid_interaction/setup.py index 7bfc388..6473e97 100644 --- a/toid_interaction/setup.py +++ b/toid_interaction/setup.py @@ -24,7 +24,8 @@ setup( }, entry_points={ 'console_scripts': [ - 'sequence = toid_interaction.mechanism.sekvenca_2026:main' + 'sequence = toid_interaction.mechanism.sekvenca_2026:main', + 'node = toid_interaction.interaction_node:main' ], }, ) diff --git a/toid_interaction/toid_interaction/__pycache__/__init__.cpython-312.pyc b/toid_interaction/toid_interaction/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..91a3ddac982c278f3b019b1c43654c16efe646ad GIT binary patch literal 205 zcmX@j%ge<81ZD@$XM*U*AOanHW&w&!XQ*V*Wb|9fP{ah}eFmxdm7`x&nx~(UUt9tt za#QsSQcDtx^j-3kOLJ56N{aOZit@8klYtD^(xUvN{1PJrBQyPy{LB>nq|(fs6fiSB zGp{7IC^5MtGd~YgARZ{4SrQ+wS5Wzj!zMRBr8Fniu80+AIU^7kgBTx~85tRin1L(+ DTX8vS literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/__pycache__/interaction_node.cpython-312.pyc b/toid_interaction/toid_interaction/__pycache__/interaction_node.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f4f6b278faf9f5e7fdc03eed1fdfe57039f384df GIT binary patch literal 4581 zcmc&%U2GKB6~41OyL)%NKQ?P%7h=c64rCKzCx+rsjPoNy5~R8aLy<~d5>y|a+}oV$T2NhPDv?8rdqn3kum}a zkq-%rUnMM|gzkjhv!!;HVr^FisYBVuk{4bhgwqgFI_`L%CXiWSRw$$&}%-m>)}d~&Gr|PcEp^nDBPy9tm>*I%XVTy)tPLnljWkFE`gq<$fmmkN37b(#x`>4 zoE+V?5|fH0YsTax93`sygaLshnu<1I$7Q*s6-`r?O~hNzmG7(p zKCKU&H%tqfvNBLntRf#6HA>TEMYqg>liZk6N+2AY=Egb08hmT;(0~QT88|nsYRnPg z&p2R4Lv0=iOv*YSy}vTcJK*rS{KgpQ_sxP3GJgxrw^zt2O&P>aic#uB>YqV?IBi7n6qM@^eTazIJ&rx|G@b zyTiXfdi&@fk1xIP!8h&O*CL?sl#+P5s@BupwRHC~eR-wt&3fO#THnE{c%Sa!U9fEl zUFSay()}7nZXv1qGl3qQGX!7WX*E{1a-r|&abh)ymxY>i({ve}(u#OJmt79eho@43 z<2{junF#ddeFG$DAYHkkHXnj)6CHDs%VYdkk8wLS{qOI;f1C#iyo!6eR6|cGX2sBf zeh_LLvBdkJvFV(bUJIh@#27~&G%cHQf5{u5Xd;Ar;BkG5d2+66lTU+$_BO8iF*H}m zN?Yc};cJH%gkO%VW^=bjz8G1`_TEeOuF`b%;`R7ycF$s8{gorNSB@-YhpTjjw%2LC zM)M1M7S1ewuuS)@bZ&>RkFR8QRx^%E`F_}#zX{DV&c%;JbFGhnE&lh;1vfUb7M$We zzQY|iw)vyY{+S%!+R=e5Z)Nm!N70Pqq4{5Q_x0SacJ# zVq+W#in#dt4M?{IJAS&zApxBTd(uu=;uq+AvN25LC=hpm`43I=U8Ey?dzEJ+##EPBpqh z(@6dc!ZO_ja-HTN6)kiu)7_8gwwpt@hQAnIracdvGmE8rG`CD&Z!BCKuR^*iY(nQV zm~0Tvq5lget%mnsfL?J65&}Fiu}MPn;#32nU~cbW5f;503!qM%!a88Xvr)^+`U;Ht zQ3Ny>*@VUT7#{>x2axI$h;WWM!+|CrKs(wSnel_z9KZ(Yun=*m zuL(o$C>??+6TOGD=kXpEn9o5XmS`X0vY$hbhYSvF3wv(y8d2EY6Yk z-5ND^%1sk)#AASRw2htes?d_6X|h}p_-+_Ak+a@&Ge;6tvB97DE;QeMMpoK8u8dZD zfAOQIp?u`%lS9$S51zEgBDu9~$w+Qds=qo^dv$1yz{}Ss>Z50Cqi5IgQZPE*dG2`qWg$@VwJ6r+gR4D)Casf&w3>pfU)i&$v~D=b?q7ge1fhsm?>GxtLSrIZ-| z{KTgxZjRm;^N-qgFST`D8TH1h*0R|724$=C1_5xhC{mzC0axWt>~ep9(H1J|sg zjJFN98gUZB-sh}wlLB0Oip&XOyZ3p@UP0p&Id;m?ESr<=7h@IX7}v2De+OP~6Bj{P ztA0m^-S8OUC*c)6X&!@SEi4Ga-$?QS+5LcY|AX}YlYIDqy!(KRJRp5f#g7Cbchh=G huwVa4C@O4U+eXA>b?&d>{5K-~?D$%kh`XF){sU!|syhGx literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py new file mode 100644 index 0000000..1a2bdcc --- /dev/null +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -0,0 +1,97 @@ +import rclpy +from rclpy.node import Node +from std_srvs.srv import Empty + +from serial.tools import list_ports + +from toid_interaction.mechanism.sekvenca_2026 import okreni, okreni_niz +from toid_interaction.mechanism.zidovi_load import ZidoviAction +from toid_interaction.mechanism.zupcanik import ZupcanikAction +from toid_msgs.srv import SendString + +class InteracitionNode(Node): + step: int = 0 + def __init__(self): + super().__init__('ToidInteractionNode') + + self.find_sigma() + + self.srv = self.create_service( + Empty, + '/sequence1', + self.sequence1_cb + ) + + self.srv = self.create_service( + SendString, + '/sequence2', + self.sequence2_cb + ) + + self.srv = self.create_service( + Empty, + '/sequence3', + self.sequence3_cb + ) + + + + self.get_logger().info("Service 'sequence1' ready.") + + def find_sigma(self): + for port_info in list_ports.comports(): + if port_info.vid == 0x18a6 and port_info.pid == 0x55d3: + break + + print(port_info.device) + self.st_motor_device_name = port_info.device + + def sequence1_cb(self, request, response): + if(self.step != 0): + return Empty.Response() + okreni(5) + zupcanik = ZupcanikAction(self.st_motor_device_name) + zupcanik.zupcanik(1, -1010, 25) + self.step = 1 + return response + + def sequence2_cb(self, request : SendString.Request, response : SendString.Response): + if(self.step != 1): + return Empty.Response() + zidovi = ZidoviAction(self.st_motor_device_name) + zidovi.beli_zid(1) + zidovi.plavi_zid(1) + + okreni_niz(request.text) + + zidovi.plavi_zid(0, TargetPos=150) + zidovi.beli_zid(0, TargetPos=450) + self.step = 2 + return response + + def sequence3_cb(self, request, response): + if(self.step != 2): + return Empty.Response() + + zupcanik = ZupcanikAction(self.st_motor_device_name) + zidovi = ZidoviAction(self.st_motor_device_name) + + zupcanik.zupcanik(1, 1010, 25) + zidovi.plavi_zid(0, TargetPos=150) + zidovi.beli_zid(0, TargetPos=150) + okreni(5) + self.step = 0 + return response + + +def main(args=None): + rclpy.init(args=args) + + node = InteracitionNode() + rclpy.spin(node) + + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/__init__.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..99ba63eb5984c5fa2a2dbf4b62b194f43a75b0d4 GIT binary patch literal 400 zcmZY4y-EZz5CGt0*E>XA6cI!a?S!>3a>Z357TSuSeb`Ha7jHKcCRycLU&3dw@EvT$ z!hTsRD?8z=t}^S-DmcaD%M24Hk4Z8|LiF#S^g}pbn#)f_-RWQtvZdyQ5wI}@Jvc8x_z8>s|5?_`7kpwS6HoD1P=moZZk1bzm27Hgwk?1HiBQF z_NrR^Q$dx86Dw{Dma6uEQ9@pG{^40<6hq zrKmF7NCCAHj#Q~+UkZv=rTfzC%PuRew5j%?z9x~aQ64O9wNl<#veHU@**)iu$AEFF zs#~r0V$R&ZIrpA>&iT%{^H+<-$Utda|7Q5_4u<&~Hq@d^R$f#>Wr^V#o*iR;t6>=p zbC%&XcNkv#P@~Mb!p`Vi`sXaPT&&F24J2Qs5A4PAq65eh<72L{Jaa|Evv-&)TI6#@ z$7_M=c^yy#uLqjN8-Q|r7SL>-18U^6ftq+DP&01=YMC*+EVAXS5SX0kn(+^G`n>#r zx@lYxH>ZXF7asyyVuEat@jy)}K@GIjtua9@w35OwASkr(O};3JYJp`e4;3uc(3I3jQRXS(skK1<`pFm z^iPgSvTa!OOn3$FxM$$H*FWUrWj$@m+4L2DctDbk17kj~Z~2o>cKK~o@^ zmv>Lh$eafl4ND%+Y{4!yy48L>TCo|(Z6==Q2$v^3O)&yH z!{X^>8V#~1fDOOP4QDhiZAV-8szx~l6HI`5QhI5R^c*;kB7B8g0wbB^nPNfOm&$rl z0hN>$Q7GsZih-q20%Vs~O@ePcFqPD6@ov5D7^=ees)11H!j8|%?w1kM0aPz&vSYp@ z+!!%eQrVtzrHWvw4Zv$D!O%Kh<^UrodRp0uXaa-{fHkTxSx;eTm8B{ZaV^D8g&e5P z7VVZ~8?p|;sEAeuf`G$P3mNGXYaxa+C=R~?XNM{7Y3 z1}TN)U8qRJ&a}d7CK=HTW5%=rXo@!INAFE5>S`?V#DhpHBe`iE(1e+I9%%)7@wC$l z^k&%2PjM~}b;?p*47lZUtarOivKE4rqHf)&Z$^~!&YW*=_jH`^_B8i4|NPzNx87|P z&~qt>358h0P$U}>`Y}j%Pj81Mp#p|>Sc=R^(-*u0qdqA;B&hxM_`2HPE~Q8*2%Tf4R6qdjrL0b!$uJ{mxEaylHzAlJhris7Cza}H`T96L8tco;Gj~p$fW`@P^PW)F9dIADz=QW0MV-CUs6H z-|y-)cNhsc&;j zLxyi(>RHbe&vNentm%GJ%z2nN53les_bK8&^=IvR%U?SF)DdgBL|QIwv|QdeePzRW zm2gEN!vcpar$0Iw?v9lnCZ&g0yri@~YB~}xDf`TQ-@V0XEr%Bj@rL7HIlpv1>RX?B zEXB@y$a&93!w1CXUeGP&Earr#h^=PD5Vse9+P`6U#Sa`>XnE?W+RTENH*JiqAY|Fp zGmr_kQ2)*kS@!y9D(dxt*fi>;A_#OOi|T9C9#Vdb_8@3b(_lj>4_4n{81=0#Mv01A z0m2F+xREpfNoH~ag)ro*>d7cgcdD97i;~+Tm~4q?E}*O2{H4;x(vSCt43O7+(jPTd zz?p>F5z^-7=RW#nM2(zi4{AZ9r0Db=MW-QVPPZdUq|(tjJd zxI4LBSLe=gJNf`qlsD;W9MZY$AUDL$>4LgU*%5MqKYDH@jEZ#YiKYoJw+|q;G34?HDiyGG=*2k|$dM|x_GSYu7`p&h4qj=NG*q!0a8+kRGc}#9`ICs$)egmP*6}7$^ zx8-lvGB(F2o`|(7!c}b^V1PY`Ny+G1#e?yv^;le8FJUW+n;jrQd+6=BV_&$BIBG(z z+g+L#iTg8;LGRA>U^y&F(16Gs)KCb6J8I^YgA~Z%3jix&fTrd9W~#0ZfI*>C<<4VL zqB7X=RQN0KiH-{CgGQSdUtCqv0P10KR`Mg#?hidm3Sb%* zo2R_PW8Q0HzS*?9*8>X5S(^~VYC2DF^<|>!+ z^Y0PB;$aIhvaQNr@7JA=GYrJG~Z|@v|R(&AS;8c=&3ZZ1LC>&cJ)A zFT0AvXASwJ76`Vp`Mcv|vil=^V~_^OxTzpq5j8mhu$N9Po?5OUcGt>*wW`?B76QMz zmi3FIu8r8wM$Bj97KnIJODWE6T5O8h4-or-72Ov(t2t{^r1sRKo9j~S>}3MKH!nx7 zTqSQ_CAIzW@~Y2s9^|Y{k@Cj1n~$W}n->ZEnl45OOLx_?H&TZR!`&uFKGp!x&|xO@ob`^`t!mEg|Uj`q~iD*|CRqs|9UTJ zeE0FCNYCY1&k%uM`%vWiFliqqjUzvZZ6(jX&}$t&5JzhPYGfSw6v#SUIrypD?;ZDf zJhIW_84vK2V^}wPJU1r2W6FqtAA*z*Ci}2fh6FztDc1d;`cGW=mt#muwYf>1(#~vhkGE(^w7>Z z03)5ONnih>#H?2~Pq{&0l1-wde9Y&4gS1n?8_D+C-*G1txlRceU;s>v5etB9YFU>3 Tr-5Oe|I&48*!(R9DShuhPtIP^ literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_write.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_write.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3ad1f28670d2ba484d71c0581378014395bd5ed9 GIT binary patch literal 3179 zcmcImO>7fa5Pr{pvAre^Nt*wJ5J-$eg%P4rBdFW70mPr7ibab>SsCw=IPp*Vb{*o# zjW}>fXr+KuDR6=#R#dg>r9D*~sd|7VM0QskEVUPIG^tvNQ)k}Vo5TcEp>|}?oA)f7EUwD2iglZ5r4j$WyR1UYX00@og1(y`Zh(4GF6O95|b507o?~f z6%%r5R2|dn;)*yKWznP<8;hn!Wl6X5uI}Qiq`I za{*J(%x<6;*8?n*3XG){6j;U zEjii>iWSI#ES_WHX#KQt6EW*!Il5FnmA+~hMz@MyXTiF@uRgVbEqNO9+n5J5FaenE zh)zz*DM@$8(<-!i3#;YBimHflNkO~2yO|fqT*>H!%%L{ZQWZ3FW;-lYE&+q6c^ejH zug?}e^(7-frKYyU)Xmf?vAYK5juso57lSv0cl#bU_N`M&vCNv+qHSoyO z!@qIG+4<%m!4Lf3LBiV57wyP8@w+y`KrqU_frM(0Y?wQMJ54aGd*sc)Ujn^K>@EnXT64jjh_l3Cf0hP)i(TP0}B!`OE5~mSFKi;xcBv~>< z@xUCyyo!`M7=P{K0Oqm&R?#??x+;Q|=#MIb(Z;GF=)&k0*8C@afk)C-C(gjBGjPEz z*$it&h~E}^hH{u8s{8`$fQAM_Zzo%FW zuEx7h_u@-}zwE-(XHtpm`csGX!TbhL=}vX}Y&13@t1rDWDicGv<7;47$y2PH3$ydH z1#ge$?OFEjUKV=qZGSBEKW*rJr--Bk;|LCrV#n_megt{(Jj_i=%jjyFtG%1Us?iR&lTFN9)_K4!=nP|e8 zVRi5khL?7rv=iAbWQaHRHn2S53i%zw@bSSJ3JN;z8d}{(}f_-4M&24X7ZE@Ip zSL?V$?K{>SPJ7uVzA@yt=mIZNqh5q7Kjww=uwibHVU@x}R4|gyt(4L~ T9fY?3Z5yWa0$nA%;XD5U>@#dJ literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/port_handler.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/port_handler.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..512aaa271fec20ea0288309131a45783348bd4f7 GIT binary patch literal 5587 zcmbtYTTC0-8J-!>m;u`uFeVVL2260Yfi|LPE+h*HkkD>u8d#~JN~Xp$zz}1TGh-4g zSmhzAV3mlVZcEfB@|2CT(&n*`+dfrorP_h5t;s%6wNjP02-1h{OZ)$4JcEr%s9DO>* z9Wpo0%Z_n@V=gi>e~*y`#re!(o-tmsz=oW!a4_^%n01`4MG74YnFrz`+1k)1$AxddpbECMZ)OM$xNGN5kR1=J(EftJf2pkBEg zXoc(rS}9imt&%H&`s6C0)eF8*wc)xFC;FvuOpYof!sT&@eH3SRkuoT=i;QG1>-J8V zwfiy$qnrvDhK_ja6*{t14622lPHSxuM*qXj197#aG9H4K#+C8{RH=aiUH zF)%pP|M5p-()s?62d?&y4I1J^I3bg;t{9bb;mA!zmoz<0bV*ld6{9>ig@brPH!Acy zbU~U^h%~VPFNtt+LQ`W?M(G@AMT&*tHHW5Q&e4&lGS;(?6PJ7=8Se0 z$XzCTs9~AE@BXH?6CR#ypnaL&5}UG30eseHTiWp1xLw;wm+M;LQJ(c3TIL^!yKn## zu|SIhv@^xWN$9MD&1UjfaR)S*wb5bDAbpnGIm$d`M?#$8(iHtXSneuVj<{gMQ57xr1bhS-sZHod4qrAdF=hMLnd2VOz$LhFeoyD<*oSkQ}rIn?W*zgX1x?um?@N-ZY%wfuMxH2^+-43fdT4a+D@(u=>OxKd|Ke}Cc;s{Q)Pog<|3MQe6eSnv8jEV_DprRXtx{+W2 zC*?p4&qq*kj$F7VjSXKOyh;F>m^!KEJh@8KL{NbOD=<0H(iD$_GBnwc@5lRB`{7ip zcnbtGfu3}rXESiZtlOH~GR-~d=AP$E>E?moS7(NAq=#>$!g6|8PB$rA(}{FbVwwM1 ztS?r(oq2oV58g}dn4|3+Zj}rUf_68RldlMVR#PGc^4IK_@Uj|>s#>!3U0N{r45BXT zfdI6MbkPLK-Imk_32e3{9f84rm-L{BN8_DHuPpY-3*3Su#GBc3YzS&tjL>M(Q8gA( zM)B4`AyE{`y{cAd|N4B%_bm>i$e2DsWS2MN3Zz|uO;_87oas1~?l|?rbt>E1wmh)q zZie8p?X50OPKaz8BrqbaGrBE+jiF$V^F?7uS4j-+G!wQcYj^R-EOxD;Aw$bNpCDs-Dyq)Dg00F78;1BjUQUMCEp|Q7WDS%~kyQZA6 z!|2{Vc-y|Z7tGS{E_*ZB(-EAs&0tvF*JSn!?pbaJsNJqBWCx7wz3_h+jrX+~yn^j( zdlJYzymbIS2yR(06NqHj7~tnl)6YY^!Oub7H>+R~KN)ChQZY)<&=AVU%*4^R+Glo{ z_P$V=c>}~tk>P~P^e3^4o4rb%J2~MlLd(xjxj6vaahuB0imH}wfEEBth-`$WMyQ_-SBr` z9abgTO8$9G{lRRt|Ba`lx(vPth_9=dTKW}X+q~Rm#J059 zwj~C)14lk@|MJ+=W9h*6f76q0`Oy5B;Pn+F(RN$B*#Q#4`R1WQTLgz%;NVUYa;aZI z?W*1l0_lVDMrD48Wb}9985?9hy$NrnA2Kp<=?J6x$yyHmpo-8)WBXxtH5p>D^?3T;T5cATX`C$2W;=6K=kj++%a zaPvI7BXY1&6a@cHIV+smaSFoWopPU0y?c}wYRrGeKMjfG2l(GVMuPVwL!66-b*QFi z33?X!5a*on3H$lUymRCRY=wx#XsDCd4whwioeb-K$#`BeWiJ`;w(oF? YsmNAT-SxcTr&+eUcxpHKd|HR*n@3s@B?hXo*D1#YGhhA*nm7O;lYfU z@up@s@|Kgqn@KEZx3ai)DvPs|EL%INc=D(?sjbADq!KldLQ%O=RAqj|e^L%J`LmT& z@}1kQ?v{{%*+(j`uHxwSz31M(k8{3z>9yRepXRhpYYZ-wt0t!EygcH_6MQBtCr& ziF=&D35F5wcZ>#3FkUp^&nTEKngs5mSukI;2$qXh!Fn+Vf4q=$F&FuH$hYC%F7Oxg zh1`n;M(zYBP5Bb`m`9Sdz_y`|2Xu|@GgfQ8bQs13DhE(L9K!X6g>q+UqN#P9yCwL1+`HR z(PtZIzF-F}5b{ACLIG%@-~cTW3PCpsMW9Y$6KJvE1T7JYK}&@a&@!PEv|K0y-7J)Y zZV@(vRtQ@_D}@TstwJSem9Q1GTBrhb3Dux(!3A0)xIwoGHK4V^Hqbhu7PMZd18orM zK^uhzP>;|E+9Y^Dw+l_6JB00^JB1yf%{O;?niFN$#Xu-97#Q(h_YGe2hrC0+F=50n z4k`=5CXl9_?mu;b+~dNAFy}=|zQRV_C8wIZ$vrgmc#H|#^?(@alG{q;1Vcfue@yV0 z66T&{XNmL zyI+_-6tlaRoyC!^HAX_imPc|8T@-V}1{+XR8C4FajKE{+5eenkFhm!n+#snV$fYF? z1%iXYN%vY(z*x3Z^(qk7|A_BvN747`BT;ws-2K|Sr($;3dOd#ym9vSgV7S(ZJ{Dj_ zjI=gkqx>tF*lNk+Qu2~K`m%>+dA!g50}l~tBVv-4VbFFO^r-Ke|IL6XOxCV7?j%Tb_a|pB zNmZ?qqcz62uA%6J30lwlq1Zj*j${{AEgxB}<~cweOkz*rbo=ee7{7Uq zE+w%?FiZj)WFbPwpbk5!W2<{Nw7Pdg!|V4>=6R8NCqp+)U*FxFXuITa$9VS|U8Etc zqX(9xj1wL~&UHf(p(nC%KTN{to2!Q~61a3TmGO0013Y$mp(r%E8=F{>i5=7*jLXg_R$5u7=$@7*_2He zSM#2x<_Nj59F9$E%=aVkknV->D_;yj4L1$QqZwnDl4nF74MCr0LY_4(&x|~>i`k{r zg)D}cWlxz(o)vkdlECHQX)58^3JDRsKX1_Po9V=`m= zo)q4mz?&|x`iE}}k6lT9gnP0fg>_)aFZyY0V}W$Wx)et3`6*p?Pwq`&OG=7W{ zjBxxqnb)q+5T0^*e2h(uJHdH!6ZWIMXU=&0&mTS7+25ZiV0UMG`v$y6&iB8nln%V& zJ>JoMTDj{}?(!Kg$~yb{dixR%c8kK7JGuwDdroK>1Knpjd(RJO8Ap5j`p%ynNNi$N zdU^-E9T(7k$C1;W$&JG*GT}&3|9mW#CO4B=H@OtHUWi|C7=$3M=>6B`ZSJ^pe>~s$ z{@e5Ub#Z4KCByUi+v3hnEqQ{HQ}g+?apz%5zB8X+A9o(ok_WZq0VEg8JoEXUxbx() zvt$ju9YQ}+^_H+uOw(H>FGX*aycE4v@>29x$+O(FdUARuYrfD={z^X+@GrW$$F7fu zTsHzEA>S3h%P)!n(LGs}%F{7+WyJ3a`p1&xTT{z>Z}`O_zb}+5u3TN*i3XI$GgKZ4 z_yna`JcRl1SP})0pd;M_-V42_2Rcr4ChW?6N6(4Voe76>*L$I}udB0TKxL35YI zL$*34dTu!#HhfpH1*R-vQ`mA1#`1<{g*;<+^>ZQeca^kl z{6yZ_Qv=@9ojoT8y5wjFKOFRq2eEmGFM%ylIPl8Zj-#hK2fSxGULi*DC}rEI8nohA zXJ4!SrY&K?UUN<`$XD36S;4S!y!K<#SVJ%VGxg1_Hp^pguG8C_`xCn;I-iR zXu=r^gnVk8XT(2tB{Y;U4GR;A5^4z%gk%gzE(_9l5R$Os2W5} zkYE~4Fdwg~|6o$8+BJ7hs%l5XVcF(bvQs#Q{-S-IlP(V|#o z%bZ)P+`Zu3v%(p2>mx_w_Tnf%$ns zpExSi;EE!Bm==zsd1V;U1bUCV!cFNAVhZueDHHz8f^o`{3oW@yylEG-B?C@eg@>7b zg)Lz-(@muZNj)%dAz1J7rPB^`*bMD>jpL>bqA6rcav;`%SZSJ$Lm#GKJ%+6e)y!lQ z1`T)YlmlK=P024eaAO8D_l{MG5Bx}vF^to44OkVdM81kNl&`ZosT}Qx$TMUYt7oNp z6g?3PFLJD(WrRL=zJ;-;x23jB{gZpiu%LJ&oKu!NKVtI(oD-X~tU(6!Ng|f$MDOJ2 zNKQyamMPa~r2|+HmK(z-MmXtF9ErVZ3aF0)aF_vfVfQVFZ+~Z(yn6UaMC@89+ujuy)3^h#vH<7|u2`gc3_c5`bZVLO- z@Hml6#fmLF9F&1?V0hFY7!Qe^^t9td2z12(5D%{)uc#oe*hNLqBrcIFPIwi(wY}*> zBw4-@zeI(G;UH}y3}-c{71xE@B*%SJ4*rt@wIvb$lcKE=s|>ouC6U~CX<5X!T(AiM z_+Ibr-srKV${kYWj-|@IQsv&cYmd97_A?8WJ&X3w47xxb5SYr9GWbiWl~r`TO#%&n@73 z&U3|OQTNyPgZYC}sl0J^&wP2y+$(={RodIVc>1;Zy|2Y9Yd?5Z0!-J=SGEI*x7N*^ zi=K*?yJybbFNo%%A~MUj%}jihCpEl)iVa;)+N6fl^JQn^_01nnO7#c+=%Q4A;)z?T zKNYosf4R&x^TK>t^KwP?QpI+uV*66X3sS`kixn@%s~cxKe&g+@p8XH3pLq7qSmTxM z<;q&@6;<12+GeiJbvy_xY;DI*QPLVU&;Yw;&pkLLmABmIu~U?^F1s3|H)r=q)h*Lq z)GgPHZPxd|A(gexbeOd<eB+aHlP$F1AdOHavARXHcD~ZZ29>w+!L}z^iNJ zvB=PPTz*Gz3B?|}_&R=MfEq$7VL#FbZ|Z1AKSMnhekR~bI2aE0^bho%KRVFe+mp!c z#*McRR%oIS?`Kk=FHu0;XFK{j&U6mINENTsL#~2&EDQj}FHpt^k+Vd)Kv)or0i5!> zC4_L1VoGL>8*4nv)I_d=aAhzi3yUHa=;5Z)dt2{rjZQ2SH3IR}Xh22u!p!-Yvngh8 z0#km;`xjt0eqf)s??^S$;?7h~d-40dlCAc`T(UUrg|fZ5WZx;-cg_xd^!5{jw69xg zKKbOF)ZFuT_TJ?*7T==170+`xaYxBFE9V_`v*jPvOP*hN z^vVw}OD~?8_ne6rmORfPWzXa6n^-7gX6VBkQhocQ#visz2Tsn{pNtn*e)CnSxFOQH zOaL%b5_8nY`1&;KqVoxIxCYbJXlOdO^lOD5Ajrm@gOE1-lkbo zeag{%OO(J8kuNHN^1#nazrw15y#`Bm$}(kz=vp-!kEzCf)Xk{w0_@J@sQ^|#Cu99` z)X(56hI0f1Q>h%mLcd856<)b948N6t&SO=4IZ_*DLx^NY%br}GC{p?a5z->rT97s5 zEW%=9kTS{LVVaiU$6q)}j{_c_Bt!af=h{3G76@zF~NW-$HG)P*!9kt_H@ zJ})(r!74CchyOTPnJvUYrFgT%1?NEEc;QW|o#_e!|I@n>`rjAfmsK@^Z?$_4dU7CU zKd@Zkh67a!$17g#LTZb|S3a%XiBz@3yO(R~zti%dWv+3d<^_qbj_}j<(K@xEhV9>( zdN4J2;nBIrhJ}VBlD#(4J$)lOo)mki!%}stRNXqa`BB^VD<9o_QuJ|nq5AZ+ec2%^ zpra|qH?0ClKBu`V{#v0 zVrH5$Lrl#OkL2qNRXl)nnkd*9SavYv)Px}khHs&LhKLD+HzO2UiiRoNC*E-!&r z2almKEURuYGcH?8a@oo%?!9yOotcS+vew)7h#}HG-L~u~iR8;utxgYGr8JxB0tG$P zsa}m5Rt+=ANcgGn0vnhXXTobfbb~DrVUB7rz@JIxb64pwMB1g{3t~GKowwMCxvv5) zsNSMhG1DfGXp|+@j0jtF=s-v(@P;}CwsQy!DUCds$w#vsF znjuA1Gj6G9n}p~`17U%ByIfSF+j+6+Dce{2u6cRxjfYnk?Z2RtI+C*FMM}w}?lUCr zahgIa@ek}3h>cj8QKqk_)K7ZV%qX)!Sxwr`MLsXzVTHxGLkF-fLJt{)lxXk0hTQ~Vc*joGvL{E5;9W;rGuW6_ zCHg6kj=`;rTCJvvBhMjZylNGo7hV-^EFhy#XV&n zBpF%DPBOO~MG-T3kZE=eRyD=0xiIfJN+wM%nKbsKOe9U3`uX~obk>Z$@V!&FPeu6; zsy=FdTq^B45i9Rnw0AE%oYNPg=VOkV7_Ypc*5i~aF$Y%R6eNYBJs5y|DHe`JLo2}` zyEXrv5Q}X!)~TE+KAe+=T#c}iRODiE$A>K$am%Vno0bZ*2waNXS;9GCp2?l!F34}_ zNw{TWXG3tN+B%widUAE5`7Slb7~f15?dBYNwSBPlSWlH38`Y; zXuu{5sA|}hEpFaEdoJc|j@c0mSj(_k2jw!EJ*u0b8Ri=oWj%2>VYsD%~{(KoE*V^u`Qc*p=0_Ip8qNNzA!Ox zGVrb2Z$;Z@_RV%IIGSU8^J?>q^pXvLf@#?YfbP?NiHpg)aSDbNuvf`bP9Y%dAQ(x; z$Kb8dE}lflhdAuRwp*Gmya>FFABK<*{rTvd@R6+T zM^l)Os+wbtk_l%x+nD(@1l837U`5cS^BMmL`BRr zW2<(Ke4TV)(z3pwPhq=E({=#Qkf&|*{^;!SIZVz%)#0DP#C;xRp_8AqVTfc2iN6ez zK|^+JL_-v&YZ#jN9yLaRn3VIfsy4rklI$zHA!Q&}mfB0Zk}85<6RmnutyZZF{1rBA zYD30#lcqP!Efv3xqRbk?hPs}HC`@Ct)F;tYb`3drE#dR~nY}9apO933lGXZvm4ZzY;&t<5?p}QbF0^^Z=e$%lcrhnbp9H0R~ueT zeH77IPkmH2I(`;BjrP`3pP%8ZeqM#*(X}-dYR^U#N?}5fJDWn$bJ*_kmq=t^_6;f2 zp)3l;Wqe<%>_q8mhBgQw(=Kqa^9}e-i7#bUPQ;qn6spRp1)Mx)otie|LF?%%o5l5X zRbhIIfGXa-~1`xa}!sxd8xQwDz2XiEj8?y8umXbe&qYO>~ZbmH~zF? zq2Xk_rhchrw^Xxx?%1QE@1J-y@YwzF%L_G~pW$W62}68n7)|7ET3&lIR9E<~(W~q$ zyg|kOd~2LV-LtH*Nm*mW2lT+3vc`Il_%;TF#Zx zZnXauYpUO>D9yC?6Sk%lvIC?t517TE;i17xMwxucu^#y~4)Rr-FfDwi^Ws?zquwKD zIe4ocHflxcU8~&nQ<5 z8y`pDcZhP>yX8CX0l!t*!io*eJ}pD8?grydSB zjxIb*I@}LwJ(;YKupyqYhnRwfqXbfzRwC zv3Nu>3e~@ff-?;bo=bU_q-3i zj|LWY9*LD4{YjSpzr%#BBSxz0vW^(3eBYzV+P#s<+K6oNXo#5{^=LM$Y^@_vI?2=T zNNAd__2f}k8+}N>P+K(_v@%Vab+t1;dqcIan3+ywdat=!CuYzOyrE<|(T23F7<(v1 zAi0KiDAwl&v_r@F16pv})O6bMoxp>@oUl;;!b0gmfTOH3KUwDU98>9`NZxavD=gj^ zy8ge+U@!1820#Q-@t;6>Z&+f;uXPk** zM`!lJYfbS_GD5^ZVZ@5-itjgQA>_D+7V}7tbFl9fz=wP@e%Qt0ALR3}OCQx>(GH5C zPWHymo{RN+V<%r%^OHf79a7!#7=L2bnO5zzmfj&! zz2VoGC-ODJInb&h-T*!Lvt5SM&3=Q5RDA;OM$U87Vdk>f-tw5Q0n_!pG%BO>J0 z$!8))DMc{3decWzl!!gFKq4x3bLT`PYs*PYaVp%@wCu?@EY@t`n}#n zp4W>r*yAIVwtKyAjQd9960wA4XA6<7L~4oD6KNvSOoaK$dnl!alW0d4yNR45@;Z_0 zL}*DwGFHT2B=S`vw~5dmEB*=*nl|y*iQFghO(MTZj!lEdqjSp z$j3zfkjNhsc}(Qbi2Nmyzb5jxL|jDZ&;na*kv4Zm=yg``6Oebg6{E>gyOL+HRIXSJ zmdz_xv!!Iku8>_Tc7vsj3bqibq0((sT1f@Y6>G7jbY(}OWz$Nn3wg&3od%0*rN_9{ z(y(%<*s^_PM=lsjoght{P_xkq(qyo-tXNH!#+5uqGToX{ipPNDqi&VWvUO#P(ejdE zg*RFb5mA#CN)}j+B#udgH1*|#>Z`tAn4v^fuTh@4f!L7XbCg&{&{@>WG+<4KZ lTLok+IByli!D*L^C^x>T_?F`{>&+ZPY4q~Ha`el3@t^) literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/scscl.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/scscl.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..01ac8ddb9d519972190feba709f04df0eda70da7 GIT binary patch literal 6216 zcmeHLOKcm*8Qxt!ypl+*UbbXOw&Z8bI1VKxQWQTV(-JMp=0g@K+u0`B&>UK#O>&i4 zDzOr&;vt9R)D$QnrzjA&hgO1H)W;MFPy;Pc6%CLjs6xcTx1Mx!?F0pKDEj}qTt4(j ziZ(@i=m7jU^UrJN|G)YFnfa5`S<65gc=$!)5A_W57c7{`R?Iy895TxcVF;UKe#Nmy zjxFUlJ;#X_!iZL45pBdO+KElvL+m0?_J}ov7i&RQ2Qml9oFJ3pV&!S#RH^GJV^G7U8G&?CLQ7- z(kUJ$2gD=fpm>yYiN{E{c$^#(pW&D>h8%v6AxGlueT%Ug@x=U5*Rf3&N-kDopWQ_9 zCWnA6HmwM_@=ZI!9)ubM)YE1if&-x)p#$Lr!m|h`5uQUhh44Ir8=(hb5aDHn(+Fn} z&LW&c7(y6ExPahAxCjt~Un${NV|pqAEHiONWI^W~Vc%mw`y3$X9}p}62sQu&D*%EW z0KpP~wWJ2Hj?@A=NFAV)H~{O36Hu71cL|zJi7ByU4E16rR0^u->FWT?OvaQmw~DGQ z2_Un;EU=`cbe74OWP+LuYF1B#GtIKVNT_)|%`+}bkmA%7HSe-%t+Q0l$T2x7%|>Il z;~8ltnkLCO)p$utq!SrQ((1>kJU1JjPsgs&L?*874UdFJ{L)y+E>5#5@#IadZdRrlpE0U}Q+IcNz2v5IY4?n*WB^ie_iQ{9rS4HVHkXR0Gm3kH z%D3V%$av?dd_&Ik_4Ez6Gjf8sZ_FiTms){-ye9`=SlLv2umx8rU+?GDsvnE!avU+wK=B7iL!3AkeH9R4pat8}q;)JwKDs z^EGcHr6PAG?@-+5nk5-eui7c@bj^-^5=7%?WhH@D)~uOCDh?ZHHXa9Y=8i#G1xeqn zCyI_i<8_`+$ckxUvy47LxeJhG3N3BRp&y3s`SLBNRx-a{T3!0&PTuWXJNkwDk8V|b zJ%343qw$UCY%V&hzB!kVzNOw-_#4B_vp(*x#@}Ox8Rvo=@L-+|aZtL#O>xlV3ik#! z!9Ng2xtrMJ3O9|VmKpASz92OJ;QJqZ|L)E8<7Yk#KWzK-wKZQ}m{R#EiaxE%HJ~1K zPMwF$uAE{sC3kYan0pm^Epm&NMe72$U|F!g)gc6!DmB%($^|zCn8#(I7+Xw-XzDLK;q{J!L)3Ymk>zQ2Q-E^18|jAHWv0MOgu*^R+Vxxq_o1MA_L{9q!-_pAK= zyTdEv$hD;pmej7E2Z!@Q|AVEq8dbQW@>jO&5Y44i?dOpBZgprn2Hl}x;NOzUPI1w` zfKI36RLTt(>{~2h1wwnF9^8vg=od{wQw;W|%XwR-#B@2Br#|emr-In+mG&@+Ep0Ij zJrCXJIDmrRuhY7_+weDzB9jd}HI?lIWY=XlrGf_wFqOx<;R)wFVKpMIznaN==5qXy z${)TLdhkl2>&VATA1$fw@qE`M;C-R9`{U3@A@#Y7`OY!0C+>mBFQlJJYX8MGE`RLu znxwuytqRjBKfT?YR+#TU<2iawMQaYja|A+H$Ae>4B)6yJw-V{;-I-mBN(<Z~suZ2in;C;nE7QxmV?TjpbK{s!}li zI}Eccn=6AYDr-wB^KzG9UKwn6>R(y^T?At^>XvkCr~Sl2FfIlH)9qVnAM~UzAmDLY zneZU;7R4~#GS0WPCSS1<~u5mT*bcTPUAGYaV^mt z7bZiV(GgELB8`u_SUu<{*t!@wAiPg|^cze$ycY!+E|f+9*j*6A-~Hg-pPbDLCsh8# zc2|!cK>uDpWR{s}cux2kyU6Y0vnmdY6?ez7C*;c9aB)GGm7>$x0&w(s5u{pcF#wDQ zB~Nh7@0I-Hf$@ms*9ilSP=WgKk>aIX!H;Qs8GNz_nV4CpLvZA!@)U79WbuT zqSfoymhuU@vEoV`84rLOjB{{Ch7D1r27INcRzvZ&axGAOTe%j9V$-7lIs*(JcW+?* zZgp^?k0 zT-m_ItG!{`8owO7?S-d>l*#LofWW9vTLG$8^~=)_-pmUxtNhE`+5K-X#0s;~1VR#^ z>O<@1(xopmy;p^38at&CkZM6!13rtSqp3JNi_}U|N+xqj%sV9M&ADjOsL>x6DBd;c z2*M=ENLicfiJh}u*Rz0j)^_?Sg1#($^q|Y7)hQX{sf5ID(q~{M ztwDd&kra6Qk#x#yykY&`fL7!|kJMkv_x1N&Hs!s2gO|S_Ciy0 zRw%T0s7!02y>lxzVC%-_Y(2DY-LEpug|>E;*;~YxeJayfFmP|H$}|-WbPCy8=y0G* sW!efI2M{Z};Xo|~0}XlSL6w0!Oe1`7#W#Fu?PX7^jw4?&@UN@oANFj4VE_OC literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/sts.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/sts.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..963885b590293e08f71272805dd6c4dc5cd2c594 GIT binary patch literal 7957 zcmeHMYiv{39lw6&+HriHHvvk*vzV9j0Ak9kCNXg!FJcGEE$yxGF(DAg;asOB;55-z zZLo(*wGX7yre@Nl1<^^FHf^9yD<7r_Y1MWuGsU{%+dlDSLePEbr~Uuu`d&MUL$|h@ zrfCP|$2tG=xaa(z=luQ0iV7zOW&eXekNl{Ha07FIKhR>FxkViE1cDwYwO$P>HhAZ4PH@M1a8TtKS;nj2^Waflw`6e~%&SVdf7 zHK`D5h+C{Bf>=j9Vm+x8*N`f)fmDlYNsYLU)Qam#o%j-|7dMbKVk2n~o5)&mBUvYI zBJ0J?nd{=n_V+mA zjhpUTwDXF-$sOLEPfXDAnpD${CrEx`M!*qItO&@PCw7D~1P20g@<};@3t=t7I)p}q zCWMU$I}mmv>_XU$umxc|f)`;Af)Ak?p&h}GZ~);Igo6l&5Dp_8K{$$V3?Ybc93TdN zM!_>-dHycIZ7$AMHgq9lZ@GuoY&e%QGm1o0VU?Os`cYEnM%fz zqtbXZb~&DsE=I@5Xq>9NB#n%Xq$EjochTg;czALwc7~3m;;MUiN!x2yt#W+yf?7VF zq^XcLUBtj zx9<0)k|V@-eqv;luqx~$6{pcyY9u+fl8}hUV9O(N!WWLnar$;rlF4Nsuy@b+Bt>1i z=E8=48dMW`E5Ho*m~ULq3H6K}MUAU6naNvgZFZ{aaFp!HG z+=T{eJCoC&pmLi_ThrVnUA2Ism$dnAo(lbj4D?=*pwUl2lB=v?`GPf!{^7DI8>~^h z&-|y-rW9H#D=FMND`}gtvh6Oa1e7p$$E@jx*QPpwWzuN!{3M8*)h~{)y5k+xfXLOs zt7=&|5|#oT9jawCK6b}WTVM^4?@|!y$`P_ zgJEUp?BWo~3=t)MDLZsoNnBY>q%(>1Q;vJr)L{luj+r{m(5eZVZ}2&x`u%rrzB_y2 zC&M4h3;w%P_bz6IE`{%+d*NG4OcNQzME)97K;)$bjs}@G=yNc7$~?IE~9-O_ITM z0jaTMA|XM~#Ar%&AR$g^lFFzgMVv1igzo_3uazU)^NlVTBkuxRGf<;$nVao7`I(wF| zvXp*<1J}nItOPv{!gstDicvtVLQNeQ42FY!5viv?&?)s$JmC^XdWso2nv4?K4MXS& zfaNUXkxM-=MDr|q>m&}%Gp+sLVtaR{z5C&|M}s8W9?v-TDvpNPqw@!Ho|@aj2ZGYn zys#3!7zu@R^efgu8_J<|(h4#WnbY-b7R+jKUFj!JsDvPy+dAQZp#euvEqZhnZp>v8g z!|+*MVf=>{{Ug-3ko9y0eWZz^&%i zH+|IqVZXBLcy>JmqXt9xc^A9o6W@KGa^iL64JrG|Tgv$>ikw!2w8E!X+K{0Qzk_Y8 z?mh}UU*c;^hOH%9P|_P4BTx%U>4~i<^pzfAFB+k1K_?vHj+MR+|Is%P-b6qrP9=bq z;$aR7#Iw{vR zLRsOk!XJKttp4|2z?#%-1*xen8XsnFQGA6_Og>ReK9v?<#bAE0COy0)!lgi)&qxy} zJT$(4gYcR%LepH`$MwJ5ym0XsyC1B{3UHw~p^2|JL4jt*ObruotNGF*Pnw?#DxLS6 zb}bX5x{eLPTdE@vj!4~|UK6`q$v8THLxOwnaDXLk^m}i?F!$n9U{L87{+lt{4ti}ee$0< zL{UBu-YeID3HrUNa5yi#axvHHwd4<1NEW!*;R3I*N1ked^clQvFh8T^46LINfMwhe z+-URpmvB6#yLOg;DYU0yoZJYo^tgw9TYWF`VAK6GzgeFZjw}3e?bucrs29Vj{|YFp z9=qs4HU%*n(wFStYMB$A5zt>8K=R)#JVPKQhC!s%71WeMfv}Rolr<}9tFf}RkP^eY zO%VE7-qdT?@+hgK9Fq$^f?{5};2MfUDm{u_uOb8hmM0P%%(K9IA(mG$Os-+`8#wqB z0Pr001?~^rvlDaYvmWma8|J_&X9KfW<~Ghnek%P~S{TZ1J$NtjiF99jIFvmcR;nZa zQg-VrSs1+zn*&fWI`NbILPn-60mBuO_`p#iqULq#m>D5M-WcJFq6&u%x<#z z5xPvz>=sk>@ zrXdjmL!MrWN@dYMlU0WwC{JE zkvfBcPL+A$4!y+=(tjM1Z`zr#8lqFkICn2BRFueIX{fILKu>`fQU%;LBSyM=VcW|I zA$F%%D{)w)fAG{$Pzv@1j`akYcTzpZ^n6}x1tmnk37etcLpX=qDxGHuQA}^BB~oP} z+J`+WGp?bfqfhtuL;_twwH~*mr)DF;-hm*;8TmqWctj`nbtk#wms%HJmwmo+Bs+Wj6GNGn&FF} z!ad{6xhiHH1$fGHf@h{Y=N4wl^;w?EnTp(+28FB5ty#Mq-8fpPoN?t`Zd?eVykdrj z$yGHo?p)m(g{#g%RnVZa8i+M@3RjtPx@O99)wK%e$)ivW%j74oU9WI;xrTL!+8P?x k)+=09PD71?BkOY&0-U92d!Jc*O(wtM-1v76D~xUb08`EJ3;+NC literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/stservo_def.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/stservo_def.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..31b89e349eb72c3769eb799fbde138a02cd2d77b GIT binary patch literal 766 zcmZXRKX21O7{=}Vj}zOmlZsD(P#26kbV5juV=7CH9oZM!-eRRWqnb8xWjjdSED)c8 z&%n1}XFzP02Nou8l{z3oDZFD!Y$IqZO8>8;122FF6m;Ch*%;LmWhme zq=yxvVD(Df?U#4Zw!EdFgGxHCggbC$m7+#br+7>8hT=D@@{Zyc#Vy58K)9y(LGc}6 z@AixC&~@~Yp?k=;Mnz++AJcEsL!NKiqoQrukW4K5U@Kiy9~A)fpq z*4(cS zh-dROjF$fw7Qy^%whWVn=Ak5rUq^mI&b1`nXcD}9e0jBQJzYd(brC#^AHl8Yk5A44 z*Hxb9uNz!%kahF6m^E^tlQp3rN?AMaiU3eTuBe%mOLEr9)tyYvl|d%aQwp3?`Vh%f XG4#|-%+&#Gy2s9gFPs`bgn54eJwp8$ literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/__pycache__/__init__.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f2cf6a54fb013678eda7999e80acffcea8e50ed4 GIT binary patch literal 215 zcmX@j%ge<81ZD@$XM*U*AOanHW&w&!XQ*V*Wb|9fP{ah}eFmxdRinq|(fs6fiSB zGp{7IC^5MtGd~YgAU8ERBQYPO4oIE6^E?KwJ!B Nd}L;1WGrF^vH<#uJy`$% literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/__pycache__/sekvenca_2026.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/__pycache__/sekvenca_2026.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e9d566be9fe78b1a97331a4e0675dd312877a306 GIT binary patch literal 2419 zcma)7U2GIp6ux)&?(FPe3#Fw$C=3Nj*Fd)m1x-XvX-h0hK_NV>n$2`)fV(?C<<4wP zYpV-FSOSDKCerxCgAbY#pN)^kH(y#?(=yaZVqf}J3%(E&Ja=|ycdHP&$((!U+;h*p z=bZ1H`*SQ7MldFYS$al5=nwwm4PS+MXaI8?DM-N@(s2r7&igcCvLGyCq%Yf8=Ig1w+54MBGZhcATRd59_U`ZLKtNHP3{&(Y zMvVIlK@8edSIxW~4?AIvT6TKWWVYpq8B;HFF-v7s*3w2^A5obTIyLy-P~SW0p#euI zuQm7NWK2bk`yEW35Vx47##s{fJ9 zV&lI8UCo>aq*!{wQgdT)CppcHbdQcZf|+BgK^bHiHNa=R52S#WTXrnAbj`JN&9}U~ zj(mZpVrVJSycp@6i*$ZX=Oc-G(amcCkUtVpWb>_k*Y-_^7Tdb!+Pc2k^Zm|+@Zm!L z5(!=%x;S(rQ`&j`>@EG8e%<_)Y<~#m(bF;c6|B@_vX)&1=U##Gh}acm08)HjRxe?j zquL#c-;-iw2R&)c4j{Q5p${PUQ<$SmAr;IM3sjRRUi704&`hP3KJ)yb65gQoEMFr3^5tnaeclr{+{>;u)$AtEyV_juJv7Yd z@I*8Ltu0~O<9+?dldC+LE+A>c-%p;bLO}u2l0C^D##dIq1OkmUbXsQTRQsf9QE0b_ zu0!d@LyQ+N{=6G>FSA#CrW(_Wa|!3z7mpP$TsiL6kRztk+^KXr-rz*aey254 zR?1=I5h%xUFXuQ=o!0WB)NPxlW_8!SXaQHkcs(%w^1AVN1MG&3A7#k8nofU<+_T-7 zJ{Aw_rjpmxH`pQA!{1t~56GGyWBgYb;XS{h-S<)CK8pR0_RgZc52D)(p>;vPJJy@R fxMRIz8*W-Vj7YRN@r%Fx9ucn`UGpPSZV~?gLjefo literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/__pycache__/zidovi_load.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/__pycache__/zidovi_load.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..aa5f819e26045cb6a6be7691b4d2775dd07f1fde GIT binary patch literal 10590 zcmeHNTWk~A86MvfJ09EPYi>Ap0)#b}I9FKMEFlmsS;%FRu)$l~&rUctL%lZ|Os;mHJQ{2W2!uYNR4nc?%0wrM$HNKk>wu*a=YB zRV60MIsf_p|NQ4a|GD^_Z|1KiQw;^r@jw49Joyeq{R0ciqsVl{Ww8#N|%H?%(8stQUztz;aH6aHK<#zr_IX%c)w zqEC$s4F$$8o;r1Ud|av;pBN7e4uo>8$(bfsczO_&)al{=q0^EX{TUkhU~u?sp#RjVkP-%T2|I|sJ#B;JOG;1+Q~;=rM^Hft zJ1swg21>brP}B{|ryBNYhyi2-Lr#p4Kyo{;7C^ha}4RXE;7c`~&gOY?O=f3I8aGPjew4PS29~ zr8wW&-g(&1$HT1u(rh@w%48rM<2e$PBe9hcb3>Ok%?x$Sh*h!Ibu)zVRtYsgMrS+@ptF4(l%< zJ`h$I;yIAB>N$u~QY)xf&DFOcW>tbZsT0&jDp?iV$RSNu@%f-r)0-hzj8oLzoIF|0 zZ?-B_Z*uiQ^>lS!TcPge2txBXYVyNsm0Bt)<}Kjs^85mBRRI^rTYeO~Dhk>)DcpQ6 zP}AmoNud-=Mc!=op~M@#qOGEy-y7C2Kz+ zzj_45r!XQl0@`sZ@_a7TD0VKZ=c@9fqoQt|5R9x5EhSX9&J=jQVXHQ8L!ks?MO&A( z6trKjXy$X4R_c5N3o1z`O>$q#>zaym%}ERD7I0NM$?Xv}-BQ2Z{1Vy())tAgzhv^|O>pR=@}gL&o6a}?@sZmX?0 zrtSj2Z99!<@pvkuNlI;OJCY?f%J;`4RK0@!*k3_^(8i1Ohe=umd(r{5o_}tZuCvm3 zZoA4#;+b1v|KpgH^S$g^*|J9z`u=0YUq8KDnfv*?U{8SckaXpCgmRksUZQ5Ltm$GM zbMPC(I`LO4StryMmFo*ukgM!|QH%S9PTt&p>Pt1`Jp=cOIze-F0QOCt zP`^Qu!GhZLJKArkV88t8qgj|m%FY45W979>#agjH@Ozbr3yff$gJX~q4YpDC-ddnx z7aUtPh)%G}LUV%o5Jz2w95Tjj!4bB^Q>QLdSqvA-o1!rYF z!6s<0egwaS#{?&wVw&cl!*aQ^pe2ve>xz+kx%jGYYi{A0VxJP_e4amZz(LeIyhYcHNz~Tt@6uucB|O@Ec97(?1&=IJURFgZ`_^rzW(k3VccBBVn{~@N;JpCnX>=r~SILo0 zy<_LXSEjsp3iq-c4~Kh>J?4GRHnyp|Q@gA8aV^wBAJ}Ow79O4Dy@Lat7)+MC;w=?) zD}q(L;;h$C1j!YyAS_C)Ask^zkFySMijr=Iz|}<3eli>8C1Zl) zPr`j@3}%&ld6Eo2nc}#}P@Lrm9_A$zv@mgf?D`m&n2qof1I4lHqruQsjz3LEoJjN- z7i3`ujL*Ofmukj2ekgby!%yJwNr62Q<~b0CGr)(L7f`0Y+>EnOlE7}u>`E27 zrLq;eCCdcN>RCP(gl@W+K6B;Q-uZ=UhdX)=fSh;1Xe&PoGjN4)?q^sb|5+cLlB8~<1StgAC+gmV3^JLj`?Z>3CY zRTN#jZ1iM|o+V@RBifp_wLPT$kC8aAL?4tn9ZPiQW16{jCa2w!F}BQifAj8F?=BfT zWR32JbdOvIRkzfs_k2$o4Yt%d(RX;+cOv6Eu~?V&y_ca|QzLgi6fI3(AADeGd&D?J zM~hhNnRhQY_hy=V?+3EYLuuFW6OGC_qWYIcW3{d6DO=;6;BT9Xl>0KxeT#-{^LuI6 zc~riTQ?^2*YfY4?VVQ1)#^yiC(!M7e$}piy>%HlgBlkVu`M~0~@@IO=U{0G`mh`(H z=}jViP_*nz`;Mp0ebAEWgzA|_W2ljpD#!p~XzWT^#JctrfDDZ(%SuCMVNujOm-XI^ z-uty{!M3D7xMH@gC}9#A>+Pd=*yW~FVmgZZC~siT<#sq^p2&+C$ha4GxX6k z?Y?vKo}sc}*P(mv%&xx0u1w>Z)CIA)C@+!LC3+taBJ;LrKaf6ja`8gi_CCxi>ln@| zlNn}}$yQOiRzrD?q(;Q1!>JLN#8!9O)BS+y7VRFG*$?bT9^-Nv6>I(Jjx*oUY1c(u z86Ti!s|_q$T@|Hh4F+pphS|5ETIg5^E-~#Qk^a1D5bwtslUD=c{TT6oJs_S<2J!5h zAYNg)5X5uj0h|I@$GIcO=Bfa)A@IL5i1+d!Vfoefav;-oAYT2BAfBx88bQ2<=LGQ@ zUjW40RTji^=jQH~Ovc+9qn1 z(*qmZ>urNj?byaPb$4o)?8VNMpj%xEh*c`+Q3RwF1A#Crfw$6B4j3Ybk#Pi*w=p>i zNgna(%9A|=zf~abVsadlK1@zPB2|KDy2?W|T@@?zl&{cJis^I}Lo{8b5DhSjL7<-E z3Sh7p)9Kj?;B;+e^%S8vT}7py=fZUKm$=9Vq*MGIg2Mg_gfr2z9ip>|;(R^`*PMD! z4&Ql$>1>PHya1*Hx9B(M%*G5{W~Q{F)LiwqFg4R!3Ir z5FNEyZ7u9gwtBI)?x{hqJM`2QRqMRZey*E>Y{L@P9rjg8#_@>+N3O*K3HIv7dnkMb zCBIIR-wAEB7C|1ORI&%_`reHW9Vap1K>(-(p19UDDwXQ*R9}|r`?vYBPUZcc!c?yK EUxp`c(*OVf literal 0 HcmV?d00001 diff --git a/toid_interaction/toid_interaction/mechanism/__pycache__/zupcanik.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/__pycache__/zupcanik.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1589cdac5e53e5442004c44d9edc275dc52dbc9a GIT binary patch literal 4610 zcmdT|T}&I<6`t|fFt*1YV;hJe29rP<^JhYSNCOE0>?DMr5Fn{acWXJG2^jphGh>=q z<94OZN>;QFv@db1KCseCOO-yLKG8Q`Dn;r;J=l>)qew++Uw9)+q*nW~J$GQRLpDGA zQnmNt-gECc_nvdUd+xnw=C2lu34;>*%iki)P7M18>S#rOKI(?8UE4LdmWyo6$&N^ruBP@IXfoI>yduTUemCMJS2 zcSc46Gc(}>4QfLiboMR*@dRVBIh?`fbayb8VD$_>0Hdq{D4j~7F#=IcHKTuo&s8yo zN7!8T!zwSSSm#p7a3~&G8VU=ML|h?A&)QMOSsQn`1&_fQYMI@ki;5>#s#Ju>Vm*7a?aYj@j ze7$-_&$H3{iiu&DBVjfkim?iLFO*`qkiaTd045kqEJxz=!B9BNBTGC`zFqclskm<; z!3$8uSYMJALY&W^2&ZCfT;P3ETw;+80})7ZiF*m5x2N}lPe?=<-@Q~M%BW;85*Ju5 zq$2U=K#UD908)I+mp;I(JNZx{gD@*11cT}N*P-mz7|1{V7*wBP@-a_V|Aj?vXoZHB zn>w@l=Vap^@UYh~MN?Ev|6PSJ70L)=i--83S^MSb0Y9S`4BD8eKO8lP`o(H(yxcxL zAbCna=&OanwOKgq56m-boN_B-i&VL%QQExMR?}&t>S~5XT9Yof;-SWmz_UzMrdq5L zY#M*555l6U!ahPYEIKr9xfQE4O=&<(qgAaC^WVElZF#mviPhT7H&!t$)@xGbw!(IZ z!wi`*ey{zFW&7V}y`~?>;x*-2i~-IODH`t+uiGVRzBQC(LBl&eD{Bx5rYa5b!c?bW zmoo_25B9OFk0IY;8<{bQWX3F-L~|Mrc7|f~84KuemGRRk5oe6rrHErj45S)0UbzK5 zaJMWTFOO){(}Vl)tr6s|u=fW>&>_Z0*LWw&NA?Nf@B`2V*=EhRa$6B?;BUsN#+qTV zr7WRQM-NBv1f{7`Kbhdsf|&| znIr>b1cSFGvH%AmeP$Hi@kl5N3V0wA@k^ffOx~FS6_M}YG|vC-x2XJiaOXRu_3B89 z;~?$$@TENCi4d~KGZA_K&CVEFrvFA{e!&w3j)!3d$X;0Hy*6nJMK(}?HTWaE5Q>KZ z9xk-Z0+g3LpoC%PJ?V?HA#R>6+vwrgPAFa<*y6+0+D2G5%zMWup@bhUFmsF~tI^XCF*Im}dD@R8VZtoqjMC3NNujfa4OJ zLQS(F2KH+v$+C=MnqdWi2B8E?aS?%q@vxdPDkPFrIn|m75xU1K#&9&jv&bxO4Tn+| zr3&S-qkhF!kxDN*CQ_Lv9v_#yJR87*lG)h0iiC8QM!G!EKq5)Pl*onv_ zjKjtUwHQ)rDo1(M7xOH{MoNf>kTxSErhHT-%@Co?A{&oPr2xT15};!t0bXPkTje4s z9ux|V#6k~(YL7-##RR?vyhUm8YlTB;l|pLS>l+EH#;|}GZ-9;c6nj;L)t^{x{o41X zuTa;UHACNZJU3Q2b~t-@@LCHka9pDfaCFQ}Tly`xBV%9QE|uD%Up!8I(|hl|w6zGh29i8>`wr&gB>RQF2)GmhhV@_4?< zx8A(z1xl6Wo$=1GhpHnE%o09OF;bqsK1z}j^`3<4}p6zfS3BlrM~fJcME;9 z5_K*+p7X8!SZ?fC%ServHyu)gKRYHjwXe-cO+DFL0J3bG-gY(s4zjaNb~ek-le^|B zhy5i{U1NVm7-2{1ApUaGTXZv6E<10__3nK0+5Guy&&>I{S%9?m4uZ7C4qMmVU+VHp zUH;9x&u-_ZZWp?y!Ic$y)g?Pld==et^uBPqa;>ZGQp^Q!P z|K`dedr)c_+%y(iZb}U|i_S3_G&gi-$6nC(mHt(i!LJsaMk#31}KPQ_xJ6s2oUR~tsFpcf>LI-{uz*jH&Fb@V5QxHDK znN$?fR518SDil575a=PGgTYWdo)ALlLFGXP{%mj@x}Lc6Q2hZ@msf8l4t=Al6?$EG zH&mZvvbE;33EAfO?3V1TFX-#{^n{^n&y>Oqp8c6<(xlg*m}h2rmRn8)d1h(M6-FRHR{$$@>duW*Af2Jb({qpzP`0>|;cV|^0V_b>ggaNP3^hH7= opterecenje_threshold or opterecenje4 > opterecenje_threshold: - print("High load detected: [ID:002] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID2, sts_present_speed_ID2, opterecenje2)) - print("High load detected: [ID:004] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID4, sts_present_speed_ID4, opterecenje4)) - packetHandler.WriteSpec(2, 0, 0) # Stop motor ID 2 - packetHandler.WriteSpec(4, 0, 0) # Stop motor ID 4 - break - if abs(TrenutnaPos_ID2 - ProslaPos_ID2) > 3000 or abs(TrenutnaPos_ID4 - ProslaPos_ID4) > 3000: - PredjeniPut_ID2 = PredjeniPut_ID2 - PredjeniPut_ID4 = PredjeniPut_ID4 + def plavi_zid(self, smer, brzina=1500, opterecenje_threshold=75, TargetPos=300): + # TargetPos = 300 # Target position in degrees + TargetPos = TargetPos * 4096 / 360 # Convert to motor ticks + if smer == 1: + Speed_ID2 = brzina # Speed for motor ID 2 + Speed_ID4 = -brzina # Speed for motor ID 4 (opposite sign) else: + Speed_ID2 = -brzina # Speed for motor ID 2 + Speed_ID4 = brzina # Speed for motor ID 4 (opposite sign) + + # Initialize PortHandler instance + portHandler = PortHandler(self.DEVICENAME) + + # Initialize PacketHandler instance + packetHandler = sts(portHandler) + + # Open port + if portHandler.openPort(): + print("Succeeded to open the port") + else: + print("Failed to open the port") + print("Press any key to terminate...") + + quit() + + # Set port baudrate + if portHandler.setBaudRate(self.BAUDRATE): + print("Succeeded to change the baudrate") + else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + + quit() + + # Set motors to Wheel Mode + for motor_id in [2, 4]: + sts_comm_result, sts_error = packetHandler.WheelMode(motor_id) + if sts_comm_result != COMM_SUCCESS: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getTxRxResult(sts_comm_result)) + ) + elif sts_error != 0: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getRxPacketError(sts_error)) + ) + + # Read initial positions for both motors + sts_present_position_ID2, sts_present_speed_ID2, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(2) + ) + sts_present_position_ID4, sts_present_speed_ID4, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(4) + ) + + print("Initial positions:") + print( + "[ID:002] PresPos:%d PresSpd:%d" + % (sts_present_position_ID2, sts_present_speed_ID2) + ) + print( + "[ID:004] PresPos:%d PresSpd:%d" + % (sts_present_position_ID4, sts_present_speed_ID4) + ) + + # Initialize variables for tracking movement + TrenutnaPos_ID2 = sts_present_position_ID2 + ProslaPos_ID2 = sts_present_position_ID2 + PredjeniPut_ID2 = 0 + + TrenutnaPos_ID4 = sts_present_position_ID4 + ProslaPos_ID4 = sts_present_position_ID4 + PredjeniPut_ID4 = 0 + packetHandler.SetMaxLoad(STS_ID, 80) + + while True: + # Write goal position, speed, and acceleration for both motors + sts_comm_result, sts_error = packetHandler.WriteSpec( + 2, Speed_ID2, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:002] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:002] %s" % packetHandler.getRxPacketError(sts_error)) + + sts_comm_result, sts_error = packetHandler.WriteSpec( + 4, Speed_ID4, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:004] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:004] %s" % packetHandler.getRxPacketError(sts_error)) + + # Update positions for both motors + ProslaPos_ID2 = TrenutnaPos_ID2 + ProslaPos_ID4 = TrenutnaPos_ID4 + + ( + sts_present_position_ID2, + sts_present_speed_ID2, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(2) + if sts_comm_result != COMM_SUCCESS: + print("[ID:002] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:002] %s" % packetHandler.getRxPacketError(sts_error)) + + ( + sts_present_position_ID4, + sts_present_speed_ID4, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(4) + if sts_comm_result != COMM_SUCCESS: + print("[ID:004] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:004] %s" % packetHandler.getRxPacketError(sts_error)) + sts_present_load2, sts_comm_result, sts_error = packetHandler.ReadLoad(2) + sts_present_load4, sts_comm_result, sts_error = packetHandler.ReadLoad(4) + opterecenje2 = (sts_present_load2 & ((1 << 10) - 1)) * 0.1 + opterecenje4 = (sts_present_load4 & ((1 << 10) - 1)) * 0.1 + print("Current positions:") + print( + "[ID:002] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID2, sts_present_speed_ID2, opterecenje2) + ) + print( + "[ID:004] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID4, sts_present_speed_ID4, opterecenje4) + ) + + TrenutnaPos_ID2 = sts_present_position_ID2 + TrenutnaPos_ID4 = sts_present_position_ID4 + + # Check if the positions have changed + if ( + opterecenje2 > opterecenje_threshold + or opterecenje4 > opterecenje_threshold + ): + print( + "High load detected: [ID:002] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID2, sts_present_speed_ID2, opterecenje2) + ) + print( + "High load detected: [ID:004] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID4, sts_present_speed_ID4, opterecenje4) + ) + packetHandler.WriteSpec(2, 0, 0) # Stop motor ID 2 + packetHandler.WriteSpec(4, 0, 0) # Stop motor ID 4 + break + if ( + abs(TrenutnaPos_ID2 - ProslaPos_ID2) > 3000 + or abs(TrenutnaPos_ID4 - ProslaPos_ID4) > 3000 + ): + PredjeniPut_ID2 = PredjeniPut_ID2 + PredjeniPut_ID4 = PredjeniPut_ID4 + else: PredjeniPut_ID2 += abs(TrenutnaPos_ID2 - ProslaPos_ID2) PredjeniPut_ID4 += abs(TrenutnaPos_ID4 - ProslaPos_ID4) - - print("PredjeniPut ID2:%d deg " % (PredjeniPut_ID2 * 360 / 4096)) - print("PredjeniPut ID4:%d deg " % (PredjeniPut_ID4 * 360 / 4096)) + print("PredjeniPut ID2:%d deg " % (PredjeniPut_ID2 * 360 / 4096)) + print("PredjeniPut ID4:%d deg " % (PredjeniPut_ID4 * 360 / 4096)) - + # Stop if both motors reach their target positions + if PredjeniPut_ID2 >= TargetPos and PredjeniPut_ID4 >= TargetPos + 10: + print("Target positions reached.") + packetHandler.WriteSpec(2, 0, 0) # Stop motor ID 2 + packetHandler.WriteSpec(4, 0, 0) # Stop motor ID 4 + break - # Stop if both motors reach their target positions - if PredjeniPut_ID2 >= TargetPos and PredjeniPut_ID4 >= TargetPos+10: - print("Target positions reached.") - packetHandler.WriteSpec(2, 0, 0) # Stop motor ID 2 - packetHandler.WriteSpec(4, 0, 0) # Stop motor ID 4 - break + # Close port + portHandler.closePort() - # Close port - portHandler.closePort() + def beli_zid(self, smer, brzina=1500, opterecenje_threshold=75, TargetPos=600): + # TargetPos = 600 # Target position in degrees + TargetPos = TargetPos * 4096 / 360 # Convert to motor ticks + if smer == 1: + Speed_ID3 = -brzina # Speed for motor ID 3 + Speed_ID5 = brzina # Speed for motor ID 5 (opposite sign) + else: + Speed_ID3 = brzina # Speed for motor ID 3 + Speed_ID5 = -brzina # Speed for motor ID 5 (opposite sign) - - -def beli_zid(smer, brzina=1500, opterecenje_threshold=75, TargetPos=600): - # TargetPos = 600 # Target position in degrees - TargetPos = TargetPos * 4096 / 360 # Convert to motor ticks - if smer == 1: - Speed_ID3 = -brzina # Speed for motor ID 3 - Speed_ID5 = brzina # Speed for motor ID 5 (opposite sign) - - else: - Speed_ID3 = brzina # Speed for motor ID 3 - Speed_ID5 = -brzina # Speed for motor ID 5 (opposite sign) - + # Initialize PortHandler instance + portHandler = PortHandler(self.DEVICENAME) - # Initialize PortHandler instance - portHandler = PortHandler(DEVICENAME) + # Initialize PacketHandler instance + packetHandler = sts(portHandler) - # Initialize PacketHandler instance - packetHandler = sts(portHandler) + # Open port + if portHandler.openPort(): + print("Succeeded to open the port") + else: + print("Failed to open the port") + print("Press any key to terminate...") - # Open port - if portHandler.openPort(): - print("Succeeded to open the port") - else: - print("Failed to open the port") - print("Press any key to terminate...") - - quit() + quit() - # Set port baudrate - if portHandler.setBaudRate(BAUDRATE): - print("Succeeded to change the baudrate") - else: - print("Failed to change the baudrate") - print("Press any key to terminate...") - - quit() + # Set port baudrate + if portHandler.setBaudRate(self.BAUDRATE): + print("Succeeded to change the baudrate") + else: + print("Failed to change the baudrate") + print("Press any key to terminate...") - # Set motors to Wheel Mode - for motor_id in [3,5]: - sts_comm_result, sts_error = packetHandler.WheelMode(motor_id) - if sts_comm_result != COMM_SUCCESS: - print("[ID:%01d] %s" % (motor_id, packetHandler.getTxRxResult(sts_comm_result))) - elif sts_error != 0: - print("[ID:%01d] %s" % (motor_id, packetHandler.getRxPacketError(sts_error))) + quit() - # Read initial positions for both motors - sts_present_position_ID3, sts_present_speed_ID3, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(3) - sts_present_position_ID5, sts_present_speed_ID5, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(5) + # Set motors to Wheel Mode + for motor_id in [3, 5]: + sts_comm_result, sts_error = packetHandler.WheelMode(motor_id) + if sts_comm_result != COMM_SUCCESS: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getTxRxResult(sts_comm_result)) + ) + elif sts_error != 0: + print( + "[ID:%01d] %s" + % (motor_id, packetHandler.getRxPacketError(sts_error)) + ) - print("Initial positions:") - # print("[ID:003] PresPos:%d PresSpd:%d" % (sts_present_position_ID3, sts_present_speed_ID3)) - # print("[ID:005] PresPos:%d PresSpd:%d" % (sts_present_position_ID5, sts_present_speed_ID5)) + # Read initial positions for both motors + sts_present_position_ID3, sts_present_speed_ID3, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(3) + ) + sts_present_position_ID5, sts_present_speed_ID5, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(5) + ) - # Initialize variables for tracking movement - TrenutnaPos_ID3 = sts_present_position_ID3 - ProslaPos_ID3 = sts_present_position_ID3 - PredjeniPut_ID3 = 0 + print("Initial positions:") + # print("[ID:003] PresPos:%d PresSpd:%d" % (sts_present_position_ID3, sts_present_speed_ID3)) + # print("[ID:005] PresPos:%d PresSpd:%d" % (sts_present_position_ID5, sts_present_speed_ID5)) - TrenutnaPos_ID5 = sts_present_position_ID5 - ProslaPos_ID5 = sts_present_position_ID5 - PredjeniPut_ID5 = 0 - packetHandler.SetMaxLoad(STS_ID, 80) - - - while True: - # Write goal position, speed, and acceleration for both motors - sts_comm_result, sts_error = packetHandler.WriteSpec(3, Speed_ID3, STS_MOVING_ACC) - if sts_comm_result != COMM_SUCCESS: - print("[ID:003] %s" % packetHandler.getTxRxResult(sts_comm_result)) - if sts_error != 0: - print("[ID:003] %s" % packetHandler.getRxPacketError(sts_error)) - - sts_comm_result, sts_error = packetHandler.WriteSpec(5, Speed_ID5, STS_MOVING_ACC) - if sts_comm_result != COMM_SUCCESS: - print("[ID:005] %s" % packetHandler.getTxRxResult(sts_comm_result)) - if sts_error != 0: - print("[ID:005] %s" % packetHandler.getRxPacketError(sts_error)) - - # Update positions for both motors - ProslaPos_ID3 = TrenutnaPos_ID3 - ProslaPos_ID5 = TrenutnaPos_ID5 - - sts_present_position_ID3, sts_present_speed_ID3, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(3) - if sts_comm_result != COMM_SUCCESS: - print("[ID:003] %s" % packetHandler.getTxRxResult(sts_comm_result)) - if sts_error != 0: - print("[ID:003] %s" % packetHandler.getRxPacketError(sts_error)) - - sts_present_position_ID5, sts_present_speed_ID5, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(5) - if sts_comm_result != COMM_SUCCESS: - print("[ID:005] %s" % packetHandler.getTxRxResult(sts_comm_result)) - if sts_error != 0: - print("[ID:005] %s" % packetHandler.getRxPacketError(sts_error)) - sts_present_load3, sts_comm_result,sts_error = packetHandler.ReadLoad(3) - sts_present_load5, sts_comm_result,sts_error = packetHandler.ReadLoad(5) - opterecenje3 = (sts_present_load3 & ((1<<10) - 1)) * 0.1 - opterecenje5 = (sts_present_load5 & ((1<<10) - 1)) * 0.1 - print("Current positions:") - print("[ID:003] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID3, sts_present_speed_ID3 , opterecenje3)) - print("[ID:005] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5)) - + # Initialize variables for tracking movement TrenutnaPos_ID3 = sts_present_position_ID3 - TrenutnaPos_ID5 = sts_present_position_ID5 - + ProslaPos_ID3 = sts_present_position_ID3 + PredjeniPut_ID3 = 0 - # Check if the positions have changed - if opterecenje3 > opterecenje_threshold or opterecenje5 > opterecenje_threshold: - print("High load detected: [ID:003] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID3, sts_present_speed_ID3, opterecenje3)) - print("High load detected: [ID:005] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5)) - packetHandler.WriteSpec(3, 0, 0) # Stop motor ID 3 - packetHandler.WriteSpec(5, 0, 0) # Stop motor ID 5 - break - if abs(TrenutnaPos_ID3 - ProslaPos_ID3) > 3000 or abs(TrenutnaPos_ID5 - ProslaPos_ID5) > 3000: + TrenutnaPos_ID5 = sts_present_position_ID5 + ProslaPos_ID5 = sts_present_position_ID5 + PredjeniPut_ID5 = 0 + packetHandler.SetMaxLoad(STS_ID, 80) + + while True: + # Write goal position, speed, and acceleration for both motors + sts_comm_result, sts_error = packetHandler.WriteSpec( + 3, Speed_ID3, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:003] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:003] %s" % packetHandler.getRxPacketError(sts_error)) + + sts_comm_result, sts_error = packetHandler.WriteSpec( + 5, Speed_ID5, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("[ID:005] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:005] %s" % packetHandler.getRxPacketError(sts_error)) + + # Update positions for both motors + ProslaPos_ID3 = TrenutnaPos_ID3 + ProslaPos_ID5 = TrenutnaPos_ID5 + + ( + sts_present_position_ID3, + sts_present_speed_ID3, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(3) + if sts_comm_result != COMM_SUCCESS: + print("[ID:003] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:003] %s" % packetHandler.getRxPacketError(sts_error)) + + ( + sts_present_position_ID5, + sts_present_speed_ID5, + sts_comm_result, + sts_error, + ) = packetHandler.ReadPosSpeed(5) + if sts_comm_result != COMM_SUCCESS: + print("[ID:005] %s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("[ID:005] %s" % packetHandler.getRxPacketError(sts_error)) + sts_present_load3, sts_comm_result, sts_error = packetHandler.ReadLoad(3) + sts_present_load5, sts_comm_result, sts_error = packetHandler.ReadLoad(5) + opterecenje3 = (sts_present_load3 & ((1 << 10) - 1)) * 0.1 + opterecenje5 = (sts_present_load5 & ((1 << 10) - 1)) * 0.1 + print("Current positions:") + print( + "[ID:003] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID3, sts_present_speed_ID3, opterecenje3) + ) + print( + "[ID:005] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5) + ) + + TrenutnaPos_ID3 = sts_present_position_ID3 + TrenutnaPos_ID5 = sts_present_position_ID5 + + # Check if the positions have changed + if ( + opterecenje3 > opterecenje_threshold + or opterecenje5 > opterecenje_threshold + ): + print( + "High load detected: [ID:003] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID3, sts_present_speed_ID3, opterecenje3) + ) + print( + "High load detected: [ID:005] PresPos:%d PresSpd:%d Load:%d" + % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5) + ) + packetHandler.WriteSpec(3, 0, 0) # Stop motor ID 3 + packetHandler.WriteSpec(5, 0, 0) # Stop motor ID 5 + break + if ( + abs(TrenutnaPos_ID3 - ProslaPos_ID3) > 3000 + or abs(TrenutnaPos_ID5 - ProslaPos_ID5) > 3000 + ): PredjeniPut_ID3 = PredjeniPut_ID3 PredjeniPut_ID5 = PredjeniPut_ID5 - else: + else: PredjeniPut_ID3 += abs(TrenutnaPos_ID3 - ProslaPos_ID3) PredjeniPut_ID5 += abs(TrenutnaPos_ID5 - ProslaPos_ID5) + print("PredjeniPut ID3:%d deg " % (PredjeniPut_ID3 * 360 / 4096)) + print("PredjeniPut ID5:%d deg " % (PredjeniPut_ID5 * 360 / 4096)) - print("PredjeniPut ID3:%d deg " % (PredjeniPut_ID3 * 360 / 4096)) - print("PredjeniPut ID5:%d deg " % (PredjeniPut_ID5 * 360 / 4096)) - - - - # Stop if both motors reach their target positions - if PredjeniPut_ID3 >= TargetPos and PredjeniPut_ID5 >= TargetPos+10 : - print("Target positions reached.") - packetHandler.WriteSpec(3, 0, 0) # Stop motor ID 3 - packetHandler.WriteSpec(5, 0, 0) # Stop motor ID 5 - break - - # Close port - portHandler.closePort() - + # Stop if both motors reach their target positions + if PredjeniPut_ID3 >= TargetPos and PredjeniPut_ID5 >= TargetPos + 10: + print("Target positions reached.") + packetHandler.WriteSpec(3, 0, 0) # Stop motor ID 3 + packetHandler.WriteSpec(5, 0, 0) # Stop motor ID 5 + break + # Close port + portHandler.closePort() diff --git a/toid_interaction/toid_interaction/mechanism/zuocanik.py b/toid_interaction/toid_interaction/mechanism/zuocanik.py deleted file mode 100644 index e358549..0000000 --- a/toid_interaction/toid_interaction/mechanism/zuocanik.py +++ /dev/null @@ -1,105 +0,0 @@ - -import sys -import os - - -from .STservo_sdk import * # Uses STServo SDK library - -# Default setting - -BAUDRATE = 1000000 # STServo default baudrate : 1000000 -DEVICENAME = 'COM4' # Check which port is being used on your controller -STS_MOVING_ACC = 50 # STServo moving acc - - - - -# Function to move the STServo to the target position -def zupcanik(STS_ID ,STS_MOVING_SPEED , TargetPos): - - # Initialize PortHandler instance - portHandler = PortHandler(DEVICENAME) - - # Initialize PacketHandler instance - packetHandler = sts(portHandler) - - # Open port - if not portHandler.openPort(): - print("Failed to open the port") - return - - # Set port baudrate - if not portHandler.setBaudRate(BAUDRATE): - print("Failed to change the baudrate") - return - - sts_comm_result, sts_error = packetHandler.WheelMode(STS_ID) - if sts_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(sts_comm_result)) - return - elif sts_error != 0: - print("%s" % packetHandler.getRxPacketError(sts_error)) - return - # Read STServo present position - sts_present_position, sts_present_speed, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(STS_ID) - if sts_comm_result != COMM_SUCCESS: - print(packetHandler.getTxRxResult(sts_comm_result)) - else: - print("[ID:%03d] PresPos:%d PresSpd:%d" % (STS_ID, sts_present_position, sts_present_speed)) - if sts_error != 0: - print(packetHandler.getRxPacketError(sts_error)) - - - TargetPos = TargetPos * 4096 / 360 # Convert to motor ticks - sts_present_position, sts_present_speed, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(STS_ID) - - print("Initial position: [ID:%03d] PresPos:%d PresSpd:%d" % (STS_ID, sts_present_position, sts_present_speed)) - - TrenutnaPos = sts_present_position - ProslaPos = sts_present_position - PredjeniPut = TrenutnaPos - ProslaPos - opterecenje = 0 - packetHandler.SetMaxLoad(STS_ID, 80) - - while True: - # Write STServo goal position/moving speed/moving acc - sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, STS_MOVING_SPEED, STS_MOVING_ACC) - if sts_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(sts_comm_result)) - if sts_error != 0: - print("%s" % packetHandler.getRxPacketError(sts_error)) - - ProslaPos = sts_present_position - sts_present_position, sts_present_speed, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(STS_ID) - sts_present_load, sts_comm_result,sts_error = packetHandler.ReadLoad(STS_ID) - sts_max_load, sts_comm_result,sts_error = packetHandler.MaxLoad(STS_ID) - opterecenje = (sts_present_load & ((1<<10) - 1)) * 0.1 - max_opterecenje = sts_max_load & 255 - print("Current position: [ID:%03d] PresPos:%d PresSpd:%d Load:%d Max:%d" % (STS_ID, sts_present_position, sts_present_speed, opterecenje, max_opterecenje)) - - if opterecenje > 75: - print("High load detected: [ID:%03d] PresPos:%d PresSpd:%d Load:%d" % (STS_ID, sts_present_position, sts_present_speed, opterecenje)) - sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, 0, 0) - if sts_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(sts_comm_result)) - if sts_error != 0: - print("%s" % packetHandler.getRxPacketError(sts_error)) - break - TrenutnaPos = sts_present_position - if abs(TrenutnaPos - ProslaPos) > 3000: - PredjeniPut = PredjeniPut - else: - PredjeniPut = PredjeniPut + abs(TrenutnaPos - ProslaPos) - - print("Distance traveled: %d deg " % (PredjeniPut * 360 / 4096)) - if PredjeniPut >= TargetPos: - print("Target position reached: [ID:%03d] PresPos:%d PresSpd:%d" % (STS_ID, sts_present_position, sts_present_speed)) - sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, 0, 0) - if sts_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(sts_comm_result)) - if sts_error != 0: - print("%s" % packetHandler.getRxPacketError(sts_error)) - break - - # Close port - portHandler.closePort() diff --git a/toid_interaction/toid_interaction/mechanism/zupcanik.py b/toid_interaction/toid_interaction/mechanism/zupcanik.py new file mode 100644 index 0000000..4eec341 --- /dev/null +++ b/toid_interaction/toid_interaction/mechanism/zupcanik.py @@ -0,0 +1,130 @@ +from .STservo_sdk import PortHandler, sts, COMM_SUCCESS + +class ZupcanikAction: + BAUDRATE: int + DEVICENAME: str + STS_MOVING_ACC: int + + def __init__(self, devicename, baudrate=1000000, sts_moving_acc=50): + self.BAUDRATE = baudrate + self.DEVICENAME = devicename + self.STS_MOVING_ACC = sts_moving_acc + + def zupcanik(self, STS_ID, STS_MOVING_SPEED, TargetPos): + + # Initialize PortHandler instance + portHandler = PortHandler(self.DEVICENAME) + + # Initialize PacketHandler instance + packetHandler = sts(portHandler) + + # Open port + if not portHandler.openPort(): + print("Failed to open the port") + return + + # Set port baudrate + if not portHandler.setBaudRate(self.BAUDRATE): + print("Failed to change the baudrate") + return + + sts_comm_result, sts_error = packetHandler.WheelMode(STS_ID) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + return + elif sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + return + # Read STServo present position + sts_present_position, sts_present_speed, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(STS_ID) + ) + if sts_comm_result != COMM_SUCCESS: + print(packetHandler.getTxRxResult(sts_comm_result)) + else: + print( + "[ID:%03d] PresPos:%d PresSpd:%d" + % (STS_ID, sts_present_position, sts_present_speed) + ) + if sts_error != 0: + print(packetHandler.getRxPacketError(sts_error)) + + TargetPos = TargetPos * 4096 / 360 # Convert to motor ticks + sts_present_position, sts_present_speed, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(STS_ID) + ) + + print( + "Initial position: [ID:%03d] PresPos:%d PresSpd:%d" + % (STS_ID, sts_present_position, sts_present_speed) + ) + + TrenutnaPos = sts_present_position + ProslaPos = sts_present_position + PredjeniPut = TrenutnaPos - ProslaPos + opterecenje = 0 + packetHandler.SetMaxLoad(STS_ID, 80) + + while True: + # Write STServo goal position/moving speed/moving acc + sts_comm_result, sts_error = packetHandler.WriteSpec( + STS_ID, STS_MOVING_SPEED, self.STS_MOVING_ACC + ) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + + ProslaPos = sts_present_position + sts_present_position, sts_present_speed, sts_comm_result, sts_error = ( + packetHandler.ReadPosSpeed(STS_ID) + ) + sts_present_load, sts_comm_result, sts_error = packetHandler.ReadLoad( + STS_ID + ) + sts_max_load, sts_comm_result, sts_error = packetHandler.MaxLoad(STS_ID) + opterecenje = (sts_present_load & ((1 << 10) - 1)) * 0.1 + max_opterecenje = sts_max_load & 255 + print( + "Current position: [ID:%03d] PresPos:%d PresSpd:%d Load:%d Max:%d" + % ( + STS_ID, + sts_present_position, + sts_present_speed, + opterecenje, + max_opterecenje, + ) + ) + + if opterecenje > 75: + print( + "High load detected: [ID:%03d] PresPos:%d PresSpd:%d Load:%d" + % (STS_ID, sts_present_position, sts_present_speed, opterecenje) + ) + sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, 0, 0) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + break + TrenutnaPos = sts_present_position + if abs(TrenutnaPos - ProslaPos) > 3000: + PredjeniPut = PredjeniPut + else: + PredjeniPut = PredjeniPut + abs(TrenutnaPos - ProslaPos) + + print("Distance traveled: %d deg " % (PredjeniPut * 360 / 4096)) + if PredjeniPut >= TargetPos: + print( + "Target position reached: [ID:%03d] PresPos:%d PresSpd:%d" + % (STS_ID, sts_present_position, sts_present_speed) + ) + sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, 0, 0) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + if sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + break + + # Close port + portHandler.closePort() diff --git a/toid_lidar/package.xml b/toid_lidar/package.xml index 0b47a33..da1b7c3 100644 --- a/toid_lidar/package.xml +++ b/toid_lidar/package.xml @@ -19,7 +19,7 @@ tf2_geometry_msgs toid_msgs visualization_msgs - rplidar + rplidar_ros diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index 1142ee6..6c0b3fd 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -15,6 +15,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ActiveElements.msg" "msg/Rival.msg" "srv/SendDouble.srv" + "srv/SendString.srv" "action/SimpleMoveCoords.action" "action/SimpleRotate.action" "action/SimpleTranslateX.action" diff --git a/toid_msgs/srv/SendString.srv b/toid_msgs/srv/SendString.srv new file mode 100644 index 0000000..70a257c --- /dev/null +++ b/toid_msgs/srv/SendString.srv @@ -0,0 +1,2 @@ +string text +--- \ No newline at end of file