From 2fc2fe998bea408594c53c5be9faa488687a4464 Mon Sep 17 00:00:00 2001 From: yutang Date: Wed, 25 Jun 2025 16:54:34 +0800 Subject: [PATCH] add controller status --- realman_src/crc_cal.py | 58 ++++++++++ realman_src/servo_arm.yaml | 6 + realman_src/servo_arm_with_io_status.py | 140 ++++++++++++++++++++++++ 3 files changed, 204 insertions(+) create mode 100644 realman_src/crc_cal.py create mode 100644 realman_src/servo_arm.yaml create mode 100644 realman_src/servo_arm_with_io_status.py diff --git a/realman_src/crc_cal.py b/realman_src/crc_cal.py new file mode 100644 index 000000000..1ee50c3aa --- /dev/null +++ b/realman_src/crc_cal.py @@ -0,0 +1,58 @@ +def crc8_maxim(data): + """ + 使用CRC-8/MAXIM算法计算CRC8校验值 + 多项式：X8+X5+X4+1=0x31 + + Args: + data: 要校验的字节数据 + + Returns: + 计算出的CRC8校验值 + """ + # CRC8表（与您提供的表相同） + CRC8TAB = [ + 0x00, 0x31, 0x62, 0x53, 0xC4, 0xF5, 0xA6, 0x97, + 0xB9, 0x88, 0xDB, 0xEA, 0x7D, 0x4C, 0x1F, 0x2E, + 0x43, 0x72, 0x21, 0x10, 0x87, 0xB6, 0xE5, 0xD4, + 0xFA, 0xCB, 0x98, 0xA9, 0x3E, 0x0F, 0x5C, 0x6D, + 0x86, 0xB7, 0xE4, 0xD5, 0x42, 0x73, 0x20, 0x11, + 0x3F, 0x0E, 0x5D, 0x6C, 0xFB, 0xCA, 0x99, 0xA8, + 0xC5, 0xF4, 0xA7, 0x96, 0x01, 0x30, 0x63, 0x52, + 0x7C, 0x4D, 0x1E, 0x2F, 0xB8, 0x89, 0xDA, 0xEB, + 0x3D, 0x0C, 0x5F, 0x6E, 0xF9, 0xC8, 0x9B, 0xAA, + 0x84, 0xB5, 0xE6, 0xD7, 0x40, 0x71, 0x22, 0x13, + 0x7E, 0x4F, 0x1C, 0x2D, 0xBA, 0x8B, 0xD8, 0xE9, + 0xC7, 0xF6, 0xA5, 0x94, 0x03, 0x32, 0x61, 0x50, + 0xBB, 0x8A, 0xD9, 0xE8, 0x7F, 0x4E, 0x1D, 0x2C, + 0x02, 0x33, 0x60, 0x51, 0xC6, 0xF7, 0xA4, 0x95, + 0xF8, 0xC9, 0x9A, 0xAB, 0x3C, 0x0D, 0x5E, 0x6F, + 0x41, 0x70, 0x23, 0x12, 0x85, 0xB4, 0xE7, 0xD6, + 0x7A, 0x4B, 0x18, 0x29, 0xBE, 0x8F, 0xDC, 0xED, + 0xC3, 0xF2, 0xA1, 0x90, 0x07, 0x36, 0x65, 0x54, + 0x39, 0x08, 0x5B, 0x6A, 0xFD, 0xCC, 0x9F, 0xAE, + 0x80, 0xB1, 0xE2, 0xD3, 0x44, 0x75, 0x26, 0x17, + 0xFC, 0xCD, 0x9E, 0xAF, 0x38, 0x09, 0x5A, 0x6B, + 0x45, 0x74, 0x27, 0x16, 0x81, 0xB0, 0xE3, 0xD2, + 0xBF, 0x8E, 0xDD, 0xEC, 0x7B, 0x4A, 0x19, 0x28, + 0x06, 0x37, 0x64, 0x55, 0xC2, 0xF3, 0xA0, 0x91, + 0x47, 0x76, 0x25, 0x14, 0x83, 0xB2, 0xE1, 0xD0, + 0xFE, 0xCF, 0x9C, 0xAD, 0x3A, 0x0B, 0x58, 0x69, + 0x04, 0x35, 0x66, 0x57, 0xC0, 0xF1, 0xA2, 0x93, + 0xBD, 0x8C, 0xDF, 0xFE, 0x79, 0x48, 0x1B, 0x2A, + 0xC1, 0xF0, 0xA3, 0x92, 0x05, 0x34, 0x67, 0x56, + 0x78, 0x49, 0x1A, 0x2B, 0xBC, 0x8D, 0xDE, 0xEF, + 0x82, 0xB3, 0xE0, 0xD1, 0x46, 0x77, 0x24, 0x15, + 0x3B, 0x0A, 0x59, 0x68, 0xFF, 0xCE, 0x9D, 0xAC + ] + + crc = 0 + for byte in data: + crc = CRC8TAB[crc ^ byte] + + return crc + + +# 数据帧：55 AA 02 00 00 67 +data = bytes([0x55, 0xAA, 0x08, 0x00, 0x02]) +crc = crc8_maxim(data) +print(f"计算出的CRC8校验值: 0x{crc:02X}") \ No newline at end of file diff --git a/realman_src/servo_arm.yaml b/realman_src/servo_arm.yaml new file mode 100644 index 000000000..06d28ee29 --- /dev/null +++ b/realman_src/servo_arm.yaml @@ -0,0 +1,6 @@ +port: /dev/ttyUSB0 +right_port: /dev/ttyUSB1 +baudrate: 460800 +joint_hex_data: "55 AA 02 00 00 67" +control_hex_data: "55 AA 08 00 00 B9" +arm_axis: 6 diff --git a/realman_src/servo_arm_with_io_status.py b/realman_src/servo_arm_with_io_status.py new file mode 100644 index 000000000..eb4e7c6ff --- /dev/null +++ b/realman_src/servo_arm_with_io_status.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +import time +import yaml +import serial +import logging +import binascii +import numpy as np + +# 配置日志记录 +logging.basicConfig( + level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s" +) + + +class ServoArm: + def __init__(self, config_file="config.yaml"): + """初始化机械臂的串口连接并发送初始数据。 + + Args: + config_file (str): 配置文件的路径。 + """ + self.config = self._load_config(config_file) + self.port = self.config["port"] + self.baudrate = self.config["baudrate"] + self.joint_hex_data = self.config["joint_hex_data"] + self.control_hex_data = self.config["control_hex_data"] + self.arm_axis = self.config.get("arm_axis", 7) + + self.serial_conn = serial.Serial(self.port, self.baudrate, timeout=0) + + self.bytes_to_send = binascii.unhexlify(self.joint_hex_data.replace(" ", "")) + self.serial_conn.write(self.bytes_to_send) + time.sleep(1) + + def _load_config(self, config_file): + """加载配置文件。 + + Args: + config_file (str): 配置文件的路径。 + + Returns: + dict: 配置文件内容。 + """ + with open(config_file, "r") as file: + config = yaml.safe_load(file) + return config + + def _bytes_to_signed_int(self, byte_data): + """将字节数据转换为有符号整数。 + + Args: + byte_data (bytes): 字节数据。 + + Returns: + int: 有符号整数。 + """ + return int.from_bytes(byte_data, byteorder="little", signed=True) + + def _parse_joint_data(self, hex_received): + """解析接收到的十六进制数据并提取关节数据。 + + Args: + hex_received (str): 接收到的十六进制字符串数据。 + + Returns: + dict: 解析后的关节数据。 + """ + logging.debug(f"hex_received: {hex_received}") + joints = {} + for i in range(self.arm_axis): + start = 14 + i * 10 + end = start + 8 + joint_hex = hex_received[start:end] + joint_byte_data = bytearray.fromhex(joint_hex) + joint_value = self._bytes_to_signed_int(joint_byte_data) / 10000.0 + joints[f"joint_{i+1}"] = joint_value + grasp_start = 14 + self.arm_axis*10 + grasp_hex = hex_received[grasp_start:grasp_start+8] + grasp_byte_data = bytearray.fromhex(grasp_hex) + # 夹爪进行归一化处理 + grasp_value = self._bytes_to_signed_int(grasp_byte_data)/1000 + # print(grasp_value) + + joints["grasp"] = grasp_value + return joints + + def _parse_controller_data(self, hex_received): + status = { + 'infrared': 0, + 'button': 0 + } + if len(hex_received) == 18: + status['infrared'] = self._bytes_to_signed_int(bytearray.fromhex(hex_received[12:14])) + status['button'] = self._bytes_to_signed_int(bytearray.fromhex(hex_received[14:16])) + # print(infrared) + return status + + def get_joint_actions(self): + """从串口读取数据并解析关节动作。 + + Returns: + dict: 包含关节数据的字典。 + """ + self.serial_conn.write(self.bytes_to_send) + time.sleep(0.02) + bytes_received = self.serial_conn.read(self.serial_conn.inWaiting()) + hex_received = binascii.hexlify(bytes_received).decode("utf-8").upper() + # print("joints:", hex_received) + actions = self._parse_joint_data(hex_received) + return actions + + def get_controller_status(self): + bytes_to_send = binascii.unhexlify(self.control_hex_data.replace(" ", "")) + self.serial_conn.write(bytes_to_send) + time.sleep(0.02) + bytes_received = self.serial_conn.read(self.serial_conn.inWaiting()) + hex_received = binascii.hexlify(bytes_received).decode("utf-8").upper() + # print("control status:", hex_received) + status = self._parse_controller_data(hex_received) + return status + + def set_gripper_action(self, action): + """设置夹爪动作。 + Args: + action (int): 夹爪动作值。 + """ + action = int(action * 1000) + action_bytes = action.to_bytes(4, byteorder="little", signed=True) + self.bytes_to_send = self.bytes_to_send[:74] + action_bytes + self.bytes_to_send[78:] + +if __name__ == "__main__": + servo_arm = ServoArm("/home/maic/LYT/lerobot/realman_src/servo_arm.yaml") + while True: + joint_actions = servo_arm.get_joint_actions() + logging.info(joint_actions) + # time.sleep(0.02) + status = servo_arm.get_controller_status() + logging.info(status) + time.sleep(0.5) \ No newline at end of file