diff --git a/config/chatgpt.yml b/config/chatgpt.yml index 607657e..0fe0f76 100644 --- a/config/chatgpt.yml +++ b/config/chatgpt.yml @@ -5,8 +5,10 @@ chatgpt: model: gpt-4o-mini persona: "You are a helpful assistant robot. You respond with short phrases where possible. Alternatively, you can respond with the following commands instead of text if you feel they are appropriate: - - animate:head_nod - - animate:head_shake" + - animate/head_nod + - animate/head_shake + - gpio/laser/on + - gpio/laser/off" dependencies: python: - openai diff --git a/config/servo_waveshare.yml b/config/servo_waveshare.yml new file mode 100644 index 0000000..f7eed89 --- /dev/null +++ b/config/servo_waveshare.yml @@ -0,0 +1,38 @@ +servos: + enabled: true + path: "modules.actuators.bus_servo.servo.Servo" # Include class name here + instances: + - name: "leg_r_hip" + model: 'ST3215' + id: 3 + range: [744, 3136] + start: 1269 + poses: + - stand: 824 + - sit: 1269 + - centre: 2048 + - name: "leg_r_knee" + model: 'ST3215' + id: 2 + range: [702, 3260] + start: 723 + poses: + - stand: 2111 + - sit: 723 + - centre: 2048 + # configure_on_boot: true + - name: "leg_r_ankle" + model: 'ST3215' + id: 1 + range: [0, 4095] + start: 2048 + - name: "neck_pan" + model: 'SC09' + id: 4 + range: [0, 1023] + start: 511 + - name: "neck_tilt" + model: 'SC09' + id: 5 + range: [0, 1023] + start: 511 \ No newline at end of file diff --git a/docs/BusServo.md b/docs/BusServo.md new file mode 100644 index 0000000..126a288 --- /dev/null +++ b/docs/BusServo.md @@ -0,0 +1,42 @@ +# BaseModule Documentation + +## Overview + +Serial bus servos allow for efficient communication and control of multiple servos over a single bus. The `Servo` class provides an interface to manage these servos, including setting positions, speeds, and handling configurations. + +## Configuration + +The `config/servo_waveshare.yml` file contains the configuration for each bus servo. The `instances` section defines the servos, their IDs, and their initial positions. The `poses` section within each instance defines various poses that can be used to set servo positions. + +## Getting Started + +To calibrate the servos, each must have their ID set individually. To achieve this, connect one servo to the driver board and run `modules/actuators/bus_servo/change_id.py`. This script will prompt you to enter the ID for the connected servo, which will then be saved permanently on the servo. + +Once this has been done for all servos, you can use the `Servo` class to control them by enabling it in the above yaml file. + +To calibrate the servo positions, set the flag `configure_on_boot` to `true` in the configuration file for each instance (servo). This will cause the servo to output it's current position in the debug log, which can then be copied into the start position, or range. Servos can be manually moved to any position to identify their range or certain poses. + +Once the start position and range are set, move the flag to the next servo and repeat the process until all servos are configured. + +Finally, remove the `configure_on_boot` flag from the configuration file and re-run the program to start using the servos with their configured positions. + +## Subsriptions + +The `Servo` class subscribes to the following topics: +`servo::mv` - to move the servo to a specific position relative to it's current position. +`servo::mvabs` - to move the servo to a specific position with absolute values. + +## Smooth initialization + +Because the `Servo` class gets the current position of the servo on initialization, there is no danger of a servo jumping from an unknown position to the start position. This is especially useful when the servos are powered on in a random position and is an advantage over hobby servos used in previous versions of the modular biped. + +## SC vs ST servos + +The `Servo` class supports both ST and SC series servos from Waveshare. The type of servo is determined by the `model` variable in the configuration file. The class will automatically use the appropriate SDK for the specified servo type. + +There are some limitations to the SC servos as they do not support continuous rotation and have a lower rotational range compared to the ST servos. + +## References +https://www.waveshare.com/wiki/ST3215_Servo +https://www.waveshare.com/wiki/SC09_Servo +https://www.waveshare.com/wiki/Bus_Servo_Adapter_(A) \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCServo_examples/ping.py b/modules/actuators/bus_servo/SCServo_examples/ping.py new file mode 100644 index 0000000..0b5ee9e --- /dev/null +++ b/modules/actuators/bus_servo/SCServo_examples/ping.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python +# +# ********* Ping Example ********* +# +# +# Available SCServo model on this example : All models using Protocol SCS +# This example is tested with a SCServo(STS/SMS/SCS), and an URT +# Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from scservo_sdk import * # Uses SCServo SDK library + +# Default setting +SCS_ID = 1 # SCServo ID : 1 +BAUDRATE = 115200 # Driver board default baudrate : 115200 +DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +protocol_end = 1 # SCServo bit end(STS/SMS=0, SCS=1) + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +packetHandler = PacketHandler(protocol_end) + +# Open port +if portHandler.openPort(): + print("Succeeded to open the port") +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + 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...") + getch() + quit() + +# Try to ping the SCServo +# Get SCServo model number +scs_model_number, scs_comm_result, scs_error = packetHandler.ping(portHandler, SCS_ID) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) +else: + print("[ID:%03d] ping Succeeded. SCServo model number : %d" % (SCS_ID, scs_model_number)) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCServo_examples/read_write.py b/modules/actuators/bus_servo/SCServo_examples/read_write.py new file mode 100644 index 0000000..02ab9d1 --- /dev/null +++ b/modules/actuators/bus_servo/SCServo_examples/read_write.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python +# +# ********* Gen Write Example ********* +# +# +# Available SCServo model on this example : All models using Protocol SCS +# This example is tested with a SCServo(STS/SMS/SCS), and an URT +# Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() + +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from scservo_sdk import * # Uses SCServo SDK library + +# Control table address +ADDR_SCS_TORQUE_ENABLE = 40 +ADDR_SCS_GOAL_ACC = 41 +ADDR_SCS_GOAL_POSITION = 42 +ADDR_SCS_GOAL_SPEED = 46 +ADDR_SCS_PRESENT_POSITION = 56 + +# Default setting +SCS_ID = 1 # SCServo ID : 1 +BAUDRATE = 115200 # Driver board default baudrate : 115200 +DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +SCS_MINIMUM_POSITION_VALUE = 100 # SCServo will rotate between this value +SCS_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the SCServo would not move when the position value is out of movable range. Check e-manual about the range of the SCServo you use.) +SCS_MOVING_STATUS_THRESHOLD = 20 # SCServo moving status threshold +SCS_MOVING_SPEED = 0 # SCServo moving speed +SCS_MOVING_ACC = 0 # SCServo moving acc +protocol_end = 1 # SCServo bit end(STS/SMS=0, SCS=1) + +index = 0 +scs_goal_position = [SCS_MINIMUM_POSITION_VALUE, SCS_MAXIMUM_POSITION_VALUE] # Goal position + + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +packetHandler = PacketHandler(protocol_end) + +# Open port +if portHandler.openPort(): + print("Succeeded to open the port") +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + 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...") + getch() + quit() + +# Write SCServo acc +scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_ACC, SCS_MOVING_ACC) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# Write SCServo speed +scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_SPEED, SCS_MOVING_SPEED) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Write SCServo goal position + scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_POSITION, scs_goal_position[index]) + if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) + elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + + while 1: + # Read SCServo present position + scs_present_position_speed, scs_comm_result, scs_error = packetHandler.read4ByteTxRx(portHandler, SCS_ID, ADDR_SCS_PRESENT_POSITION) + if scs_comm_result != COMM_SUCCESS: + print(packetHandler.getTxRxResult(scs_comm_result)) + elif scs_error != 0: + print(packetHandler.getRxPacketError(scs_error)) + + scs_present_position = SCS_LOWORD(scs_present_position_speed) + scs_present_speed = SCS_HIWORD(scs_present_position_speed) + print("[ID:%03d] GoalPos:%03d PresPos:%03d PresSpd:%03d" + % (SCS_ID, scs_goal_position[index], scs_present_position, SCS_TOHOST(scs_present_speed, 15))) + + if not (abs(scs_goal_position[index] - scs_present_position_speed) > SCS_MOVING_STATUS_THRESHOLD): + break + + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS_ID, ADDR_SCS_TORQUE_ENABLE, 0) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/__init__.py b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/__init__.py new file mode 100644 index 0000000..3316e49 --- /dev/null +++ b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/__init__.py @@ -0,0 +1,6 @@ +#!/usr/bin/env python + +from .port_handler import * +from .packet_handler import * +from .group_sync_read import * +from .group_sync_write import * \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/group_sync_read.py b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/group_sync_read.py new file mode 100644 index 0000000..514f9d0 --- /dev/null +++ b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/group_sync_read.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python + +from .scservo_def import * + +class GroupSyncRead: + def __init__(self, port, ph, start_address, data_length): + self.port = port + 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, scs_id): + if scs_id in self.data_dict: # scs_id already exist + return False + + self.data_dict[scs_id] = [] # [0] * self.data_length + + self.is_param_changed = True + return True + + def removeParam(self, scs_id): + if scs_id not in self.data_dict: # NOT exist + return + + del self.data_dict[scs_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.port, self.start_address, self.data_length, self.param, + len(self.data_dict.keys()) * 1) + + def rxPacket(self): + self.last_result = False + + result = COMM_RX_FAIL + + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + for scs_id in self.data_dict: + self.data_dict[scs_id], result, _ = self.ph.readRx(self.port, scs_id, self.data_length) + if result != COMM_SUCCESS: + return result + + if result == COMM_SUCCESS: + self.last_result = True + + return result + + def txRxPacket(self): + result = self.txPacket() + if result != COMM_SUCCESS: + return result + + return self.rxPacket() + + def isAvailable(self, scs_id, address, data_length): + #if self.last_result is False or scs_id not in self.data_dict: + if scs_id not in self.data_dict: + return False + + if (address < self.start_address) or (self.start_address + self.data_length - data_length < address): + return False + + if len(self.data_dict[scs_id]) self.data_length: # input data is longer than set + return False + + self.data_dict[scs_id] = data + + self.is_param_changed = True + return True + + def removeParam(self, scs_id): + if scs_id not in self.data_dict: # NOT exist + return + + del self.data_dict[scs_id] + + self.is_param_changed = True + + def changeParam(self, scs_id, data): + if scs_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[scs_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.port, self.start_address, self.data_length, self.param, + len(self.data_dict.keys()) * (1 + self.data_length)) \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/packet_handler.py b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/packet_handler.py new file mode 100644 index 0000000..edb3d10 --- /dev/null +++ b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/packet_handler.py @@ -0,0 +1,10 @@ +#!/usr/bin/env python + +from .scservo_def import * +from .protocol_packet_handler import * + + +def PacketHandler(protocol_end): + # FIXME: float or int-to-float comparison can generate weird behaviour + SCS_SETEND(protocol_end) + return protocol_packet_handler() \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/port_handler.py b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/port_handler.py new file mode 100644 index 0000000..79d34d0 --- /dev/null +++ b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/port_handler.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python + +import time +import serial +import sys +import platform + +LATENCY_TIMER = 16 +DEFAULT_BAUDRATE = 1000000 + + +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) + (LATENCY_TIMER * 2.0) + 2.0 + + 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 \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/protocol_packet_handler.py b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/protocol_packet_handler.py new file mode 100644 index 0000000..1f81da0 --- /dev/null +++ b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/protocol_packet_handler.py @@ -0,0 +1,464 @@ +#!/usr/bin/env python + +from .scservo_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 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 "[RxPacketError] Input voltage error!" + + if error & ERRBIT_ANGLE: + return "[RxPacketError] Angle sen error!" + + if error & ERRBIT_OVERHEAT: + return "[RxPacketError] Overheat error!" + + if error & ERRBIT_OVERELE: + return "[RxPacketError] OverEle error!" + + if error & ERRBIT_OVERLOAD: + return "[RxPacketError] Overload error!" + + return "" + + def txPacket(self, port, txpacket): + checksum = 0 + total_packet_length = txpacket[PKT_LENGTH] + 4 # 4: HEADER0 HEADER1 ID LENGTH + + if port.is_using: + return COMM_PORT_BUSY + port.is_using = True + + # check max packet length + if total_packet_length > TXPACKET_MAX_LEN: + port.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 + port.clearPort() + written_packet_length = port.writePort(txpacket) + if total_packet_length != written_packet_length: + port.is_using = False + return COMM_TX_FAIL + + return COMM_SUCCESS + + def rxPacket(self, port): + 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(port.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 port.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 port.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + + port.is_using = False + + #print "[RxPacket] %r" % rxpacket + + return rxpacket, result + + def txRxPacket(self, port, txpacket): + rxpacket = None + error = 0 + + # tx packet + result = self.txPacket(port, 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): + port.is_using = False + return rxpacket, result, error + + # set packet timeout + if txpacket[PKT_INSTRUCTION] == INST_READ: + port.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6) + else: + port.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + + # rx packet + while True: + rxpacket, result = self.rxPacket(port) + 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, port, scs_id): + model_number = 0 + error = 0 + + txpacket = [0] * 6 + + if scs_id >= BROADCAST_ID: + return model_number, COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = scs_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_PING + + rxpacket, result, error = self.txRxPacket(port, txpacket) + + if result == COMM_SUCCESS: + data_read, result, error = self.readTxRx(port, scs_id, 3, 2) # Address 3 : Model Number + if result == COMM_SUCCESS: + model_number = SCS_MAKEWORD(data_read[0], data_read[1]) + + return model_number, result, error + + def action(self, port, scs_id): + txpacket = [0] * 6 + + txpacket[PKT_ID] = scs_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_ACTION + + _, result, _ = self.txRxPacket(port, txpacket) + + return result + + def readTx(self, port, scs_id, address, length): + + txpacket = [0] * 8 + + if scs_id >= BROADCAST_ID: + return COMM_NOT_AVAILABLE + + txpacket[PKT_ID] = scs_id + txpacket[PKT_LENGTH] = 4 + txpacket[PKT_INSTRUCTION] = INST_READ + txpacket[PKT_PARAMETER0 + 0] = address + txpacket[PKT_PARAMETER0 + 1] = length + + result = self.txPacket(port, txpacket) + + # set packet timeout + if result == COMM_SUCCESS: + port.setPacketTimeout(length + 6) + + return result + + def readRx(self, port, scs_id, length): + result = COMM_TX_FAIL + error = 0 + + rxpacket = None + data = [] + + while True: + rxpacket, result = self.rxPacket(port) + + if result != COMM_SUCCESS or rxpacket[PKT_ID] == scs_id: + break + + if result == COMM_SUCCESS and rxpacket[PKT_ID] == scs_id: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) + + return data, result, error + + def readTxRx(self, port, scs_id, address, length): + txpacket = [0] * 8 + data = [] + + if scs_id >= BROADCAST_ID: + return data, COMM_NOT_AVAILABLE, 0 + + txpacket[PKT_ID] = scs_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(port, txpacket) + if result == COMM_SUCCESS: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) + + return data, result, error + + def read1ByteTx(self, port, scs_id, address): + return self.readTx(port, scs_id, address, 1) + + def read1ByteRx(self, port, scs_id): + data, result, error = self.readRx(port, scs_id, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read1ByteTxRx(self, port, scs_id, address): + data, result, error = self.readTxRx(port, scs_id, address, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTx(self, port, scs_id, address): + return self.readTx(port, scs_id, address, 2) + + def read2ByteRx(self, port, scs_id): + data, result, error = self.readRx(port, scs_id, 2) + data_read = SCS_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTxRx(self, port, scs_id, address): + data, result, error = self.readTxRx(port, scs_id, address, 2) + data_read = SCS_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTx(self, port, scs_id, address): + return self.readTx(port, scs_id, address, 4) + + def read4ByteRx(self, port, scs_id): + data, result, error = self.readRx(port, scs_id, 4) + data_read = SCS_MAKEDWORD(SCS_MAKEWORD(data[0], data[1]), + SCS_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTxRx(self, port, scs_id, address): + data, result, error = self.readTxRx(port, scs_id, address, 4) + data_read = SCS_MAKEDWORD(SCS_MAKEWORD(data[0], data[1]), + SCS_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def writeTxOnly(self, port, scs_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = scs_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(port, txpacket) + port.is_using = False + + return result + + def writeTxRx(self, port, scs_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = scs_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(port, txpacket) + + return result, error + + def write1ByteTxOnly(self, port, scs_id, address, data): + data_write = [data] + return self.writeTxOnly(port, scs_id, address, 1, data_write) + + def write1ByteTxRx(self, port, scs_id, address, data): + data_write = [data] + return self.writeTxRx(port, scs_id, address, 1, data_write) + + def write2ByteTxOnly(self, port, scs_id, address, data): + data_write = [SCS_LOBYTE(data), SCS_HIBYTE(data)] + return self.writeTxOnly(port, scs_id, address, 2, data_write) + + def write2ByteTxRx(self, port, scs_id, address, data): + data_write = [SCS_LOBYTE(data), SCS_HIBYTE(data)] + return self.writeTxRx(port, scs_id, address, 2, data_write) + + def write4ByteTxOnly(self, port, scs_id, address, data): + data_write = [SCS_LOBYTE(SCS_LOWORD(data)), + SCS_HIBYTE(SCS_LOWORD(data)), + SCS_LOBYTE(SCS_HIWORD(data)), + SCS_HIBYTE(SCS_HIWORD(data))] + return self.writeTxOnly(port, scs_id, address, 4, data_write) + + def write4ByteTxRx(self, port, scs_id, address, data): + data_write = [SCS_LOBYTE(SCS_LOWORD(data)), + SCS_HIBYTE(SCS_LOWORD(data)), + SCS_LOBYTE(SCS_HIWORD(data)), + SCS_HIBYTE(SCS_HIWORD(data))] + return self.writeTxRx(port, scs_id, address, 4, data_write) + + def regWriteTxOnly(self, port, scs_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = scs_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(port, txpacket) + port.is_using = False + + return result + + def regWriteTxRx(self, port, scs_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = scs_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(port, txpacket) + + return result, error + + def syncReadTx(self, port, 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] + + result = self.txPacket(port, txpacket) + if result == COMM_SUCCESS: + port.setPacketTimeout((6 + data_length) * param_length) + + return result + + + def syncWriteTxOnly(self, port, 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(port, txpacket) + + return result \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/scservo_def.py b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/scservo_def.py new file mode 100644 index 0000000..be38ce2 --- /dev/null +++ b/modules/actuators/bus_servo/SCServo_examples/scservo_sdk/scservo_def.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python + +BROADCAST_ID = 0xFE # 254 +MAX_ID = 0xFC # 252 +SCS_END = 0 + +# Instruction for DXL 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 # + + +# Macro for Control Table Value +def SCS_GETEND(): + global SCS_END + return SCS_END + +def SCS_SETEND(e): + global SCS_END + SCS_END = e + +def SCS_TOHOST(a, b): + if (a & (1<> 16) & 0xFFFF + + +def SCS_LOBYTE(w): + global SCS_END + if SCS_END==0: + return w & 0xFF + else: + return (w >> 8) & 0xFF + + +def SCS_HIBYTE(w): + global SCS_END + if SCS_END==0: + return (w >> 8) & 0xFF + else: + return w & 0xFF \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCServo_examples/sync_read_write.py b/modules/actuators/bus_servo/SCServo_examples/sync_read_write.py new file mode 100644 index 0000000..9a79227 --- /dev/null +++ b/modules/actuators/bus_servo/SCServo_examples/sync_read_write.py @@ -0,0 +1,216 @@ +#!/usr/bin/env python +# +# ********* Sync Read and Sync Write Example ********* +# +# +# Available SCServo model on this example : All models using Protocol SCS +# This example is tested with a SCServo(STS/SMS), and an URT +# Be sure that SCServo(STS/SMS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from scservo_sdk import * # Uses SCServo SDK library + +# Control table address +ADDR_SCS_TORQUE_ENABLE = 40 +ADDR_STS_GOAL_ACC = 41 +ADDR_STS_GOAL_POSITION = 42 +ADDR_STS_GOAL_SPEED = 46 +ADDR_STS_PRESENT_POSITION = 56 + +# Default setting +SCS1_ID = 1 # SCServo#1 ID : 1 +SCS2_ID = 2 # SCServo#1 ID : 2 +BAUDRATE = 115200 # Driver board default baudrate : 115200 +DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +SCS_MINIMUM_POSITION_VALUE = 100 # SCServo will rotate between this value +SCS_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the SCServo would not move when the position value is out of movable range. Check e-manual about the range of the SCServo you use.) +SCS_MOVING_STATUS_THRESHOLD = 20 # SCServo moving status threshold +SCS_MOVING_SPEED = 0 # SCServo moving speed +SCS_MOVING_ACC = 0 # SCServo moving acc +protocol_end = 1 # SCServo bit end(STS/SMS=0, SCS=1) + +index = 0 +scs_goal_position = [SCS_MINIMUM_POSITION_VALUE, SCS_MAXIMUM_POSITION_VALUE] # Goal position + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +packetHandler = PacketHandler(protocol_end) + +# Initialize GroupSyncWrite instance +groupSyncWrite = GroupSyncWrite(portHandler, packetHandler, ADDR_STS_GOAL_POSITION, 2) + +# Initialize GroupSyncRead instace for Present Position +groupSyncRead = GroupSyncRead(portHandler, packetHandler, ADDR_STS_PRESENT_POSITION, 4) + +# Open port +if portHandler.openPort(): + print("Succeeded to open the port") +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + 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...") + getch() + quit() + +# SCServo#1 acc +scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS1_ID, ADDR_STS_GOAL_ACC, SCS_MOVING_ACC) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# SCServo#2 acc +scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS2_ID, ADDR_STS_GOAL_ACC, SCS_MOVING_ACC) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# SCServo#1 speed +scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS1_ID, ADDR_STS_GOAL_SPEED, SCS_MOVING_SPEED) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# SCServo#2 speed +scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS2_ID, ADDR_STS_GOAL_SPEED, SCS_MOVING_SPEED) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# Add parameter storage for SCServo#1 present position value +scs_addparam_result = groupSyncRead.addParam(SCS1_ID) +if scs_addparam_result != True: + print("[ID:%03d] groupSyncRead addparam failed" % SCS1_ID) + quit() + +# Add parameter storage for SCServo#2 present position value +scs_addparam_result = groupSyncRead.addParam(SCS2_ID) +if scs_addparam_result != True: + print("[ID:%03d] groupSyncRead addparam failed" % SCS2_ID) + quit() + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Allocate goal position value into byte array + param_goal_position = [SCS_LOBYTE(scs_goal_position[index]), SCS_HIBYTE(scs_goal_position[index])] + + # Add SCServo#1 goal position value to the Syncwrite parameter storage + scs_addparam_result = groupSyncWrite.addParam(SCS1_ID, param_goal_position) + if scs_addparam_result != True: + print("[ID:%03d] groupSyncWrite addparam failed" % SCS1_ID) + quit() + + # Add SCServo#2 goal position value to the Syncwrite parameter storage + scs_addparam_result = groupSyncWrite.addParam(SCS2_ID, param_goal_position) + if scs_addparam_result != True: + print("[ID:%03d] groupSyncWrite addparam failed" % SCS2_ID) + quit() + + # Syncwrite goal position + scs_comm_result = groupSyncWrite.txPacket() + if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) + + # Clear syncwrite parameter storage + groupSyncWrite.clearParam() + + while 1: + # Syncread present position + scs_comm_result = groupSyncRead.txRxPacket() + if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) + + # Check if groupsyncread data of SCServo#1 is available + scs_getdata_result = groupSyncRead.isAvailable(SCS1_ID, ADDR_STS_PRESENT_POSITION, 4) + scs1_present_position_speed = 0 + scs2_present_position_speed = 0 + if scs_getdata_result == True: + # Get SCServo#1 present position value + scs1_present_position_speed = groupSyncRead.getData(SCS1_ID, ADDR_STS_PRESENT_POSITION, 4) + else: + print("[ID:%03d] groupSyncRead getdata failed" % SCS1_ID) + + # Check if groupsyncread data of SCServo#2 is available + scs_getdata_result = groupSyncRead.isAvailable(SCS2_ID, ADDR_STS_PRESENT_POSITION, 4) + if scs_getdata_result == True: + # Get SCServo#2 present position value + scs2_present_position_speed = groupSyncRead.getData(SCS2_ID, ADDR_STS_PRESENT_POSITION, 4) + else: + print("[ID:%03d] groupSyncRead getdata failed" % SCS2_ID) + + scs1_present_position = SCS_LOWORD(scs1_present_position_speed) + scs1_present_speed = SCS_HIWORD(scs1_present_position_speed) + scs2_present_position = SCS_LOWORD(scs2_present_position_speed) + scs2_present_speed = SCS_HIWORD(scs2_present_position_speed) + print("[ID:%03d] GoalPos:%03d PresPos:%03d PresSpd:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d PresSpd:%03d" + % (SCS1_ID, scs_goal_position[index], scs1_present_position, SCS_TOHOST(scs1_present_speed, 15), + SCS2_ID, scs_goal_position[index], scs2_present_position, SCS_TOHOST(scs2_present_speed, 15))) + + if not ((abs(scs_goal_position[index] - scs1_present_position_speed) > SCS_MOVING_STATUS_THRESHOLD) and (abs(scs_goal_position[index] - scs2_present_position_speed) > SCS_MOVING_STATUS_THRESHOLD)): + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +# Clear syncread parameter storage +groupSyncRead.clearParam() + +# SCServo#1 torque +scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS1_ID, ADDR_SCS_TORQUE_ENABLE, 0) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# SCServo#2 torque +scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS2_ID, ADDR_SCS_TORQUE_ENABLE, 0) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCServo_examples/sync_write.py b/modules/actuators/bus_servo/SCServo_examples/sync_write.py new file mode 100644 index 0000000..09c4a8b --- /dev/null +++ b/modules/actuators/bus_servo/SCServo_examples/sync_write.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python +# +# ********* Sync Write Example ********* +# +# +# Available SCServo model on this example : All models using Protocol SCS +# This example is tested with a SCServo(STS/SMS/SCS), and an URT +# Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from scservo_sdk import * # Uses SCServo SDK library + +# Control table address +ADDR_SCS_TORQUE_ENABLE = 40 +ADDR_STS_GOAL_ACC = 41 +ADDR_STS_GOAL_POSITION = 42 +ADDR_STS_GOAL_SPEED = 46 +ADDR_STS_PRESENT_POSITION = 56 + + +# Default setting +SCS1_ID = 1 # SCServo#1 ID : 1 +SCS2_ID = 2 # SCServo#1 ID : 2 +BAUDRATE = 115200 # Driver board default baudrate : 115200 +DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +SCS_MINIMUM_POSITION_VALUE = 100 # SCServo will rotate between this value +SCS_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the SCServo would not move when the position value is out of movable range. Check e-manual about the range of the SCServo you use.) +SCS_MOVING_STATUS_THRESHOLD = 20 # SCServo moving status threshold +SCS_MOVING_SPEED = 0 # SCServo moving speed +SCS_MOVING_ACC = 0 # SCServo moving acc +protocol_end = 1 # SCServo bit end(STS/SMS=0, SCS=1) + +index = 0 +scs_goal_position = [SCS_MINIMUM_POSITION_VALUE, SCS_MAXIMUM_POSITION_VALUE] # Goal position + + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +packetHandler = PacketHandler(protocol_end) + +# Initialize GroupSyncWrite instance +groupSyncWrite = GroupSyncWrite(portHandler, packetHandler, ADDR_STS_GOAL_POSITION, 2) + +# Open port +if portHandler.openPort(): + print("Succeeded to open the port") +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + 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...") + getch() + quit() + +# SCServo#1 acc +scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS1_ID, ADDR_STS_GOAL_ACC, SCS_MOVING_ACC) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# SCServo#2 acc +scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS2_ID, ADDR_STS_GOAL_ACC, SCS_MOVING_ACC) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# SCServo#1 speed +scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS1_ID, ADDR_STS_GOAL_SPEED, SCS_MOVING_SPEED) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# SCServo#2 speed +scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS2_ID, ADDR_STS_GOAL_SPEED, SCS_MOVING_SPEED) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Allocate goal position value into byte array + param_goal_position = [SCS_LOBYTE(scs_goal_position[index]), SCS_HIBYTE(scs_goal_position[index])] + + # Add SCServo#1 goal position value to the Syncwrite parameter storage + scs_addparam_result = groupSyncWrite.addParam(SCS1_ID, param_goal_position) + if scs_addparam_result != True: + print("[ID:%03d] groupSyncWrite addparam failed" % SCS1_ID) + quit() + + # Add SCServo#2 goal position value to the Syncwrite parameter storage + scs_addparam_result = groupSyncWrite.addParam(SCS2_ID, param_goal_position) + if scs_addparam_result != True: + print("[ID:%03d] groupSyncWrite addparam failed" % SCS2_ID) + quit() + + # Syncwrite goal position + scs_comm_result = groupSyncWrite.txPacket() + if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) + + # Clear syncwrite parameter storage + groupSyncWrite.clearParam() + + while 1: + # Read SCServo#1 present position + scs1_present_position_speed, scs_comm_result, scs_error = packetHandler.read4ByteTxRx(portHandler, SCS1_ID, ADDR_STS_PRESENT_POSITION) + if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) + elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + + # Read SCServo#2 present position + scs2_present_position_speed, scs_comm_result, scs_error = packetHandler.read4ByteTxRx(portHandler, SCS2_ID, ADDR_STS_PRESENT_POSITION) + if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) + elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + + scs1_present_position = SCS_LOWORD(scs1_present_position_speed) + scs1_present_speed = SCS_HIWORD(scs1_present_position_speed) + scs2_present_position = SCS_LOWORD(scs2_present_position_speed) + scs2_present_speed = SCS_HIWORD(scs2_present_position_speed) + print("[ID:%03d] GoalPos:%03d PresPos:%03d PresSpd:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d PresSpd:%03d" + % (SCS1_ID, scs_goal_position[index], scs1_present_position, SCS_TOHOST(scs1_present_speed, 15), + SCS2_ID, scs_goal_position[index], scs2_present_position, SCS_TOHOST(scs2_present_speed, 15))) + + if not ((abs(scs_goal_position[index] - scs1_present_position) > SCS_MOVING_STATUS_THRESHOLD) and (abs(scs_goal_position[index] - scs2_present_position) > SCS_MOVING_STATUS_THRESHOLD)): + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +# SCServo#1 torque +scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS1_ID, ADDR_SCS_TORQUE_ENABLE, 0) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# SCServo#2 torque +scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS2_ID, ADDR_SCS_TORQUE_ENABLE, 0) +if scs_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(scs_comm_result)) +elif scs_error != 0: + print("%s" % packetHandler.getRxPacketError(scs_error)) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCservo_sdk/__init__.py b/modules/actuators/bus_servo/SCservo_sdk/__init__.py new file mode 100644 index 0000000..3316e49 --- /dev/null +++ b/modules/actuators/bus_servo/SCservo_sdk/__init__.py @@ -0,0 +1,6 @@ +#!/usr/bin/env python + +from .port_handler import * +from .packet_handler import * +from .group_sync_read import * +from .group_sync_write import * \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCservo_sdk/group_sync_read.py b/modules/actuators/bus_servo/SCservo_sdk/group_sync_read.py new file mode 100644 index 0000000..514f9d0 --- /dev/null +++ b/modules/actuators/bus_servo/SCservo_sdk/group_sync_read.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python + +from .scservo_def import * + +class GroupSyncRead: + def __init__(self, port, ph, start_address, data_length): + self.port = port + 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, scs_id): + if scs_id in self.data_dict: # scs_id already exist + return False + + self.data_dict[scs_id] = [] # [0] * self.data_length + + self.is_param_changed = True + return True + + def removeParam(self, scs_id): + if scs_id not in self.data_dict: # NOT exist + return + + del self.data_dict[scs_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.port, self.start_address, self.data_length, self.param, + len(self.data_dict.keys()) * 1) + + def rxPacket(self): + self.last_result = False + + result = COMM_RX_FAIL + + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + for scs_id in self.data_dict: + self.data_dict[scs_id], result, _ = self.ph.readRx(self.port, scs_id, self.data_length) + if result != COMM_SUCCESS: + return result + + if result == COMM_SUCCESS: + self.last_result = True + + return result + + def txRxPacket(self): + result = self.txPacket() + if result != COMM_SUCCESS: + return result + + return self.rxPacket() + + def isAvailable(self, scs_id, address, data_length): + #if self.last_result is False or scs_id not in self.data_dict: + if scs_id not in self.data_dict: + return False + + if (address < self.start_address) or (self.start_address + self.data_length - data_length < address): + return False + + if len(self.data_dict[scs_id]) self.data_length: # input data is longer than set + return False + + self.data_dict[scs_id] = data + + self.is_param_changed = True + return True + + def removeParam(self, scs_id): + if scs_id not in self.data_dict: # NOT exist + return + + del self.data_dict[scs_id] + + self.is_param_changed = True + + def changeParam(self, scs_id, data): + if scs_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[scs_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.port, self.start_address, self.data_length, self.param, + len(self.data_dict.keys()) * (1 + self.data_length)) \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCservo_sdk/packet_handler.py b/modules/actuators/bus_servo/SCservo_sdk/packet_handler.py new file mode 100644 index 0000000..edb3d10 --- /dev/null +++ b/modules/actuators/bus_servo/SCservo_sdk/packet_handler.py @@ -0,0 +1,10 @@ +#!/usr/bin/env python + +from .scservo_def import * +from .protocol_packet_handler import * + + +def PacketHandler(protocol_end): + # FIXME: float or int-to-float comparison can generate weird behaviour + SCS_SETEND(protocol_end) + return protocol_packet_handler() \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCservo_sdk/port_handler.py b/modules/actuators/bus_servo/SCservo_sdk/port_handler.py new file mode 100644 index 0000000..79d34d0 --- /dev/null +++ b/modules/actuators/bus_servo/SCservo_sdk/port_handler.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python + +import time +import serial +import sys +import platform + +LATENCY_TIMER = 16 +DEFAULT_BAUDRATE = 1000000 + + +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) + (LATENCY_TIMER * 2.0) + 2.0 + + 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 \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCservo_sdk/protocol_packet_handler.py b/modules/actuators/bus_servo/SCservo_sdk/protocol_packet_handler.py new file mode 100644 index 0000000..1f81da0 --- /dev/null +++ b/modules/actuators/bus_servo/SCservo_sdk/protocol_packet_handler.py @@ -0,0 +1,464 @@ +#!/usr/bin/env python + +from .scservo_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 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 "[RxPacketError] Input voltage error!" + + if error & ERRBIT_ANGLE: + return "[RxPacketError] Angle sen error!" + + if error & ERRBIT_OVERHEAT: + return "[RxPacketError] Overheat error!" + + if error & ERRBIT_OVERELE: + return "[RxPacketError] OverEle error!" + + if error & ERRBIT_OVERLOAD: + return "[RxPacketError] Overload error!" + + return "" + + def txPacket(self, port, txpacket): + checksum = 0 + total_packet_length = txpacket[PKT_LENGTH] + 4 # 4: HEADER0 HEADER1 ID LENGTH + + if port.is_using: + return COMM_PORT_BUSY + port.is_using = True + + # check max packet length + if total_packet_length > TXPACKET_MAX_LEN: + port.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 + port.clearPort() + written_packet_length = port.writePort(txpacket) + if total_packet_length != written_packet_length: + port.is_using = False + return COMM_TX_FAIL + + return COMM_SUCCESS + + def rxPacket(self, port): + 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(port.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 port.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 port.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + + port.is_using = False + + #print "[RxPacket] %r" % rxpacket + + return rxpacket, result + + def txRxPacket(self, port, txpacket): + rxpacket = None + error = 0 + + # tx packet + result = self.txPacket(port, 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): + port.is_using = False + return rxpacket, result, error + + # set packet timeout + if txpacket[PKT_INSTRUCTION] == INST_READ: + port.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6) + else: + port.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + + # rx packet + while True: + rxpacket, result = self.rxPacket(port) + 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, port, scs_id): + model_number = 0 + error = 0 + + txpacket = [0] * 6 + + if scs_id >= BROADCAST_ID: + return model_number, COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = scs_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_PING + + rxpacket, result, error = self.txRxPacket(port, txpacket) + + if result == COMM_SUCCESS: + data_read, result, error = self.readTxRx(port, scs_id, 3, 2) # Address 3 : Model Number + if result == COMM_SUCCESS: + model_number = SCS_MAKEWORD(data_read[0], data_read[1]) + + return model_number, result, error + + def action(self, port, scs_id): + txpacket = [0] * 6 + + txpacket[PKT_ID] = scs_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_ACTION + + _, result, _ = self.txRxPacket(port, txpacket) + + return result + + def readTx(self, port, scs_id, address, length): + + txpacket = [0] * 8 + + if scs_id >= BROADCAST_ID: + return COMM_NOT_AVAILABLE + + txpacket[PKT_ID] = scs_id + txpacket[PKT_LENGTH] = 4 + txpacket[PKT_INSTRUCTION] = INST_READ + txpacket[PKT_PARAMETER0 + 0] = address + txpacket[PKT_PARAMETER0 + 1] = length + + result = self.txPacket(port, txpacket) + + # set packet timeout + if result == COMM_SUCCESS: + port.setPacketTimeout(length + 6) + + return result + + def readRx(self, port, scs_id, length): + result = COMM_TX_FAIL + error = 0 + + rxpacket = None + data = [] + + while True: + rxpacket, result = self.rxPacket(port) + + if result != COMM_SUCCESS or rxpacket[PKT_ID] == scs_id: + break + + if result == COMM_SUCCESS and rxpacket[PKT_ID] == scs_id: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) + + return data, result, error + + def readTxRx(self, port, scs_id, address, length): + txpacket = [0] * 8 + data = [] + + if scs_id >= BROADCAST_ID: + return data, COMM_NOT_AVAILABLE, 0 + + txpacket[PKT_ID] = scs_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(port, txpacket) + if result == COMM_SUCCESS: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) + + return data, result, error + + def read1ByteTx(self, port, scs_id, address): + return self.readTx(port, scs_id, address, 1) + + def read1ByteRx(self, port, scs_id): + data, result, error = self.readRx(port, scs_id, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read1ByteTxRx(self, port, scs_id, address): + data, result, error = self.readTxRx(port, scs_id, address, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTx(self, port, scs_id, address): + return self.readTx(port, scs_id, address, 2) + + def read2ByteRx(self, port, scs_id): + data, result, error = self.readRx(port, scs_id, 2) + data_read = SCS_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTxRx(self, port, scs_id, address): + data, result, error = self.readTxRx(port, scs_id, address, 2) + data_read = SCS_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTx(self, port, scs_id, address): + return self.readTx(port, scs_id, address, 4) + + def read4ByteRx(self, port, scs_id): + data, result, error = self.readRx(port, scs_id, 4) + data_read = SCS_MAKEDWORD(SCS_MAKEWORD(data[0], data[1]), + SCS_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTxRx(self, port, scs_id, address): + data, result, error = self.readTxRx(port, scs_id, address, 4) + data_read = SCS_MAKEDWORD(SCS_MAKEWORD(data[0], data[1]), + SCS_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def writeTxOnly(self, port, scs_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = scs_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(port, txpacket) + port.is_using = False + + return result + + def writeTxRx(self, port, scs_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = scs_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(port, txpacket) + + return result, error + + def write1ByteTxOnly(self, port, scs_id, address, data): + data_write = [data] + return self.writeTxOnly(port, scs_id, address, 1, data_write) + + def write1ByteTxRx(self, port, scs_id, address, data): + data_write = [data] + return self.writeTxRx(port, scs_id, address, 1, data_write) + + def write2ByteTxOnly(self, port, scs_id, address, data): + data_write = [SCS_LOBYTE(data), SCS_HIBYTE(data)] + return self.writeTxOnly(port, scs_id, address, 2, data_write) + + def write2ByteTxRx(self, port, scs_id, address, data): + data_write = [SCS_LOBYTE(data), SCS_HIBYTE(data)] + return self.writeTxRx(port, scs_id, address, 2, data_write) + + def write4ByteTxOnly(self, port, scs_id, address, data): + data_write = [SCS_LOBYTE(SCS_LOWORD(data)), + SCS_HIBYTE(SCS_LOWORD(data)), + SCS_LOBYTE(SCS_HIWORD(data)), + SCS_HIBYTE(SCS_HIWORD(data))] + return self.writeTxOnly(port, scs_id, address, 4, data_write) + + def write4ByteTxRx(self, port, scs_id, address, data): + data_write = [SCS_LOBYTE(SCS_LOWORD(data)), + SCS_HIBYTE(SCS_LOWORD(data)), + SCS_LOBYTE(SCS_HIWORD(data)), + SCS_HIBYTE(SCS_HIWORD(data))] + return self.writeTxRx(port, scs_id, address, 4, data_write) + + def regWriteTxOnly(self, port, scs_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = scs_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(port, txpacket) + port.is_using = False + + return result + + def regWriteTxRx(self, port, scs_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = scs_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(port, txpacket) + + return result, error + + def syncReadTx(self, port, 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] + + result = self.txPacket(port, txpacket) + if result == COMM_SUCCESS: + port.setPacketTimeout((6 + data_length) * param_length) + + return result + + + def syncWriteTxOnly(self, port, 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(port, txpacket) + + return result \ No newline at end of file diff --git a/modules/actuators/bus_servo/SCservo_sdk/scservo_def.py b/modules/actuators/bus_servo/SCservo_sdk/scservo_def.py new file mode 100644 index 0000000..be38ce2 --- /dev/null +++ b/modules/actuators/bus_servo/SCservo_sdk/scservo_def.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python + +BROADCAST_ID = 0xFE # 254 +MAX_ID = 0xFC # 252 +SCS_END = 0 + +# Instruction for DXL 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 # + + +# Macro for Control Table Value +def SCS_GETEND(): + global SCS_END + return SCS_END + +def SCS_SETEND(e): + global SCS_END + SCS_END = e + +def SCS_TOHOST(a, b): + if (a & (1<> 16) & 0xFFFF + + +def SCS_LOBYTE(w): + global SCS_END + if SCS_END==0: + return w & 0xFF + else: + return (w >> 8) & 0xFF + + +def SCS_HIBYTE(w): + global SCS_END + if SCS_END==0: + return (w >> 8) & 0xFF + else: + return w & 0xFF \ No newline at end of file diff --git a/modules/actuators/bus_servo/STServo_examples/change_id.py b/modules/actuators/bus_servo/STServo_examples/change_id.py new file mode 100644 index 0000000..52f443e --- /dev/null +++ b/modules/actuators/bus_servo/STServo_examples/change_id.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python +# +# ********* Change Servo ID Example ********* +# +# Works for both STS (ST3215) and SCSCL (SC09) servos. +# Requires each servo to be connected individually. +# RUN THIS BEFORE USING THE SERVOS IN ANY OTHER EXAMPLE! +# + +import sys +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * + +# User config +SERVO_TYPE = input("Enter servo type (STS for ST3215, SCSCL for SC09): ").strip().upper() +SERVO_ID = int(input("Enter current servo ID (1-253): ")) +BAUDRATE = 1000000 +DEVICENAME = '/dev/ttyACM0' # Change as needed + +# Initialize PortHandler +portHandler = PortHandler(DEVICENAME) + +# Select correct packet handler +if SERVO_TYPE == "STS": + packetHandler = sts(portHandler) + ID_ADDR = 5 +elif SERVO_TYPE == "SCSCL": + packetHandler = scscl(portHandler) + ID_ADDR = 5 +else: + print("Unknown servo type.") + sys.exit(1) + +# Open port +if portHandler.openPort(): + print("Succeeded to open the port") +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + sys.exit(1) + +# Set baudrate +if portHandler.setBaudRate(BAUDRATE): + print("Succeeded to change the baudrate") +else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + getch() + portHandler.closePort() + sys.exit(1) + +while True: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Read present position/speed (optional, for feedback) + try: + if SERVO_TYPE == "STS": + pos, spd, comm_result, err = packetHandler.ReadPosSpeed(SERVO_ID) + else: + pos, spd, comm_result, err = packetHandler.ReadPosSpeed(SERVO_ID) + if comm_result == COMM_SUCCESS: + print(f"[ID:{SERVO_ID:03d}] PresPos:{pos} PresSpd:{spd}") + else: + print(packetHandler.getTxRxResult(comm_result)) + if err != 0: + print(packetHandler.getRxPacketError(err)) + except Exception as e: + print(f"Could not read servo: {e}") + + # Get new ID + try: + new_id = int(input("Enter new ID (1-253): ")) + except Exception: + print("Invalid input.") + continue + if new_id < 1 or new_id > 253: + print("Invalid ID. Please enter a value between 1 and 253.") + continue + + # Unlock EEPROM + unlock_result, unlock_error = packetHandler.unLockEprom(SERVO_ID) + if unlock_result != COMM_SUCCESS or unlock_error != 0: + print("Failed to unlock EEPROM: %s %s" % ( + packetHandler.getTxRxResult(unlock_result), + packetHandler.getRxPacketError(unlock_error) + )) + continue + + # Write new ID + write_result, write_error = packetHandler.write1ByteTxRx(SERVO_ID, ID_ADDR, new_id) + if write_result != COMM_SUCCESS or write_error != 0: + print("Failed to change ID: %s %s" % ( + packetHandler.getTxRxResult(write_result), + packetHandler.getRxPacketError(write_error) + )) + continue + + # Lock EEPROM + lock_result, lock_error = packetHandler.LockEprom(new_id) + if lock_result != COMM_SUCCESS or lock_error != 0: + print("Failed to lock EEPROM: %s %s" % ( + packetHandler.getTxRxResult(lock_result), + packetHandler.getRxPacketError(lock_error) + )) + continue + + print(f"{SERVO_TYPE} servo ID changed successfully to {new_id}") + SERVO_ID = new_id + +portHandler.closePort() diff --git a/modules/actuators/bus_servo/STServo_examples/ping.py b/modules/actuators/bus_servo/STServo_examples/ping.py new file mode 100644 index 0000000..c3f2dc4 --- /dev/null +++ b/modules/actuators/bus_servo/STServo_examples/ping.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python +# +# ********* Ping Example ********* +# +# +# Available STServo model on this example : All models using Protocol STS +# This example is tested with a STServo and an URT +# + +import sys +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * # Uses STServo SDK library + +# Default setting +STS_ID = 1 # STServo ID : 1 +BAUDRATE = 1000000 # STServo default baudrate : 1000000 +DEVICENAME = '/dev/ttyACM0' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +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...") + getch() + 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...") + getch() + quit() + +# Try to ping the STServo +# Get STServo model number +sts_model_number, sts_comm_result, sts_error = packetHandler.ping(STS_ID) +if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) +else: + print("[ID:%03d] ping Succeeded. STServo model number : %d" % (STS_ID, sts_model_number)) +if sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + +# Close port +portHandler.closePort() diff --git a/modules/actuators/bus_servo/STServo_examples/read.py b/modules/actuators/bus_servo/STServo_examples/read.py new file mode 100644 index 0000000..744caa2 --- /dev/null +++ b/modules/actuators/bus_servo/STServo_examples/read.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python +# +# ********* Gen Write Example ********* +# +# +# Available STServo model on this example : All models using Protocol STS +# This example is tested with a STServo and an URT +# + +import sys +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() + +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * # Uses STServo SDK library + +# Default setting +STS_ID = 1 # STServo ID : 1 +BAUDRATE = 1000000 # STServo default baudrate : 1000000 +DEVICENAME = '/dev/ttyACM0' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +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...") + getch() + 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...") + getch() + quit() + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + # 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)) + +# Close port +portHandler.closePort() diff --git a/modules/actuators/bus_servo/STServo_examples/read_all.py b/modules/actuators/bus_servo/STServo_examples/read_all.py new file mode 100644 index 0000000..fbac511 --- /dev/null +++ b/modules/actuators/bus_servo/STServo_examples/read_all.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python +# +# ********* Gen Write Example ********* +# +# +# Available STServo model on this example : All models using Protocol STS +# This example is tested with a STServo and an URT +# + +import os +import sys + + + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() + +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * + +# Default setting +STS_ID = 1 # STServo ID : 1 +BAUDRATE = 1000000 # STServo default baudrate : 1000000 +DEVICENAME = '/dev/ttyACM0' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" +SERVO_CNT = 20 # Number of servos to read + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +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...") + getch() + 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...") + getch() + quit() + +# Instead of a single ID, define a range to scan +STS_ID_RANGE = range(1, SERVO_CNT) # Typical servo ID range (1-SERVO_CNT) + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + print("Scanning for %d servos..." % SERVO_CNT) + for STS_ID in STS_ID_RANGE: + sts_present_position, sts_present_speed, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(STS_ID) + if sts_comm_result == COMM_SUCCESS: + print(f"[ID:{STS_ID:03d}] PresPos:{sts_present_position} PresSpd:{sts_present_speed}") + if sts_error != 0: + print(packetHandler.getRxPacketError(sts_error)) + print("Scan complete.") + +# Close port +portHandler.closePort() diff --git a/modules/actuators/bus_servo/STServo_examples/read_write.py b/modules/actuators/bus_servo/STServo_examples/read_write.py new file mode 100644 index 0000000..6082510 --- /dev/null +++ b/modules/actuators/bus_servo/STServo_examples/read_write.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python +# +# ********* Gen Write Example ********* +# +# +# Available STServo model on this example : All models using Protocol STS +# This example is tested with a STServo and an URT +# + +import sys +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() + +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * # Uses STServo SDK library + +# Default setting +STS_ID = 1 # STServo ID : 1 +BAUDRATE = 1000000 # STServo default baudrate : 1000000 +DEVICENAME = 'COM11' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" +STS_MINIMUM_POSITION_VALUE = 0 # STServo will rotate between this value +STS_MAXIMUM_POSITION_VALUE = 4095 +STS_MOVING_SPEED = 2400 # STServo moving speed +STS_MOVING_ACC = 50 # STServo moving acc + +index = 0 +sts_goal_position = [STS_MINIMUM_POSITION_VALUE, STS_MAXIMUM_POSITION_VALUE] # Goal position + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +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...") + getch() + 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...") + getch() + quit() + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Write STServo goal position/moving speed/moving acc + sts_comm_result, sts_error = packetHandler.WritePosEx(STS_ID, sts_goal_position[index], STS_MOVING_SPEED, STS_MOVING_ACC) + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + elif sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + + while 1: + # 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] GoalPos:%d PresPos:%d PresSpd:%d" % (STS_ID, sts_goal_position[index], sts_present_position, sts_present_speed)) + if sts_error != 0: + print(packetHandler.getRxPacketError(sts_error)) + + # Read STServo moving status + moving, sts_comm_result, sts_error = packetHandler.ReadMoving(STS_ID) + if sts_comm_result != COMM_SUCCESS: + print(packetHandler.getTxRxResult(sts_comm_result)) + + if moving==0: + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +# Close port +portHandler.closePort() diff --git a/modules/actuators/bus_servo/STServo_examples/reg_write.py b/modules/actuators/bus_servo/STServo_examples/reg_write.py new file mode 100644 index 0000000..48c3a85 --- /dev/null +++ b/modules/actuators/bus_servo/STServo_examples/reg_write.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python +# +# ********* Gen Write Example ********* +# +# +# Available STServo model on this example : All models using Protocol STS +# This example is tested with a STServo and an URT +# + +import sys +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() + +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * # Uses STServo SDK library + +# Default setting +BAUDRATE = 1000000 # STServo default baudrate : 1000000 +DEVICENAME = 'COM11' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" +STS_MINIMUM_POSITION_VALUE = 0 # STServo will rotate between this value +STS_MAXIMUM_POSITION_VALUE = 4095 +STS_MOVING_SPEED = 2400 # STServo moving speed +STS_MOVING_ACC = 50 # STServo moving acc + +index = 0 +sts_goal_position = [STS_MINIMUM_POSITION_VALUE, STS_MAXIMUM_POSITION_VALUE] # Goal position + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +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...") + getch() + 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...") + getch() + quit() + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Write STServo goal position/moving speed/moving acc + for sts_id in range(1, 11): + sts_comm_result, sts_error = packetHandler.RegWritePosEx(sts_id, sts_goal_position[index], 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)) + packetHandler.RegAction() + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +# Close port +portHandler.closePort() diff --git a/modules/actuators/bus_servo/STServo_examples/sync_read.py b/modules/actuators/bus_servo/STServo_examples/sync_read.py new file mode 100644 index 0000000..2ff4b4c --- /dev/null +++ b/modules/actuators/bus_servo/STServo_examples/sync_read.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python +# +# ********* Sync Read Example ********* +# +# +# Available STServo model on this example : All models using Protocol STS +# This example is tested with a STServo and an URT +# + +import sys +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * # Uses STServo SDK library + +# Default setting +BAUDRATE = 1000000 # SCServo default baudrate : 1000000 +DEVICENAME = 'COM11' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +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...") + getch() + 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...") + getch() + quit() + +groupSyncRead = GroupSyncRead(packetHandler, STS_PRESENT_POSITION_L, 4) + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + for sts_id in range(1, 11): + # Add parameter storage for STServo#1~10 present position value + sts_addparam_result = groupSyncRead.addParam(sts_id) + if sts_addparam_result != True: + print("[ID:%03d] groupSyncRead addparam failed" % sts_id) + + sts_comm_result = groupSyncRead.txRxPacket() + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + + for sts_id in range(1, 11): + # Check if groupsyncread data of STServo#1~10 is available + sts_data_result, sts_error = groupSyncRead.isAvailable(scs_id, STS_PRESENT_POSITION_L, 4) + if sts_data_result == True: + # Get STServo#scs_id present position value + sts_present_position = groupSyncRead.getData(sts_id, STS_PRESENT_POSITION_L, 2) + sts_present_speed = groupSyncRead.getData(sts_id, STS_PRESENT_SPEED_L, 2) + print("[ID:%03d] PresPos:%d PresSpd:%d" % (sts_id, sts_present_position, packetHandler.sts_tohost(sts_present_speed, 15))) + else: + print("[ID:%03d] groupSyncRead getdata failed" % sts_id) + continue + if sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) + groupSyncRead.clearParam() +# Close port +portHandler.closePort() diff --git a/modules/actuators/bus_servo/STServo_examples/sync_read_write.py b/modules/actuators/bus_servo/STServo_examples/sync_read_write.py new file mode 100644 index 0000000..20bf103 --- /dev/null +++ b/modules/actuators/bus_servo/STServo_examples/sync_read_write.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python +# +# ********* Sync Read and Sync Write Example ********* +# +# +# Available STServo model on this example : All models using Protocol STS +# This example is tested with a STServo and an URT +# + +import sys +import os +import time + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * # Uses STServo SDK library + +# Control table address + +# Default setting +BAUDRATE = 1000000 # STServo default baudrate : 1000000 +DEVICENAME = 'COM11' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +STS_MINIMUM_POSITION_VALUE = 0 # SCServo will rotate between this value +STS_MAXIMUM_POSITION_VALUE = 4095 +STS_MOVING_SPEED = 2400 # SCServo moving speed +STS_MOVING_ACC = 50 # SCServo moving acc + +index = 0 +sts_goal_position = [STS_MINIMUM_POSITION_VALUE, STS_MAXIMUM_POSITION_VALUE] # Goal position + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +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...") + getch() + 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...") + getch() + quit() + +groupSyncRead = GroupSyncRead(packetHandler, STS_PRESENT_POSITION_L, 11) + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + for sts_id in range(1, 11): + # Add SCServo#1~10 goal position\moving speed\moving accc value to the Syncwrite parameter storage + sts_addparam_result = packetHandler.SyncWritePosEx(sts_id, sts_goal_position[index], STS_MOVING_SPEED, STS_MOVING_ACC) + if sts_addparam_result != True: + print("[ID:%03d] groupSyncWrite addparam failed" % sts_id) + + # Syncwrite goal position + sts_comm_result = packetHandler.groupSyncWrite.txPacket() + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + + # Clear syncwrite parameter storage + packetHandler.groupSyncWrite.clearParam() + time.sleep(0.002) #wait for servo status moving=1 + while 1: + # Add parameter storage for STServo#1~10 present position value + for sts_id in range(1, 11): + sts_addparam_result = groupSyncRead.addParam(scs_id) + if sts_addparam_result != True: + print("[ID:%03d] groupSyncRead addparam failed" % sts_id) + + sts_comm_result = groupSyncRead.txRxPacket() + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + + sts_last_moving = 0; + for sts_id in range(1, 11): + # Check if groupsyncread data of STServo#1~10 is available + sts_data_result, sts_error = groupSyncRead.isAvailable(sts_id, STS_PRESENT_POSITION_L, 11) + if sts_data_result == True: + # Get STServo#scs_id present position moving value + sts_present_position = groupSyncRead.getData(sts_id, STS_PRESENT_POSITION_L, 2) + sts_present_speed = groupSyncRead.getData(sts_id, STS_PRESENT_SPEED_L, 2) + sts_present_moving = groupSyncRead.getData(sts_id, STS_MOVING, 1) + # print(scs_present_moving) + print("[ID:%03d] PresPos:%d PresSpd:%d" % (sts_id, sts_present_position, packetHandler.sts_tohost(sts_present_speed, 15))) + if sts_present_moving==1: + sts_last_moving = 1; + else: + print("[ID:%03d] groupSyncRead getdata failed" % sts_id) + continue + if sts_error: + print(packetHandler.getRxPacketError(sts_error)) + print("---") + + # Clear syncread parameter storage + groupSyncRead.clearParam() + if sts_last_moving==0: + break + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +# Close port +portHandler.closePort() diff --git a/modules/actuators/bus_servo/STServo_examples/sync_write.py b/modules/actuators/bus_servo/STServo_examples/sync_write.py new file mode 100644 index 0000000..4fc3c2a --- /dev/null +++ b/modules/actuators/bus_servo/STServo_examples/sync_write.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python +# +# ********* Sync Write Example ********* +# +# +# Available STServo model on this example : All models using Protocol STS +# This example is tested with a STServo and an URT +# + +import sys +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * # Uses STServo SDK library + +# Default setting +BAUDRATE = 1000000 # STServo default baudrate : 1000000 +DEVICENAME = 'COM11' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +STS_MINIMUM_POSITION_VALUE = 0 # STServo will rotate between this value +STS_MAXIMUM_POSITION_VALUE = 4095 +STS_MOVING_SPEED = 2400 # STServo moving speed +STS_MOVING_ACC = 50 # STServo moving acc + +index = 0 +sts_goal_position = [STS_MINIMUM_POSITION_VALUE, STS_MAXIMUM_POSITION_VALUE] # Goal position + + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +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...") + getch() + 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...") + getch() + quit() + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + for sts_id in range(1, 11): + # Add STServo#1~10 goal position\moving speed\moving accc value to the Syncwrite parameter storage + sts_addparam_result = packetHandler.SyncWritePosEx(sts_id, sts_goal_position[index], STS_MOVING_SPEED, STS_MOVING_ACC) + if sts_addparam_result != True: + print("[ID:%03d] groupSyncWrite addparam failed" % sts_id) + + # Syncwrite goal position + sts_comm_result = packetHandler.groupSyncWrite.txPacket() + if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) + + # Clear syncwrite parameter storage + packetHandler.groupSyncWrite.clearParam() + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +# Close port +portHandler.closePort() diff --git a/modules/actuators/bus_servo/STServo_examples/wheel.py b/modules/actuators/bus_servo/STServo_examples/wheel.py new file mode 100644 index 0000000..67b7555 --- /dev/null +++ b/modules/actuators/bus_servo/STServo_examples/wheel.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python +# +# ********* Gen Write Example ********* +# +# +# Available STServo model on this example : All models using Protocol STS +# This example is tested with a STServo and an URT +# + +import sys +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() + +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * # Uses STServo SDK library + +# Default setting +STS_ID = 1 # STServo ID : 1 +BAUDRATE = 1000000 # STServo default baudrate : 1000000 +DEVICENAME = 'COM11' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" +STS_MOVING_SPEED0 = 2400 # STServo moving speed +STS_MOVING_SPEED1 = -2400 # STServo moving speed +STS_MOVING_ACC = 50 # STServo moving acc + +index = 0 +sts_move_speed = [STS_MOVING_SPEED0, 0, STS_MOVING_SPEED1, 0] + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +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...") + getch() + 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...") + getch() + quit() + +sts_comm_result, sts_error = packetHandler.WheelMode(STS_ID) +if sts_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(sts_comm_result)) +elif sts_error != 0: + print("%s" % packetHandler.getRxPacketError(sts_error)) +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Write STServo goal position/moving speed/moving acc + sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, sts_move_speed[index], 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)) + + # Change move speed + index += 1 + if index == 4: + index = 0 + +# Close port +portHandler.closePort() diff --git a/modules/actuators/bus_servo/STServo_examples/write.py b/modules/actuators/bus_servo/STServo_examples/write.py new file mode 100644 index 0000000..9e66b55 --- /dev/null +++ b/modules/actuators/bus_servo/STServo_examples/write.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python +# +# ********* Gen Write Example ********* +# +# +# Available STServo model on this example : All models using Protocol STS +# This example is tested with a STServo and an URT +# + +import sys +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() + +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * # Uses STServo SDK library + +# Default setting +STS_ID = 2 # STServo ID : 1 +BAUDRATE = 1000000 # STServo default baudrate : 1000000 +DEVICENAME = '/dev/ttyACM0' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" +STS_MINIMUM_POSITION_VALUE = 0 # STServo will rotate between this value +STS_MAXIMUM_POSITION_VALUE = 2047 #4095 +STS_MOVING_SPEED = 2400 # STServo moving speed +STS_MOVING_ACC = 50 # STServo moving acc + +index = 0 +sts_goal_position = [STS_MINIMUM_POSITION_VALUE, STS_MAXIMUM_POSITION_VALUE] # Goal position + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Get methods and members of Protocol +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...") + getch() + 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...") + getch() + quit() + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Write STServo goal position/moving speed/moving acc + sts_comm_result, sts_error = packetHandler.WritePosEx(STS_ID, sts_goal_position[index], 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)) + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +# Close port +portHandler.closePort() diff --git a/modules/actuators/bus_servo/STservo_sdk/__init__.py b/modules/actuators/bus_servo/STservo_sdk/__init__.py new file mode 100644 index 0000000..58551ab --- /dev/null +++ b/modules/actuators/bus_servo/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 * diff --git a/modules/actuators/bus_servo/STservo_sdk/group_sync_read.py b/modules/actuators/bus_servo/STservo_sdk/group_sync_read.py new file mode 100644 index 0000000..dad8d33 --- /dev/null +++ b/modules/actuators/bus_servo/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 diff --git a/modules/actuators/bus_servo/STservo_sdk/group_sync_write.py b/modules/actuators/bus_servo/STservo_sdk/group_sync_write.py new file mode 100644 index 0000000..b2ce36d --- /dev/null +++ b/modules/actuators/bus_servo/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)) diff --git a/modules/actuators/bus_servo/STservo_sdk/port_handler.py b/modules/actuators/bus_servo/STservo_sdk/port_handler.py new file mode 100644 index 0000000..63f0943 --- /dev/null +++ b/modules/actuators/bus_servo/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 \ No newline at end of file diff --git a/modules/actuators/bus_servo/STservo_sdk/protocol_packet_handler.py b/modules/actuators/bus_servo/STservo_sdk/protocol_packet_handler.py new file mode 100644 index 0000000..3e8412c --- /dev/null +++ b/modules/actuators/bus_servo/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<> 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 diff --git a/modules/actuators/bus_servo/STservo_sdk/scscl.py b/modules/actuators/bus_servo/STservo_sdk/scscl.py new file mode 100644 index 0000000..c107a83 --- /dev/null +++ b/modules/actuators/bus_servo/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.sts_lobyte(position), self.sts_hibyte(position), self.sts_lobyte(time), self.sts_hibyte(time), self.sts_lobyte(speed), self.sts_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.sts_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.sts_loword(scs_present_position_speed) + scs_present_speed = self.sts_hiword(scs_present_position_speed) + return scs_present_position, self.sts_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.sts_lobyte(position), self.sts_hibyte(position), self.sts_lobyte(time), self.sts_hibyte(time), self.sts_lobyte(speed), self.sts_hibyte(speed)] + return self.groupSyncWrite.addParam(scs_id, txpacket) + + def RegWritePos(self, scs_id, position, time, speed): + txpacket = [self.sts_lobyte(position), self.sts_hibyte(position), self.sts_lobyte(time), self.sts_hibyte(time), self.sts_lobyte(speed), self.sts_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.sts_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) diff --git a/modules/actuators/bus_servo/STservo_sdk/sts.py b/modules/actuators/bus_servo/STservo_sdk/sts.py new file mode 100644 index 0000000..7d92ef7 --- /dev/null +++ b/modules/actuators/bus_servo/STservo_sdk/sts.py @@ -0,0 +1,111 @@ +#!/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(读写)-------- +STS_TORQUE_ENABLE = 40 +STS_ACC = 41 +STS_GOAL_POSITION_L = 42 +STS_GOAL_POSITION_H = 43 +STS_GOAL_TIME_L = 44 +STS_GOAL_TIME_H = 45 +STS_GOAL_SPEED_L = 46 +STS_GOAL_SPEED_H = 47 +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, STS_ACC, 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, STS_PRESENT_POSITION_L) + return self.sts_tohost(sts_present_position, 15), 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 ReadMoving(self, sts_id): + moving, sts_comm_result, sts_error = self.read1ByteTxRx(sts_id, STS_MOVING) + return moving, 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 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) + diff --git a/modules/actuators/bus_servo/STservo_sdk/stservo_def.py b/modules/actuators/bus_servo/STservo_sdk/stservo_def.py new file mode 100644 index 0000000..a5fb1f5 --- /dev/null +++ b/modules/actuators/bus_servo/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 # diff --git a/modules/actuators/bus_servo/change_id.py b/modules/actuators/bus_servo/change_id.py new file mode 100644 index 0000000..6b5fd43 --- /dev/null +++ b/modules/actuators/bus_servo/change_id.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python +# +# ********* Change Servo ID Example ********* +# +# Works for both STS (ST3215) and SCSCL (SC09) servos. +# Requires each servo to be connected individually. +# RUN THIS BEFORE USING THE SERVOS IN THIS PROGRAM! +# + +import sys +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +sys.path.append("..") +from STservo_sdk import * + +# User config +SERVO_TYPE = input("Enter servo type (STS for ST3215, SCSCL for SC09): ").strip().upper() +SERVO_ID = int(input("Enter current servo ID (1-253): ")) +BAUDRATE = 1000000 +DEVICENAME = '/dev/ttyACM0' # Change as needed + +# Initialize PortHandler +portHandler = PortHandler(DEVICENAME) + +# Select correct packet handler +if SERVO_TYPE == "STS": + packetHandler = sts(portHandler) + ID_ADDR = 5 +elif SERVO_TYPE == "SCSCL": + packetHandler = scscl(portHandler) + ID_ADDR = 5 +else: + print("Unknown servo type.") + sys.exit(1) + +# Open port +if portHandler.openPort(): + print("Succeeded to open the port") +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + sys.exit(1) + +# Set baudrate +if portHandler.setBaudRate(BAUDRATE): + print("Succeeded to change the baudrate") +else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + getch() + portHandler.closePort() + sys.exit(1) + +while True: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Read present position/speed (optional, for feedback) + try: + if SERVO_TYPE == "STS": + pos, spd, comm_result, err = packetHandler.ReadPosSpeed(SERVO_ID) + else: + pos, spd, comm_result, err = packetHandler.ReadPosSpeed(SERVO_ID) + if comm_result == COMM_SUCCESS: + print(f"[ID:{SERVO_ID:03d}] PresPos:{pos} PresSpd:{spd}") + else: + print(packetHandler.getTxRxResult(comm_result)) + if err != 0: + print(packetHandler.getRxPacketError(err)) + except Exception as e: + print(f"Could not read servo: {e}") + + # Get new ID + try: + new_id = int(input("Enter new ID (1-253): ")) + except Exception: + print("Invalid input.") + continue + if new_id < 1 or new_id > 253: + print("Invalid ID. Please enter a value between 1 and 253.") + continue + + # Unlock EEPROM + unlock_result, unlock_error = packetHandler.unLockEprom(SERVO_ID) + if unlock_result != COMM_SUCCESS or unlock_error != 0: + print("Failed to unlock EEPROM: %s %s" % ( + packetHandler.getTxRxResult(unlock_result), + packetHandler.getRxPacketError(unlock_error) + )) + continue + + # Write new ID + write_result, write_error = packetHandler.write1ByteTxRx(SERVO_ID, ID_ADDR, new_id) + if write_result != COMM_SUCCESS or write_error != 0: + print("Failed to change ID: %s %s" % ( + packetHandler.getTxRxResult(write_result), + packetHandler.getRxPacketError(write_error) + )) + continue + + # Lock EEPROM + lock_result, lock_error = packetHandler.LockEprom(new_id) + if lock_result != COMM_SUCCESS or lock_error != 0: + print("Failed to lock EEPROM: %s %s" % ( + packetHandler.getTxRxResult(lock_result), + packetHandler.getRxPacketError(lock_error) + )) + continue + + print(f"{SERVO_TYPE} servo ID changed successfully to {new_id}") + SERVO_ID = new_id + +portHandler.closePort() diff --git a/modules/actuators/bus_servo/servo.py b/modules/actuators/bus_servo/servo.py new file mode 100644 index 0000000..3939896 --- /dev/null +++ b/modules/actuators/bus_servo/servo.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python + +import sys +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() + +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +# sys.path.append("..") +# Uses STServo and SCServo SDK library +from modules.actuators.bus_servo.STservo_sdk import * +from modules.actuators.bus_servo.SCservo_sdk import * + +from modules.base_module import BaseModule + +# Control table address +ADDR_TORQUE_ENABLE = 40 # Same address for both ST and SC servos +ADDR_SCS_GOAL_ACC = 41 +ADDR_SCS_GOAL_POSITION = 42 # Used in SCServo move +ADDR_SCS_GOAL_SPEED = 46 +ADDR_SCS_PRESENT_POSITION = 56 + + +class Servo(BaseModule): + def __init__(self, **kwargs): + """ + Servo class + """ + + self.identifier = kwargs.get('name') + self.model = kwargs.get('model', 'ST') + self.index = kwargs.get('id') + self.range = kwargs.get('range') + self.start = kwargs.get('start') # Default start position + self.poses = kwargs.get('poses') # Dictionary of poses + self.baudrate = kwargs.get('baudrate', 1000000) + self.port = kwargs.get('port', '/dev/ttyACM0') + self.configure_on_boot = kwargs.get('configure_on_boot', False) # Loop to show position for manual configuration + self.pos = self.start + self.speed = 2400 # 3073 + self.acceleration = 50 + + # Initialize PortHandler instance + # Set the port path + # Get methods and members of PortHandlerLinux or PortHandlerWindows + self.portHandler = PortHandler(self.port) + + # if model starts with ST or SC, use the appropriate packet handler + if self.model.startswith('ST'): + self.packetHandler = sts(self.portHandler) + elif self.model.startswith('SC'): + self.packetHandler = PacketHandler(1) # 1 = protocol_end in examples + else: + raise ValueError(f"Unknown servo model: {self.model}. Supported models are ST and SC.") + + # Open port + if not self.portHandler.openPort(): + raise Exception("Failed to open the port") + + # Set port baudrate + if not self.portHandler.setBaudRate(self.baudrate): + raise Exception("Failed to change the baudrate") + + + def exit(self): + # Detach servo + if self.model.startswith('ST'): + # Disable torque for STServo + sts_comm_result, sts_error = self.packetHandler.write1ByteTxRx(self.index, ADDR_TORQUE_ENABLE, 0) + if not self.handle_errors(sts_comm_result, sts_error): + self.log(f"ST Servo {self.identifier} disabled") + else: + # Disable torque for SCServo + scs_comm_result, scs_error = self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_TORQUE_ENABLE, 0) + if not self.handle_errors(scs_comm_result, scs_error): + self.log(f"SC Servo {self.identifier} disabled") + + self.portHandler.closePort() + + def setup_messaging(self): + self.subscribe('servo:' + self.identifier + ':mvabs', self.move) + self.subscribe('servo:' + self.identifier + ':mv', self.move_relative) + self.subscribe('system/exit', self.exit) + + if self.configure_on_boot: + while True: + self.get_position() # Log will show current position repeatedly to help with manual configuration + + self.pos = self.get_position() # Get initial position to avoid jumping from unknown position + + # Move to start position + # if self.get_pose_value('stand') is not None: + # self.start = self.get_pose_value('stand') + self.move(self.start) + + def move(self, position): + """ + Move the servo to an absolute position. + :param position: Position to move to (0-100) + """ + if position < self.range[0] or position > self.range[1]: + self.log(f"Position {position} out of range ({self.range[0]}-{self.range[1]})", level='error') + return + + # Write STServo goal position + if self.model.startswith('ST'): + sts_comm_result, sts_error = self.packetHandler.WritePosEx(self.index, position, self.speed, self.acceleration) + if not self.handle_errors(sts_comm_result, sts_error): + self.log(f"Moved servo {self.identifier} from {self.pos} to position {position}") + self.pos = position # Update current position + elif self.model.startswith('SC'): + self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_ACC, self.acceleration) + self.packetHandler.write2ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_SPEED, self.speed) + self.packetHandler.write2ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_POSITION, position) + self.log(f"Moved servo {self.identifier} from {self.pos} to position {position}") + + def move_relative(self, delta): + """ + Move the servo relative to its current position. + :param delta: Change in position (can be negative) + """ + new_position = self.pos + delta + if new_position < self.range[0] or new_position > self.range[1]: + self.log(f"Position {new_position} out of range ({self.range[0]}-{self.range[1]})", level='error') + return + + # Move to new position + self.move(new_position) + + def get_position(self): + """ + Get the current position of the servo. + """ + if self.model.startswith('ST'): + # Read STServo present position + sts_present_position, sts_present_speed, sts_comm_result, sts_error = self.packetHandler.ReadPosSpeed(self.index) + if not self.handle_errors(sts_comm_result, sts_error): + self.log("[ID:%03d] PresPos:%d PresSpd:%d" % (STS_ID, sts_present_position, sts_present_speed)) + return sts_present_position + else: + return self.sc_get_position_speed('position') + + def get_speed(self): + """ + Get the current speed of the servo. + """ + if self.model.startswith('ST'): + # Read STServo present position + sts_present_position, sts_present_speed, sts_comm_result, sts_error = self.packetHandler.ReadPosSpeed(self.index) + if not self.handle_errors(sts_comm_result, sts_error): + return sts_present_speed + else: + return self.sc_get_position_speed('speed') + + + def sc_get_position_speed(self, pos_or_speed): + # Read SCServo present position + scs_present_position_speed, scs_comm_result, scs_error = self.packetHandler.read4ByteTxRx(self.portHandler, self.index, ADDR_SCS_PRESENT_POSITION) + if not self.handle_errors(scs_comm_result, scs_error): + scs_present_position = SCS_LOWORD(scs_present_position_speed) + scs_present_speed = SCS_HIWORD(scs_present_position_speed) + if pos_or_speed == 'position': + return scs_present_position + elif pos_or_speed == 'speed': + return SCS_TOHOST(scs_present_speed, 15) + + def enable_continuous(self): + """ + Set the servo mode. + :param mode: Mode to set (e.g., 'wheel', 'position') + """ + if self.model.startswith('SC'): + raise ValueError("Continuous mode is not supported for SCServo models.") + sts_comm_result, sts_error = self.packetHandler.WheelMode(self.index) + if not self.handle_errors(sts_comm_result, sts_error): + self.log(f"Servo {self.name} set to wheel mode") + + def turn_wheel(self, speed): + if self.model.startswith('SC'): + raise ValueError("Continuous mode is not supported for SCServo models.") + sts_comm_result, sts_error = self.packetHandler.WriteSpec(self.index, speed, self.acceleration) + if not self.handle_errors(sts_comm_result, sts_error): + self.log(f"Servo {self.name} turned at speed {speed}") + + def handle_errors(self, comm_result, error): + """ + Handle communication errors. + :param comm_result: Communication result + :param error: Error code + """ + if comm_result != COMM_SUCCESS: + self.log("%s" % self.packetHandler.getTxRxResult(comm_result), level='error') + # log stack trace for debugging + return True + if error != 0: + self.log("%s" % self.packetHandler.getRxPacketError(error), level='error') + return True + return False + + def get_pose_value(self, pose_name): + """ + Returns the position value for the given pose name from self.poses. + """ + if not self.poses: + return None + for pose in self.poses: + if pose_name in pose: + return pose[pose_name] + return None # or raise an exception if preferred + + + + + + + + + + + diff --git a/tests/test_sensor.py b/tests/test_sensor.py index ee37fc7..6bfdae9 100644 --- a/tests/test_sensor.py +++ b/tests/test_sensor.py @@ -41,7 +41,7 @@ def test_loop(self): with mock.patch.object(sensor, 'read', return_value=True): with mock.patch.object(sensor, 'publish') as mock_publish: sensor.loop() - mock_publish.assert_called_with('motion') + mock_publish.assert_called_with('gpio/motion') with mock.patch.object(sensor, 'read', return_value=False): with mock.patch.object(sensor, 'publish') as mock_publish: