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 0000000..91a3dda Binary files /dev/null and b/toid_interaction/toid_interaction/__pycache__/__init__.cpython-312.pyc differ 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 0000000..f4f6b27 Binary files /dev/null and b/toid_interaction/toid_interaction/__pycache__/interaction_node.cpython-312.pyc differ 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 0000000..99ba63e Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/__init__.cpython-312.pyc differ diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_read.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_read.cpython-312.pyc new file mode 100644 index 0000000..d14c148 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_read.cpython-312.pyc differ 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 0000000..3ad1f28 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_write.cpython-312.pyc differ 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 0000000..512aaa2 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/port_handler.cpython-312.pyc differ diff --git a/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/protocol_packet_handler.cpython-312.pyc b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/protocol_packet_handler.cpython-312.pyc new file mode 100644 index 0000000..22f2fa5 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/protocol_packet_handler.cpython-312.pyc differ 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 0000000..01ac8dd Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/scscl.cpython-312.pyc differ 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 0000000..963885b Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/sts.cpython-312.pyc differ 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 0000000..31b89e3 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/stservo_def.cpython-312.pyc differ 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 0000000..f2cf6a5 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/__pycache__/__init__.cpython-312.pyc differ 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 0000000..e9d566b Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/__pycache__/sekvenca_2026.cpython-312.pyc differ 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 0000000..aa5f819 Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/__pycache__/zidovi_load.cpython-312.pyc differ 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 0000000..1589cda Binary files /dev/null and b/toid_interaction/toid_interaction/mechanism/__pycache__/zupcanik.cpython-312.pyc differ diff --git a/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py b/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py index 30677b9..442f370 100644 --- a/toid_interaction/toid_interaction/mechanism/sekvenca_2026.py +++ b/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 -#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 -#sekvecncu 4 nije jos zavrsila poslacu ti to naknadno -from .zidovi_load import * -from .zuocanik import * +# 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 +# ne bude da ima opet previse fajlova +# sekvecncu 4 nije jos zavrsila poslacu ti to naknadno +from .zidovi_load import ZidoviAction +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 - zupcanik(1, -1000, 25) + zidovi = ZidoviAction('/dev/ttyACM1') + 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() - diff --git a/toid_interaction/toid_interaction/mechanism/zidovi_load.py b/toid_interaction/toid_interaction/mechanism/zidovi_load.py index 62d1c9d..b623e2b 100644 --- a/toid_interaction/toid_interaction/mechanism/zidovi_load.py +++ b/toid_interaction/toid_interaction/mechanism/zidovi_load.py @@ -1,284 +1,366 @@ -import sys -import os - -from .STservo_sdk import * +from .STservo_sdk import PortHandler, sts, COMM_SUCCESS, STS_ID # 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): - #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(DEVICENAME) +class ZidoviAction: + BAUDRATE : int + DEVICENAME : str + STS_MOVING_ACC: int + def __init__( + self, + devicename, # Check which port is being used on your controller + baudrate=1000000, # STServo default baudrate + sts_moving_acc=0, # STServo moving acceleration + ): + self.BAUDRATE = baudrate + self.DEVICENAME = devicename + self.STS_MOVING_ACC = sts_moving_acc - # 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(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 + 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