toid_bt/behavior_trees/behavior1.xml

@@ -1,5 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
+  <BehaviorTree ID="seq">
+    <Sequence>
+      <Seq1 service_name=""/>
+      <Seq2 text="0101"
+            service_name=""/>
+      <Seq3 service_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
   <BehaviorTree ID="seq1">
     <Sequence>
       <ZeroOdom service_name=""/>
@@ -70,6 +79,20 @@
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
+    <Action ID="Seq1">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+    <Action ID="Seq2">
+      <input_port name="text"
+                  type="std::string"/>
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+    <Action ID="Seq3">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
     <Action ID="TranslateX">
       <input_port name="x"
                   type="double"/>

toid_bt/behavior_trees/calibration_routines.xml

@@ -26,14 +26,14 @@
                       action_name=""/>
       </Sequence>
       <ForceSuccess>
-        <Timeout msec="13000">
+        <DetectStuck timeout="1.000000">
           <MovePointSimple x="-1.0"
                            y="0"
                            theta="0"
                            max_speed="0.070000"
                            backwards="true"
                            action_name=""/>
-        </Timeout>
+        </DetectStuck>
       </ForceSuccess>
       <SetWidth width="{width}"
                 count="1"
@@ -72,14 +72,14 @@
                       action_name=""/>
       </Sequence>
       <ForceSuccess>
-        <Timeout msec="9000">
+        <DetectStuck timeout="1.000000">
           <MovePointSimple x="-0.2"
                            y="0"
                            theta="0"
                            max_speed="0.05"
                            backwards="true"
                            action_name=""/>
-        </Timeout>
+        </DetectStuck>
       </ForceSuccess>
       <Sleep msec="1000"/>
       <EndCalib service_name=""/>
@@ -102,6 +102,11 @@
 
   <!-- Description of Node Models (used by Groot) -->
   <TreeNodesModel>
+    <Decorator ID="DetectStuck">
+      <input_port name="timeout"
+                  default="1.000000"
+                  type="double"/>
+    </Decorator>
     <Action ID="EndCalib">
       <input_port name="service_name"
                   type="std::string">Service name</input_port>

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -5,45 +5,55 @@
     <!-- Description of Node Models (used by Groot) -->
     <TreeNodesModel>
         <Decorator ID="DetectStuck">
-            <input_port name="timeout" type="double" default="1.000000"/>
+            <input_port name="timeout" default="1.000000" type="double"/>
         </Decorator>
         <Action ID="EndCalib">
-            <input_port name="service_name" type="std::string" default="">Service name</input_port>
+            <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
         <Action ID="MovePointSimple">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
             <input_port name="theta" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
-            <input_port name="backwards" type="bool" default="false"/>
+            <input_port name="backwards" default="false" type="bool"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
         <Action ID="RotateSimple">
             <input_port name="angle" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
-            <input_port name="min_angle" type="double" default="0.000000"/>
+            <input_port name="min_angle" default="0.000000" type="double"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
         <Action ID="RotateTowards">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
-            <input_port name="min_angle" type="double" default="0.000000"/>
+            <input_port name="min_angle" default="0.000000" type="double"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="Seq1">
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+        <Action ID="Seq2">
+            <input_port name="text" type="std::string"/>
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+        <Action ID="Seq3">
+            <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
         <Action ID="SetWidth">
             <input_port name="width" type="double"/>
-            <input_port name="count" type="int" default="1"/>
+            <input_port name="count" default="1" type="int"/>
             <output_port name="new_width" type="double"/>
-            <input_port name="service_name" type="std::string" default="">Service name</input_port>
+            <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
         <Action ID="TranslateX">
             <input_port name="x" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
         <Action ID="ZeroOdom">
-            <input_port name="service_name" type="std::string" default="">Service name</input_port>
+            <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
     </TreeNodesModel>
 </root>

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<toid_msgs::srv::SendString>
+{
+public:
+  SendTextNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<std::string>("text"),
+    });
+  }
+
+  bool setRequest(typename Request::SharedPtr &req) override { 
+    std::string text = getInput<std::string>("text").value_or("");
+    req->text = text;
+
+    return true;
+  }
+
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+};
+
+}  // namespace toid

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<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
 
+  factory.registerNodeType<toid::EmptySrvNode>("Seq1", BT::RosNodeParams(nh, "/sequence1"));
+
+  BT::RosNodeParams service_params(nh, "/sequence2");
+  service_params.server_timeout = std::chrono::seconds(15);
+  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params);
+
+  factory.registerNodeType<toid::EmptySrvNode>("Seq3", BT::RosNodeParams(nh, "/sequence3"));
+
   factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);
 
   std::cout << describeCustomNodes() << std::endl;

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'
         ],
     },
 )

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()

toid_interaction/toid_interaction/mechanism/sekvenca_2026.py

@@ -1,64 +1,67 @@
-#u ovom fajlu sam napisala celu sekvencu koju obuhvata hvatanje zirova podizanje, okretanje, spustanje i odlaganje
+# u ovom fajlu sam napisala celu sekvencu koju obuhvata hvatanje zirova podizanje, okretanje, spustanje i odlaganje
-#dole cu napisaci koje funkicje su koja sekvenca ono sto smo pricali , nisam htela da pravim jos jedan fajl sa sekvencama da 
+# dole cu napisaci koje funkicje su koja sekvenca ono sto smo pricali , nisam htela da pravim jos jedan fajl sa sekvencama da
-#ne bude da ima opet previse fajlova
+# ne bude da ima opet previse fajlova
-#sekvecncu 4 nije jos zavrsila poslacu ti to naknadno
+# sekvecncu 4 nije jos zavrsila poslacu ti to naknadno
-from .zidovi_load import *
+from .zidovi_load import ZidoviAction
-from .zuocanik import *
+from .zupcanik import ZupcanikAction
 import time
 import serial
 import serial.tools.list_ports as list_ports
-import platform
 
-SERIAL_ID = '503359270A70619F'
+SERIAL_ID = "50443405C8C3B21C"
 
 
 def okreni(i):
- # Funkci
     for port_info in list_ports.comports():
-        print(port_info.name)
         if port_info.serial_number == SERIAL_ID:
             break
 
     ser = serial.Serial(port_info.device, 115200, timeout=1)
     ser.write(str(i).encode())
 
+
 def okreni_niz(broj):
     """
     Funkcija koja prima string od 4 karaktera (0 ili 1)
     """
     if len(broj) != 4:
         raise ValueError("Binarni niz mora imati tačno 4 karaktera")
-    
+
+    okreni(6)
     for i, char in enumerate(broj):
-        if char == '1':
+        if char == "1":
             okreni(i + 1)
 
 
 def main():
 
-       #sekvenca 1
+    zidovi = ZidoviAction('/dev/ttyACM1')
-      zupcanik(1, -1000, 25)
+    zupcanik = ZupcanikAction('/dev/ttyACM1')
 
-       #sekvenca 2
-      beli_zid(1)
-      plavi_zid(1)
-     
-      okreni_niz("1010")
-     
-      plavi_zid(0)
-      beli_zid(0)
-    
-      #sekvenca 3
-      zupcanik(1, 1000, 25)
 
-      time.sleep(1)
+    # sekvenca 1
+    okreni(5)
+    zupcanik.zupcanik(1, -1010, 25)
 
-     
+    # sekvenca 2
+    zidovi.beli_zid(1)
+    zidovi.plavi_zid(1)
 
- 
+    okreni_niz("1010")
+
+    zidovi.plavi_zid(0, TargetPos=150)
+    zidovi.beli_zid(0, TargetPos=450)
+
+
+    # sekvenca 3
+
+    zupcanik.zupcanik(1, 1010, 25)
+
+    zidovi.plavi_zid(0, TargetPos=150)
+    zidovi.beli_zid(0, TargetPos=150)
+
+    time.sleep(1)
 
-    
 
 if __name__ == "__main__":
     main()
-

toid_interaction/toid_interaction/mechanism/zidovi_load.py

@@ -1,284 +1,366 @@
-import sys
+from .STservo_sdk import PortHandler, sts, COMM_SUCCESS, STS_ID
-import os
-
-from .STservo_sdk import *
 # da li ovo dole treba da importujem ako je nalazi u STservo_sdk?
-#from stservo_def import COMM_SUCCESS
+# from stservo_def import COMM_SUCCESS
 
 # Default settings
-BAUDRATE = 1000000  # STServo default baudrate
-DEVICENAME = 'COM4'  # Check which port is being used on your controller
-STS_MOVING_ACC = 0  # STServo moving acceleration
 
 
-def plavi_zid(smer, brzina=1500, opterecenje_threshold=75,TargetPos=300): 
+class ZidoviAction:
-    #TargetPos = 300 # Target position in degrees
+    BAUDRATE : int
-    TargetPos = TargetPos * 4096 / 360  # Convert to motor ticks
+    DEVICENAME : str
-    if smer == 1:
+    STS_MOVING_ACC: int
-     Speed_ID2 =brzina  # Speed for motor ID 2
+    def __init__(
-     Speed_ID4 = -brzina   # Speed for motor ID 4 (opposite sign)
+        self,
-    else:
+        devicename,  # Check which port is being used on your controller
-     Speed_ID2 = -brzina  # Speed for motor ID 2
+        baudrate=1000000,  # STServo default baudrate
-     Speed_ID4 = brzina   # Speed for motor ID 4 (opposite sign)
+        sts_moving_acc=0,  # STServo moving acceleration
-    
+    ):
-    # Initialize PortHandler instance
+        self.BAUDRATE = baudrate
-    portHandler = PortHandler(DEVICENAME)
+        self.DEVICENAME = devicename
+        self.STS_MOVING_ACC = sts_moving_acc
 
-    # Initialize PacketHandler instance
-    packetHandler = sts(portHandler)
 
-    # Open port
+    def plavi_zid(self, smer, brzina=1500, opterecenje_threshold=75, TargetPos=300):
-    if portHandler.openPort():
+        # TargetPos = 300 # Target position in degrees
-        print("Succeeded to open the port")
+        TargetPos = TargetPos * 4096 / 360  # Convert to motor ticks
-    else:
+        if smer == 1:
-        print("Failed to open the port")
+            Speed_ID2 = brzina  # Speed for motor ID 2
-        print("Press any key to terminate...")
+            Speed_ID4 = -brzina  # Speed for motor ID 4 (opposite sign)
-        
-        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 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, 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, 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:
+            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 ID2:%d deg " % (PredjeniPut_ID2 * 360 / 4096))
-        print("PredjeniPut ID4:%d deg " % (PredjeniPut_ID4 * 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
+        # Close port
-        if PredjeniPut_ID2 >= TargetPos and PredjeniPut_ID4 >= TargetPos+10:
+        portHandler.closePort()
-            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
+    def beli_zid(self, smer, brzina=1500, opterecenje_threshold=75, TargetPos=600):
-    portHandler.closePort()
+        # 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)
-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
+        # Initialize PacketHandler instance
-    portHandler = PortHandler(DEVICENAME)
+        packetHandler = sts(portHandler)
 
-    # Initialize PacketHandler instance
+        # Open port
-    packetHandler = sts(portHandler)
+        if portHandler.openPort():
+            print("Succeeded to open the port")
+        else:
+            print("Failed to open the port")
+            print("Press any key to terminate...")
 
-    # Open port
+            quit()
-    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
+        # Set port baudrate
-    if portHandler.setBaudRate(BAUDRATE):
+        if portHandler.setBaudRate(self.BAUDRATE):
-        print("Succeeded to change the baudrate")
+            print("Succeeded to change the baudrate")
-    else:
+        else:
-        print("Failed to change the baudrate")
+            print("Failed to change the baudrate")
-        print("Press any key to terminate...")
+            print("Press any key to terminate...")
-        
-        quit()
 
-    # Set motors to Wheel Mode
+            quit()
-    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)))
 
-    # Read initial positions for both motors
+        # Set motors to Wheel Mode
-    sts_present_position_ID3, sts_present_speed_ID3, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(3)
+        for motor_id in [3, 5]:
-    sts_present_position_ID5, sts_present_speed_ID5, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(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:")
+        # Read initial positions for both motors
-  #  print("[ID:003] PresPos:%d PresSpd:%d" % (sts_present_position_ID3, sts_present_speed_ID3))
+        sts_present_position_ID3, sts_present_speed_ID3, sts_comm_result, sts_error = (
-   # print("[ID:005] PresPos:%d PresSpd:%d" % (sts_present_position_ID5, sts_present_speed_ID5))
+            packetHandler.ReadPosSpeed(3)
+        )
+        sts_present_position_ID5, sts_present_speed_ID5, sts_comm_result, sts_error = (
+            packetHandler.ReadPosSpeed(5)
+        )
 
-    # Initialize variables for tracking movement
+        print("Initial positions:")
-    TrenutnaPos_ID3 = sts_present_position_ID3
+        #  print("[ID:003] PresPos:%d PresSpd:%d" % (sts_present_position_ID3, sts_present_speed_ID3))
-    ProslaPos_ID3 = sts_present_position_ID3
+        # print("[ID:005] PresPos:%d PresSpd:%d" % (sts_present_position_ID5, sts_present_speed_ID5))
-    PredjeniPut_ID3 = 0
 
-    TrenutnaPos_ID5 = sts_present_position_ID5
+        # Initialize variables for tracking movement
-    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))
-      
         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
+        TrenutnaPos_ID5 = sts_present_position_ID5
-        if opterecenje3 > opterecenje_threshold or opterecenje5 > opterecenje_threshold:
+        ProslaPos_ID5 = sts_present_position_ID5
-            print("High load detected: [ID:003] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID3, sts_present_speed_ID3, opterecenje3))
+        PredjeniPut_ID5 = 0
-            print("High load detected: [ID:005] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5))
+        packetHandler.SetMaxLoad(STS_ID, 80)
-            packetHandler.WriteSpec(3, 0, 0)  # Stop motor ID 3
+
-            packetHandler.WriteSpec(5, 0, 0)  # Stop motor ID 5
+        while True:
-            break
+            # Write goal position, speed, and acceleration for both motors
-        if abs(TrenutnaPos_ID3 - ProslaPos_ID3) > 3000 or abs(TrenutnaPos_ID5 - ProslaPos_ID5) > 3000:
+            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))
+            # Stop if both motors reach their target positions
-        print("PredjeniPut ID5:%d deg " % (PredjeniPut_ID5 * 360 / 4096))
+            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
-        # Stop if both motors reach their target positions
+                break
-        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()
-
 
+        # Close port
+        portHandler.closePort()

toid_interaction/toid_interaction/mechanism/zuocanik.py

@@ -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()

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()

toid_lidar/package.xml

@@ -19,7 +19,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>toid_msgs</depend>
   <depend>visualization_msgs</depend>
-  <depend>rplidar</depend>
+  <depend>rplidar_ros</depend>
 
 
 

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"

toid_msgs/srv/SendString.srv

@@ -0,0 +1,2 @@
+string text
+---