toid_interaction/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>toid_interaction</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="82343504+pimpest@users.noreply.github.com">pimpest</maintainer>
+  <license>MIT</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <depend>python3-serial</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

toid_interaction/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/toid_interaction
+[install]
+install_scripts=$base/lib/toid_interaction

toid_interaction/setup.py

@@ -0,0 +1,30 @@
+from setuptools import find_packages, setup
+
+package_name = 'toid_interaction'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='pimpest',
+    maintainer_email='82343504+pimpest@users.noreply.github.com',
+    description='TODO: Package description',
+    license='MIT',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'sequence = toid_interaction.mechanism.sekvenca_2026:main'
+        ],
+    },
+)

toid_interaction/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

toid_interaction/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

toid_interaction/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py

@@ -0,0 +1,8 @@
+#!/usr/bin/env python
+
+from .port_handler import *
+from .protocol_packet_handler import *
+from .group_sync_write import *
+from .group_sync_read import *
+from .sts import *
+from .scscl import *

toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py

@@ -0,0 +1,151 @@
+#!/usr/bin/env python
+
+from .stservo_def import *
+
+class GroupSyncRead:
+    def __init__(self, ph, start_address, data_length):
+        self.ph = ph
+        self.start_address = start_address
+        self.data_length = data_length
+
+        self.last_result = False
+        self.is_param_changed = False
+        self.param = []
+        self.data_dict = {}
+
+        self.clearParam()
+
+    def makeParam(self):
+        if not self.data_dict:  # len(self.data_dict.keys()) == 0:
+            return
+
+        self.param = []
+
+        for scs_id in self.data_dict:
+            self.param.append(scs_id)
+
+    def addParam(self, sts_id):
+        if sts_id in self.data_dict:  # sts_id already exist
+            return False
+
+        self.data_dict[sts_id] = []  # [0] * self.data_length
+
+        self.is_param_changed = True
+        return True
+
+    def removeParam(self, sts_id):
+        if sts_id not in self.data_dict:  # NOT exist
+            return
+
+        del self.data_dict[sts_id]
+
+        self.is_param_changed = True
+
+    def clearParam(self):
+        self.data_dict.clear()
+
+    def txPacket(self):
+        if len(self.data_dict.keys()) == 0:
+
+            return COMM_NOT_AVAILABLE
+
+        if self.is_param_changed is True or not self.param:
+            self.makeParam()
+
+        return self.ph.syncReadTx(self.start_address, self.data_length, self.param, len(self.data_dict.keys()))
+
+    def rxPacket(self):
+        self.last_result = True
+
+        result = COMM_RX_FAIL
+
+        if len(self.data_dict.keys()) == 0:
+            return COMM_NOT_AVAILABLE
+
+        result, rxpacket = self.ph.syncReadRx(self.data_length, len(self.data_dict.keys()))
+        # print(rxpacket)
+        if len(rxpacket) >= (self.data_length+6):
+            for sts_id in self.data_dict:
+                self.data_dict[sts_id], result = self.readRx(rxpacket, sts_id, self.data_length)
+                if result != COMM_SUCCESS:
+                    self.last_result = False
+                # print(sts_id)
+        else:
+            self.last_result = False
+        # print(self.last_result)
+        return result
+
+    def txRxPacket(self):
+        result = self.txPacket()
+        if result != COMM_SUCCESS:
+            return result
+
+        return self.rxPacket()
+
+    def readRx(self, rxpacket, sts_id, data_length):
+        # print(sts_id)
+        # print(rxpacket)
+        data = []
+        rx_length = len(rxpacket)
+        # print(rx_length)
+        rx_index = 0;
+        while (rx_index+6+data_length) <= rx_length:
+            headpacket = [0x00, 0x00, 0x00]
+            while rx_index < rx_length:
+                headpacket[2] = headpacket[1];
+                headpacket[1] = headpacket[0];
+                headpacket[0] = rxpacket[rx_index];
+                rx_index += 1
+                if (headpacket[2] == 0xFF) and (headpacket[1] == 0xFF) and headpacket[0] == sts_id:
+                    # print(rx_index)
+                    break
+            # print(rx_index+3+data_length)
+            if (rx_index+3+data_length) > rx_length:
+                break;
+            if rxpacket[rx_index] != (data_length+2):
+                rx_index += 1
+                # print(rx_index)
+                continue
+            rx_index += 1
+            Error = rxpacket[rx_index]
+            rx_index += 1
+            calSum = sts_id + (data_length+2) + Error
+            data = [Error]
+            data.extend(rxpacket[rx_index : rx_index+data_length])
+            for i in range(0, data_length):
+                calSum += rxpacket[rx_index]
+                rx_index += 1
+            calSum = ~calSum & 0xFF
+            # print(calSum)
+            if calSum != rxpacket[rx_index]:
+                return None, COMM_RX_CORRUPT
+            return data, COMM_SUCCESS 
+        # print(rx_index)
+        return None, COMM_RX_CORRUPT
+
+    def isAvailable(self, sts_id, address, data_length):
+        #if self.last_result is False or sts_id not in self.data_dict:
+        if sts_id not in self.data_dict:
+            return False, 0
+
+        if (address < self.start_address) or (self.start_address + self.data_length - data_length < address):
+            return False, 0
+        if not self.data_dict[sts_id]:
+            return False, 0
+        if len(self.data_dict[sts_id])<(data_length+1):
+            return False, 0
+        return True, self.data_dict[sts_id][0]
+
+    def getData(self, sts_id, address, data_length):
+        if data_length == 1:
+            return self.data_dict[sts_id][address-self.start_address+1]
+        elif data_length == 2:
+            return self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+1],
+                                self.data_dict[sts_id][address-self.start_address+2])
+        elif data_length == 4:
+            return self.ph.scs_makedword(self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+1],
+                                              self.data_dict[sts_id][address-self.start_address+2]),
+                                 self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+3],
+                                              self.data_dict[sts_id][address-self.start_address+4]))
+        else:
+            return 0

toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python
+
+from .stservo_def import *
+
+class GroupSyncWrite:
+    def __init__(self, ph, start_address, data_length):
+        self.ph = ph
+        self.start_address = start_address
+        self.data_length = data_length
+
+        self.is_param_changed = False
+        self.param = []
+        self.data_dict = {}
+
+        self.clearParam()
+
+    def makeParam(self):
+        if not self.data_dict:
+            return
+
+        self.param = []
+
+        for sts_id in self.data_dict:
+            if not self.data_dict[sts_id]:
+                return
+
+            self.param.append(sts_id)
+            self.param.extend(self.data_dict[sts_id])
+
+    def addParam(self, sts_id, data):
+        if sts_id in self.data_dict:  # sts_id already exist
+            return False
+
+        if len(data) > self.data_length:  # input data is longer than set
+            return False
+
+        self.data_dict[sts_id] = data
+
+        self.is_param_changed = True
+        return True
+
+    def removeParam(self, sts_id):
+        if sts_id not in self.data_dict:  # NOT exist
+            return
+
+        del self.data_dict[sts_id]
+
+        self.is_param_changed = True
+
+    def changeParam(self, sts_id, data):
+        if sts_id not in self.data_dict:  # NOT exist
+            return False
+
+        if len(data) > self.data_length:  # input data is longer than set
+            return False
+
+        self.data_dict[sts_id] = data
+
+        self.is_param_changed = True
+        return True
+
+    def clearParam(self):
+        self.data_dict.clear()
+
+    def txPacket(self):
+        if len(self.data_dict.keys()) == 0:
+            return COMM_NOT_AVAILABLE
+
+        if self.is_param_changed is True or not self.param:
+            self.makeParam()
+
+        return self.ph.syncWriteTxOnly(self.start_address, self.data_length, self.param,
+                                       len(self.data_dict.keys()) * (1 + self.data_length))

toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py

@@ -0,0 +1,115 @@
+#!/usr/bin/env python
+
+import time
+import serial
+import sys
+import platform
+
+DEFAULT_BAUDRATE = 1000000
+LATENCY_TIMER = 50 
+
+class PortHandler(object):
+    def __init__(self, port_name):
+        self.is_open = False
+        self.baudrate = DEFAULT_BAUDRATE
+        self.packet_start_time = 0.0
+        self.packet_timeout = 0.0
+        self.tx_time_per_byte = 0.0
+
+        self.is_using = False
+        self.port_name = port_name
+        self.ser = None
+
+    def openPort(self):
+        return self.setBaudRate(self.baudrate)
+
+    def closePort(self):
+        self.ser.close()
+        self.is_open = False
+
+    def clearPort(self):
+        self.ser.flush()
+
+    def setPortName(self, port_name):
+        self.port_name = port_name
+
+    def getPortName(self):
+        return self.port_name
+
+    def setBaudRate(self, baudrate):
+        baud = self.getCFlagBaud(baudrate)
+
+        if baud <= 0:
+            # self.setupPort(38400)
+            # self.baudrate = baudrate
+            return False  # TODO: setCustomBaudrate(baudrate)
+        else:
+            self.baudrate = baudrate
+            return self.setupPort(baud)
+
+    def getBaudRate(self):
+        return self.baudrate
+
+    def getBytesAvailable(self):
+        return self.ser.in_waiting
+
+    def readPort(self, length):
+        if (sys.version_info > (3, 0)):
+            return self.ser.read(length)
+        else:
+            return [ord(ch) for ch in self.ser.read(length)]
+
+    def writePort(self, packet):
+        return self.ser.write(packet)
+
+    def setPacketTimeout(self, packet_length):
+        self.packet_start_time = self.getCurrentTime()
+        self.packet_timeout = (self.tx_time_per_byte * packet_length) + (self.tx_time_per_byte * 3.0) + LATENCY_TIMER
+
+    def setPacketTimeoutMillis(self, msec):
+        self.packet_start_time = self.getCurrentTime()
+        self.packet_timeout = msec
+
+    def isPacketTimeout(self):
+        if self.getTimeSinceStart() > self.packet_timeout:
+            self.packet_timeout = 0
+            return True
+
+        return False
+
+    def getCurrentTime(self):
+        return round(time.time() * 1000000000) / 1000000.0
+
+    def getTimeSinceStart(self):
+        time_since = self.getCurrentTime() - self.packet_start_time
+        if time_since < 0.0:
+            self.packet_start_time = self.getCurrentTime()
+
+        return time_since
+
+    def setupPort(self, cflag_baud):
+        if self.is_open:
+            self.closePort()
+
+        self.ser = serial.Serial(
+            port=self.port_name,
+            baudrate=self.baudrate,
+            # parity = serial.PARITY_ODD,
+            # stopbits = serial.STOPBITS_TWO,
+            bytesize=serial.EIGHTBITS,
+            timeout=0
+        )
+
+        self.is_open = True
+
+        self.ser.reset_input_buffer()
+
+        self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0
+
+        return True
+
+    def getCFlagBaud(self, baudrate):
+        if baudrate in [4800, 9600, 14400, 19200, 38400, 57600, 115200, 128000, 250000, 500000, 1000000]:
+            return baudrate
+        else:
+            return -1          

toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py

@@ -0,0 +1,530 @@
+#!/usr/bin/env python
+
+from .stservo_def import *
+
+TXPACKET_MAX_LEN = 250
+RXPACKET_MAX_LEN = 250
+
+# for Protocol Packet
+PKT_HEADER0 = 0
+PKT_HEADER1 = 1
+PKT_ID = 2
+PKT_LENGTH = 3
+PKT_INSTRUCTION = 4
+PKT_ERROR = 4
+PKT_PARAMETER0 = 5
+
+# Protocol Error bit
+ERRBIT_VOLTAGE = 1
+ERRBIT_ANGLE = 2
+ERRBIT_OVERHEAT = 4
+ERRBIT_OVERELE = 8
+ERRBIT_OVERLOAD = 32
+
+
+class protocol_packet_handler(object):
+    def __init__(self, portHandler, protocol_end):
+        #self.sts_setend(protocol_end)# STServo bit end(STS/SMS=0, SCS=1)
+        self.portHandler = portHandler
+        self.sts_end = protocol_end
+
+    def sts_getend(self):
+        return self.sts_end
+
+    def sts_setend(self, e):
+        self.sts_end = e
+
+    def sts_tohost(self, a, b):
+        if (a & (1<<b)):
+            return -(a & ~(1<<b))
+        else:
+            return a
+
+    def sts_toscs(self, a, b):
+        if (a<0):
+            return (-a | (1<<b))
+        else:
+            return a
+
+    def sts_makeword(self, a, b):
+        if self.sts_end==0:
+            return (a & 0xFF) | ((b & 0xFF) << 8)
+        else:
+            return (b & 0xFF) | ((a & 0xFF) << 8)
+
+    def sts_makedword(self, a, b):
+        return (a & 0xFFFF) | (b & 0xFFFF) << 16
+
+    def sts_loword(self, l):
+        return l & 0xFFFF
+
+    def sts_hiword(self, h):
+        return (h >> 16) & 0xFFFF
+
+    def sts_lobyte(self, w):
+        if self.sts_end==0:
+            return w & 0xFF
+        else:
+            return (w >> 8) & 0xFF
+
+    def sts_hibyte(self, w):
+        if self.sts_end==0:
+            return (w >> 8) & 0xFF
+        else:
+            return w & 0xFF
+        
+    def getProtocolVersion(self):
+        return 1.0
+
+    def getTxRxResult(self, result):
+        if result == COMM_SUCCESS:
+            return "[TxRxResult] Communication success!"
+        elif result == COMM_PORT_BUSY:
+            return "[TxRxResult] Port is in use!"
+        elif result == COMM_TX_FAIL:
+            return "[TxRxResult] Failed transmit instruction packet!"
+        elif result == COMM_RX_FAIL:
+            return "[TxRxResult] Failed get status packet from device!"
+        elif result == COMM_TX_ERROR:
+            return "[TxRxResult] Incorrect instruction packet!"
+        elif result == COMM_RX_WAITING:
+            return "[TxRxResult] Now receiving status packet!"
+        elif result == COMM_RX_TIMEOUT:
+            return "[TxRxResult] There is no status packet!"
+        elif result == COMM_RX_CORRUPT:
+            return "[TxRxResult] Incorrect status packet!"
+        elif result == COMM_NOT_AVAILABLE:
+            return "[TxRxResult] Protocol does not support this function!"
+        else:
+            return ""
+
+    def getRxPacketError(self, error):
+        if error & ERRBIT_VOLTAGE:
+            return "[ServoStatus] Input voltage error!"
+
+        if error & ERRBIT_ANGLE:
+            return "[ServoStatus] Angle sen error!"
+
+        if error & ERRBIT_OVERHEAT:
+            return "[ServoStatus] Overheat error!"
+
+        if error & ERRBIT_OVERELE:
+            return "[ServoStatus] OverEle error!"
+        
+        if error & ERRBIT_OVERLOAD:
+            return "[ServoStatus] Overload error!"
+
+        return ""
+
+    def txPacket(self, txpacket):
+        checksum = 0
+        total_packet_length = txpacket[PKT_LENGTH] + 4  # 4: HEADER0 HEADER1 ID LENGTH
+
+        if self.portHandler.is_using:
+            return COMM_PORT_BUSY
+        self.portHandler.is_using = True
+
+        # check max packet length
+        if total_packet_length > TXPACKET_MAX_LEN:
+            self.portHandler.is_using = False
+            return COMM_TX_ERROR
+
+        # make packet header
+        txpacket[PKT_HEADER0] = 0xFF
+        txpacket[PKT_HEADER1] = 0xFF
+
+        # add a checksum to the packet
+        for idx in range(2, total_packet_length - 1):  # except header, checksum
+            checksum += txpacket[idx]
+
+        txpacket[total_packet_length - 1] = ~checksum & 0xFF
+
+        #print "[TxPacket] %r" % txpacket
+
+        # tx packet
+        self.portHandler.clearPort()
+        written_packet_length = self.portHandler.writePort(txpacket)
+        if total_packet_length != written_packet_length:
+            self.portHandler.is_using = False
+            return COMM_TX_FAIL
+
+        return COMM_SUCCESS
+
+    def rxPacket(self):
+        rxpacket = []
+
+        result = COMM_TX_FAIL
+        checksum = 0
+        rx_length = 0
+        wait_length = 6  # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM)
+
+        while True:
+            rxpacket.extend(self.portHandler.readPort(wait_length - rx_length))
+            rx_length = len(rxpacket)
+            if rx_length >= wait_length:
+                # find packet header
+                for idx in range(0, (rx_length - 1)):
+                    if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF):
+                        break
+
+                if idx == 0:  # found at the beginning of the packet
+                    if (rxpacket[PKT_ID] > 0xFD) or (rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) or (
+                            rxpacket[PKT_ERROR] > 0x7F):
+                        # unavailable ID or unavailable Length or unavailable Error
+                        # remove the first byte in the packet
+                        del rxpacket[0]
+                        rx_length -= 1
+                        continue
+
+                    # re-calculate the exact length of the rx packet
+                    if wait_length != (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1):
+                        wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1
+                        continue
+
+                    if rx_length < wait_length:
+                        # check timeout
+                        if self.portHandler.isPacketTimeout():
+                            if rx_length == 0:
+                                result = COMM_RX_TIMEOUT
+                            else:
+                                result = COMM_RX_CORRUPT
+                            break
+                        else:
+                            continue
+
+                    # calculate checksum
+                    for i in range(2, wait_length - 1):  # except header, checksum
+                        checksum += rxpacket[i]
+                    checksum = ~checksum & 0xFF
+
+                    # verify checksum
+                    if rxpacket[wait_length - 1] == checksum:
+                        result = COMM_SUCCESS
+                    else:
+                        result = COMM_RX_CORRUPT
+                    break
+
+                else:
+                    # remove unnecessary packets
+                    del rxpacket[0: idx]
+                    rx_length -= idx
+
+            else:
+                # check timeout
+                if self.portHandler.isPacketTimeout():
+                    if rx_length == 0:
+                        result = COMM_RX_TIMEOUT
+                    else:
+                        result = COMM_RX_CORRUPT
+                    break
+
+        self.portHandler.is_using = False
+        return rxpacket, result
+
+    def txRxPacket(self, txpacket):
+        rxpacket = None
+        error = 0
+
+        # tx packet
+        result = self.txPacket(txpacket)
+        if result != COMM_SUCCESS:
+            return rxpacket, result, error
+
+        # (ID == Broadcast ID) == no need to wait for status packet or not available
+        if (txpacket[PKT_ID] == BROADCAST_ID):
+            self.portHandler.is_using = False
+            return rxpacket, result, error
+
+        # set packet timeout
+        if txpacket[PKT_INSTRUCTION] == INST_READ:
+            self.portHandler.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6)
+        else:
+            self.portHandler.setPacketTimeout(6)  # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM
+
+        # rx packet
+        while True:
+            rxpacket, result = self.rxPacket()
+            if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]:
+                break
+
+        if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]:
+            error = rxpacket[PKT_ERROR]
+
+        return rxpacket, result, error
+
+    def ping(self, sts_id):
+        model_number = 0
+        error = 0
+
+        txpacket = [0] * 6
+
+        if sts_id >= BROADCAST_ID:
+            return model_number, COMM_NOT_AVAILABLE, error
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = 2
+        txpacket[PKT_INSTRUCTION] = INST_PING
+
+        rxpacket, result, error = self.txRxPacket(txpacket)
+
+        if result == COMM_SUCCESS:
+            data_read, result, error = self.readTxRx(sts_id, 3, 2)  # Address 3 : Model Number
+            if result == COMM_SUCCESS:
+                model_number = self.sts_makeword(data_read[0], data_read[1])
+
+        return model_number, result, error
+
+    def action(self, sts_id):
+        txpacket = [0] * 6
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = 2
+        txpacket[PKT_INSTRUCTION] = INST_ACTION
+
+        _, result, _ = self.txRxPacket(txpacket)
+
+        return result
+
+    def readTx(self, sts_id, address, length):
+
+        txpacket = [0] * 8
+
+        if sts_id >= BROADCAST_ID:
+            return COMM_NOT_AVAILABLE
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = 4
+        txpacket[PKT_INSTRUCTION] = INST_READ
+        txpacket[PKT_PARAMETER0 + 0] = address
+        txpacket[PKT_PARAMETER0 + 1] = length
+
+        result = self.txPacket(txpacket)
+
+        # set packet timeout
+        if result == COMM_SUCCESS:
+            self.portHandler.setPacketTimeout(length + 6)
+
+        return result
+
+    def readRx(self, sts_id, length):
+        result = COMM_TX_FAIL
+        error = 0
+
+        rxpacket = None
+        data = []
+
+        while True:
+            rxpacket, result = self.rxPacket()
+
+            if result != COMM_SUCCESS or rxpacket[PKT_ID] == sts_id:
+                break
+
+        if result == COMM_SUCCESS and rxpacket[PKT_ID] == sts_id:
+            error = rxpacket[PKT_ERROR]
+
+            data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0+length])
+
+        return data, result, error
+
+    def readTxRx(self, sts_id, address, length):
+        txpacket = [0] * 8
+        data = []
+
+        if sts_id >= BROADCAST_ID:
+            return data, COMM_NOT_AVAILABLE, 0
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = 4
+        txpacket[PKT_INSTRUCTION] = INST_READ
+        txpacket[PKT_PARAMETER0 + 0] = address
+        txpacket[PKT_PARAMETER0 + 1] = length
+
+        rxpacket, result, error = self.txRxPacket(txpacket)
+        if result == COMM_SUCCESS:
+            error = rxpacket[PKT_ERROR]
+
+            data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0+length])
+
+        return data, result, error
+
+    def read1ByteTx(self, sts_id, address):
+        return self.readTx(sts_id, address, 1)
+
+    def read1ByteRx(self, sts_id):
+        data, result, error = self.readRx(sts_id, 1)
+        data_read = data[0] if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def read1ByteTxRx(self, sts_id, address):
+        data, result, error = self.readTxRx(sts_id, address, 1)
+        data_read = data[0] if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def read2ByteTx(self, sts_id, address):
+        return self.readTx(sts_id, address, 2)
+
+    def read2ByteRx(self, sts_id):
+        data, result, error = self.readRx(sts_id, 2)
+        data_read = self.sts_makeword(data[0], data[1]) if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def read2ByteTxRx(self, sts_id, address):
+        data, result, error = self.readTxRx(sts_id, address, 2)
+        data_read = self.sts_makeword(data[0], data[1]) if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def read4ByteTx(self, sts_id, address):
+        return self.readTx(sts_id, address, 4)
+
+    def read4ByteRx(self, sts_id):
+        data, result, error = self.readRx(sts_id, 4)
+        data_read = self.sts_makedword(self.sts_makeword(data[0], data[1]),
+                                  self.sts_makeword(data[2], data[3])) if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def read4ByteTxRx(self, sts_id, address):
+        data, result, error = self.readTxRx(sts_id, address, 4)
+        data_read = self.sts_makedword(self.sts_makeword(data[0], data[1]),
+                                  self.sts_makeword(data[2], data[3])) if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def writeTxOnly(self, sts_id, address, length, data):
+        txpacket = [0] * (length + 7)
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = length + 3
+        txpacket[PKT_INSTRUCTION] = INST_WRITE
+        txpacket[PKT_PARAMETER0] = address
+
+        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
+
+        result = self.txPacket(txpacket)
+        self.portHandler.is_using = False
+
+        return result
+
+    def writeTxRx(self, sts_id, address, length, data):
+        txpacket = [0] * (length + 7)
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = length + 3
+        txpacket[PKT_INSTRUCTION] = INST_WRITE
+        txpacket[PKT_PARAMETER0] = address
+
+        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
+        rxpacket, result, error = self.txRxPacket(txpacket)
+
+        return result, error
+
+    def write1ByteTxOnly(self, sts_id, address, data):
+        data_write = [data]
+        return self.writeTxOnly(sts_id, address, 1, data_write)
+
+    def write1ByteTxRx(self, sts_id, address, data):
+        data_write = [data]
+        return self.writeTxRx(sts_id, address, 1, data_write)
+
+    def write2ByteTxOnly(self, sts_id, address, data):
+        data_write = [self.sts_lobyte(data), self.sts_hibyte(data)]
+        return self.writeTxOnly(sts_id, address, 2, data_write)
+
+    def write2ByteTxRx(self, sts_id, address, data):
+        data_write = [self.sts_lobyte(data), self.sts_hibyte(data)]
+        return self.writeTxRx(sts_id, address, 2, data_write)
+
+    def write4ByteTxOnly(self, sts_id, address, data):
+        data_write = [self.sts_lobyte(self.sts_loword(data)),
+                      self.sts_hibyte(self.sts_loword(data)),
+                      self.sts_lobyte(self.sts_hiword(data)),
+                      self.sts_hibyte(self.sts_hiword(data))]
+        return self.writeTxOnly(sts_id, address, 4, data_write)
+
+    def write4ByteTxRx(self, sts_id, address, data):
+        data_write = [self.sts_lobyte(self.sts_loword(data)),
+                      self.sts_hibyte(self.sts_loword(data)),
+                      self.sts_lobyte(self.sts_hiword(data)),
+                      self.sts_hibyte(self.sts_hiword(data))]
+        return self.writeTxRx(sts_id, address, 4, data_write)
+
+    def regWriteTxOnly(self, sts_id, address, length, data):
+        txpacket = [0] * (length + 7)
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = length + 3
+        txpacket[PKT_INSTRUCTION] = INST_REG_WRITE
+        txpacket[PKT_PARAMETER0] = address
+
+        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
+
+        result = self.txPacket(txpacket)
+        self.portHandler.is_using = False
+
+        return result
+
+    def regWriteTxRx(self, sts_id, address, length, data):
+        txpacket = [0] * (length + 7)
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = length + 3
+        txpacket[PKT_INSTRUCTION] = INST_REG_WRITE
+        txpacket[PKT_PARAMETER0] = address
+
+        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
+
+        _, result, error = self.txRxPacket(txpacket)
+
+        return result, error
+
+    def syncReadTx(self, start_address, data_length, param, param_length):
+        txpacket = [0] * (param_length + 8)
+        # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN CHKSUM
+
+        txpacket[PKT_ID] = BROADCAST_ID
+        txpacket[PKT_LENGTH] = param_length + 4  # 7: INST START_ADDR DATA_LEN CHKSUM
+        txpacket[PKT_INSTRUCTION] = INST_SYNC_READ
+        txpacket[PKT_PARAMETER0 + 0] = start_address
+        txpacket[PKT_PARAMETER0 + 1] = data_length
+
+        txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length]
+
+        # print(txpacket)
+        result = self.txPacket(txpacket)
+        return result
+
+    def syncReadRx(self, data_length, param_length):
+        wait_length = (6 + data_length) * param_length
+        self.portHandler.setPacketTimeout(wait_length)
+        rxpacket = []
+        rx_length = 0
+        while True:
+            rxpacket.extend(self.portHandler.readPort(wait_length - rx_length))
+            rx_length = len(rxpacket)
+            if rx_length >= wait_length:
+                result = COMM_SUCCESS
+                break
+            else:
+                # check timeout
+                if self.portHandler.isPacketTimeout():
+                    if rx_length == 0:
+                        result = COMM_RX_TIMEOUT
+                    else:
+                        result = COMM_RX_CORRUPT
+                    break
+        self.portHandler.is_using = False
+        return result, rxpacket
+
+    def syncWriteTxOnly(self, start_address, data_length, param, param_length):
+        txpacket = [0] * (param_length + 8)
+        # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM
+
+        txpacket[PKT_ID] = BROADCAST_ID
+        txpacket[PKT_LENGTH] = param_length + 4  # 4: INST START_ADDR DATA_LEN ... CHKSUM
+        txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE
+        txpacket[PKT_PARAMETER0 + 0] = start_address
+        txpacket[PKT_PARAMETER0 + 1] = data_length
+
+        txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length]
+
+        _, result, _ = self.txRxPacket(txpacket)
+
+        return result

toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python
+
+from .stservo_def import *
+from .protocol_packet_handler import *
+from .group_sync_write import *
+
+#波特率定义
+SCSCL_1M = 0
+SCSCL_0_5M = 1
+SCSCL_250K = 2
+SCSCL_128K = 3
+SCSCL_115200 = 4
+SCSCL_76800 = 5
+SCSCL_57600 = 6
+SCSCL_38400 = 7
+
+#内存表定义
+#-------EPROM(只读)--------
+SCSCL_MODEL_L = 3
+SCSCL_MODEL_H = 4
+
+#-------EPROM(读写)--------
+scs_id = 5
+SCSCL_BAUD_RATE = 6
+SCSCL_MIN_ANGLE_LIMIT_L = 9
+SCSCL_MIN_ANGLE_LIMIT_H = 10
+SCSCL_MAX_ANGLE_LIMIT_L = 11
+SCSCL_MAX_ANGLE_LIMIT_H = 12
+SCSCL_CW_DEAD = 26
+SCSCL_CCW_DEAD = 27
+
+#-------SRAM(读写)--------
+SCSCL_TORQUE_ENABLE = 40
+SCSCL_GOAL_POSITION_L = 42
+SCSCL_GOAL_POSITION_H = 43
+SCSCL_GOAL_TIME_L = 44
+SCSCL_GOAL_TIME_H = 45
+SCSCL_GOAL_SPEED_L = 46
+SCSCL_GOAL_SPEED_H = 47
+SCSCL_LOCK = 48
+
+#-------SRAM(只读)--------
+SCSCL_PRESENT_POSITION_L  = 56
+SCSCL_PRESENT_POSITION_H = 57
+SCSCL_PRESENT_SPEED_L = 58
+SCSCL_PRESENT_SPEED_H = 59
+SCSCL_PRESENT_LOAD_L = 60
+SCSCL_PRESENT_LOAD_H = 61
+SCSCL_PRESENT_VOLTAGE = 62
+SCSCL_PRESENT_TEMPERATURE = 63
+SCSCL_MOVING = 66
+SCSCL_PRESENT_CURRENT_L = 69
+SCSCL_PRESENT_CURRENT_H = 70
+
+class scscl(protocol_packet_handler):
+    def __init__(self, portHandler):
+        protocol_packet_handler.__init__(self, portHandler, 1)
+        self.groupSyncWrite = GroupSyncWrite(self, SCSCL_GOAL_POSITION_L, 6)
+
+    def WritePos(self, scs_id, position, time, speed):
+        txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)]
+        return self.writeTxRx(scs_id, SCSCL_GOAL_POSITION_L, len(txpacket), txpacket)
+
+    def ReadPos(self, scs_id):
+        scs_present_position, scs_comm_result, scs_error = self.read2ByteTxRx(scs_id, SCSCL_PRESENT_POSITION_L)
+        return scs_present_position, scs_comm_result, scs_error
+
+    def ReadSpeed(self, scs_id):
+        scs_present_speed, scs_comm_result, scs_error = self.read2ByteTxRx(scs_id, SCSCL_PRESENT_SPEED_L)
+        return self.scs_tohost(scs_present_speed, 15), scs_comm_result, scs_error
+
+    def ReadPosSpeed(self, scs_id):
+        scs_present_position_speed, scs_comm_result, scs_error = self.read4ByteTxRx(scs_id, SCSCL_PRESENT_POSITION_L)
+        scs_present_position = self.scs_loword(scs_present_position_speed)
+        scs_present_speed = self.scs_hiword(scs_present_position_speed)
+        return scs_present_position, self.scs_tohost(scs_present_speed, 15), scs_comm_result, scs_error
+
+    def ReadMoving(self, scs_id):
+        moving, scs_comm_result, scs_error = self.read1ByteTxRx(scs_id, SCSCL_MOVING)
+        return moving, scs_comm_result, scs_error
+
+    def SyncWritePos(self, scs_id, position, time, speed):
+        txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)]
+        return self.groupSyncWrite.addParam(scs_id, txpacket)
+
+    def RegWritePos(self, scs_id, position, time, speed):
+        txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)]
+        return self.regWriteTxRx(scs_id, SCSCL_GOAL_POSITION_L, len(txpacket), txpacket)
+
+    def RegAction(self):
+        return self.action(BROADCAST_ID)
+
+    def PWMMode(self, scs_id):
+        txpacket = [0, 0, 0, 0]
+        return self.writeTxRx(scs_id, SCSCL_MIN_ANGLE_LIMIT_L, len(txpacket), txpacket)
+
+    def WritePWM(self, scs_id, time):
+        return self.write2ByteTxRx(scs_id, SCSCL_GOAL_TIME_L, self.scs_toscs(time, 10))
+
+    def LockEprom(self, scs_id):
+        return self.write1ByteTxRx(scs_id, SCSCL_LOCK, 1)
+
+    def unLockEprom(self, scs_id):
+        return self.write1ByteTxRx(scs_id, SCSCL_LOCK, 0)

toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py

@@ -0,0 +1,7 @@
+from setuptools import setup, find_packages
+
+setup(
+    name='STservo_sdk',
+    version='0.1',
+    packages=find_packages(),
+)

toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python
+
+from .stservo_def import *
+from .protocol_packet_handler import *
+from .group_sync_read import *
+from .group_sync_write import *
+
+#波特率定义
+STS_1M = 0
+STS_0_5M = 1
+STS_250K = 2
+STS_128K = 3
+STS_115200 = 4
+STS_76800 = 5
+STS_57600 = 6
+STS_38400 = 7
+
+#内存表定义
+#-------EPROM(只读)--------
+STS_MODEL_L = 3
+STS_MODEL_H = 4
+
+#-------EPROM(读写)--------
+STS_ID = 5
+STS_BAUD_RATE = 6
+STS_MIN_ANGLE_LIMIT_L = 9
+STS_MIN_ANGLE_LIMIT_H = 10
+STS_MAX_ANGLE_LIMIT_L = 11
+STS_MAX_ANGLE_LIMIT_H = 12
+STS_CW_DEAD = 26
+STS_CCW_DEAD = 27
+STS_OFS_L = 31
+STS_OFS_H = 32
+STS_MODE = 33
+
+#-------SRAM(读写)--------
+MAX_POSITION_L = 42
+STS_GOAL_POSITION_H = 43
+STS_GOAL_TIME_L = 44
+STS_GOAL_TIME_H = 45
+STS_LOAD_LIMIT = 36
+STS_TORQUE_ENABLE = 40
+STS_ACC = 41
+STS_GOAL_SPEED_L = 46
+STS_GOAL_SPEED_H = 47
+STS_MAX_LOAD = 0x30
+STS_LOCK = 55
+
+#-------SRAM(只读)--------
+STS_PRESENT_POSITION_L = 56
+STS_PRESENT_POSITION_H = 57
+STS_PRESENT_SPEED_L = 58
+STS_PRESENT_SPEED_H = 59
+STS_PRESENT_LOAD_L = 60
+STS_PRESENT_LOAD_H = 61
+STS_PRESENT_VOLTAGE = 62
+STS_PRESENT_TEMPERATURE = 63
+STS_MOVING = 66
+STS_PRESENT_CURRENT_L = 69
+STS_PRESENT_CURRENT_H = 70
+
+class sts(protocol_packet_handler):
+    def __init__(self, portHandler):
+        protocol_packet_handler.__init__(self, portHandler, 0)
+        self.groupSyncWrite = GroupSyncWrite(self, 40, 7)
+
+    def WritePosEx(self, sts_id, position, speed, acc):
+        txpacket = [acc, self.sts_lobyte(position), self.sts_hibyte(position), 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)]
+        return self.writeTxRx(sts_id, STS_ACC, len(txpacket), txpacket)
+
+    def ReadPos(self, sts_id):
+        sts_present_position, sts_comm_result, sts_error = self.read2ByteTxRx(sts_id, 60)
+        return self.sts_tohost(sts_present_position, 15), sts_comm_result, sts_error
+    
+    def ReadLoad(self, sts_id):
+         sts_present_load, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_PRESENT_LOAD_L)  # 60
+         return self.sts_tohost(sts_present_load, 15), sts_comm_result, sts_error
+
+    def ReadMaxLoad(self, sts_id):
+         sts_max_load, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_LOAD_LIMIT)  # 0x36
+         return sts_max_load, sts_comm_result, sts_error
+
+    def ReadSpeed(self, sts_id):
+        sts_present_speed, sts_comm_result, sts_error = self.read2ByteTxRx(sts_id, STS_PRESENT_SPEED_L)
+        return self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error
+
+    def ReadPosSpeed(self, sts_id):
+        sts_present_position_speed, sts_comm_result, sts_error = self.read4ByteTxRx(sts_id, STS_PRESENT_POSITION_L)
+        sts_present_position = self.sts_loword(sts_present_position_speed)
+        sts_present_speed = self.sts_hiword(sts_present_position_speed)
+        return self.sts_tohost(sts_present_position, 15), self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error
+
+    def ReadPosLoad(self, sts_id):
+        sts_present_position_speed, sts_comm_result, sts_error = self.read4ByteTxRx(sts_id, 60)
+        sts_present_position = self.sts_loword(sts_present_position_speed)
+        sts_present_speed = self.sts_hiword(sts_present_position_speed)
+        return self.sts_tohost(sts_present_position, 15), self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error
+    
+    def ReadPosLoad1(self, sts_id):
+        sts_present_position_speed, sts_comm_result, sts_error = self.read4ByteTxRx(sts_id, 61)
+        sts_present_position = self.sts_loword(sts_present_position_speed)
+        sts_present_speed = self.sts_hiword(sts_present_position_speed)
+        return self.sts_tohost(sts_present_position, 15), self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error
+
+    def ReadMoving(self, sts_id):
+        moving, sts_comm_result, sts_error = self.read1ByteTxRx(sts_id, STS_MOVING)
+        return moving, sts_comm_result, sts_error
+   
+    def MaxLoad(self, sts_id):
+        max_load, sts_comm_result, sts_error = self.read1ByteTxRx(sts_id, STS_LOAD_LIMIT)
+        return max_load, sts_comm_result, sts_error
+
+    def SyncWritePosEx(self, sts_id, position, speed, acc):
+        txpacket = [acc, self.sts_lobyte(position), self.sts_hibyte(position), 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)]
+        return self.groupSyncWrite.addParam(sts_id, txpacket)
+
+    def RegWritePosEx(self, sts_id, position, speed, acc):
+        txpacket = [acc, self.sts_lobyte(position), self.sts_hibyte(position), 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)]
+        return self.regWriteTxRx(sts_id, STS_ACC, len(txpacket), txpacket)
+
+    def RegAction(self):
+        return self.action(BROADCAST_ID)
+
+    def WheelMode(self, sts_id):
+        return self.write1ByteTxRx(sts_id, STS_MODE, 1)
+    
+    def SetMaxLoad(self, sts_id, load):
+        return self.write2ByteTxRx(sts_id, STS_MAX_LOAD, int(load*10))
+
+    def WriteSpec(self, sts_id, speed, acc):
+        speed = self.sts_toscs(speed, 15)
+        txpacket = [acc, 0, 0, 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)]
+        return self.writeTxRx(sts_id, STS_ACC, len(txpacket), txpacket)
+
+    def LockEprom(self, sts_id):
+        return self.write1ByteTxRx(sts_id, STS_LOCK, 1)
+
+    def unLockEprom(self, sts_id):
+        return self.write1ByteTxRx(sts_id, STS_LOCK, 0)
+

toid_interaction/toid_interaction/mechanism/STservo_sdk/stservo_def.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+
+BROADCAST_ID = 0xFE  # 254
+MAX_ID = 0xFC  # 252
+STS_END = 0
+
+# Instruction for STS Protocol
+INST_PING = 1
+INST_READ = 2
+INST_WRITE = 3
+INST_REG_WRITE = 4
+INST_ACTION = 5
+INST_SYNC_WRITE = 131  # 0x83
+INST_SYNC_READ = 130  # 0x82
+
+# Communication Result
+COMM_SUCCESS = 0  # tx or rx packet communication success
+COMM_PORT_BUSY = -1  # Port is busy (in use)
+COMM_TX_FAIL = -2  # Failed transmit instruction packet
+COMM_RX_FAIL = -3  # Failed get status packet
+COMM_TX_ERROR = -4  # Incorrect instruction packet
+COMM_RX_WAITING = -5  # Now recieving status packet
+COMM_RX_TIMEOUT = -6  # There is no status packet
+COMM_RX_CORRUPT = -7  # Incorrect status packet
+COMM_NOT_AVAILABLE = -9  #

toid_interaction/toid_interaction/mechanism/sekvenca_2026.py

@@ -0,0 +1,64 @@
+#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 *
+import time
+import serial
+import serial.tools.list_ports as list_ports
+import platform
+
+SERIAL_ID = '503359270A70619F'
+
+
+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")
+    
+    for i, char in enumerate(broj):
+        if char == '1':
+            okreni(i + 1)
+
+
+def main():
+
+       #sekvenca 1
+      zupcanik(1, -1000, 25)
+
+       #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)
+
+     
+
+ 
+
+    
+
+if __name__ == "__main__":
+    main()
+

toid_interaction/toid_interaction/mechanism/zidovi_load.py

@@ -0,0 +1,284 @@
+import sys
+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
+
+# 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)
+
+    # 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
+        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))
+
+    
+
+        # 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()
+
+
+    
+    
+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(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(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 [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
+    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)
+
+    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))
+
+    # Initialize variables for tracking movement
+    TrenutnaPos_ID3 = sts_present_position_ID3
+    ProslaPos_ID3 = sts_present_position_ID3
+    PredjeniPut_ID3 = 0
+
+    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))
+      
+        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:
+                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))
+
+    
+
+        # 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()
+
+

toid_interaction/toid_interaction/mechanism/zuocanik.py

@@ -0,0 +1,105 @@
+
+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()