Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

finished toid_interaction node

Browse Source
This commit is contained in:
Pimpest
2026-04-03 21:00:57 +02:00
parent 26ed379300
commit 038568c7e3
28 changed files with 708 additions and 407 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
toid_bt/behavior_trees/behavior1.xml
View File

@@ -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"/>

13
toid_bt/behavior_trees/calibration_routines.xml
View File

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

42
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -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="backwards" type="bool" default="false"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
<input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="backwards" default="false" type="bool"/>
<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="min_angle" type="double" default="0.000000"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
<input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="min_angle" default="0.000000" type="double"/>
<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="min_angle" type="double" default="0.000000"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
<input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="min_angle" default="0.000000" type="double"/>
<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="action_name" type="std::string" default="">Action server name</input_port>
<input_port name="max_speed" default="0.000000" type="double"/>
<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>

43
toid_bt/include/toid_bt/plugins/send_text_action.hpp Normal file
View File

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

9
toid_bt/src/bt_executor.cpp
View File

@@ -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;

3
toid_interaction/setup.py
View File

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

BIN
toid_interaction/toid_interaction/__pycache__/__init__.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/__pycache__/interaction_node.cpython-312.pyc Normal file
View File

Binary file not shown.

97
toid_interaction/toid_interaction/interaction_node.py Normal file
View File

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

BIN
toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/__init__.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_read.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/group_sync_write.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/port_handler.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/protocol_packet_handler.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/scscl.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/sts.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/mechanism/STservo_sdk/__pycache__/stservo_def.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/mechanism/__pycache__/__init__.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/mechanism/__pycache__/sekvenca_2026.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/mechanism/__pycache__/zidovi_load.cpython-312.pyc Normal file
View File

Binary file not shown.

BIN
toid_interaction/toid_interaction/mechanism/__pycache__/zupcanik.cpython-312.pyc Normal file
View File

Binary file not shown.

63
toid_interaction/toid_interaction/mechanism/sekvenca_2026.py
View File

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

582
toid_interaction/toid_interaction/mechanism/zidovi_load.py
View File

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

105
toid_interaction/toid_interaction/mechanism/zuocanik.py
View File

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

130
toid_interaction/toid_interaction/mechanism/zupcanik.py Normal file
View File

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

2
toid_lidar/package.xml
View File

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

1
toid_msgs/CMakeLists.txt
View File

@@ -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"

2
toid_msgs/srv/SendString.srv Normal file
View File

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