#!/usr/bin/env python3 # -*- coding: utf-8 -*- import time import serial class LeArmSerial: def __init__(self, port="/dev/ttyUSB0", baudrate=9600): self.port = port self.baudrate = baudrate self.ser = None def open(self): self.ser = serial.Serial() self.ser.port = self.port self.ser.baudrate = self.baudrate self.ser.bytesize = serial.EIGHTBITS self.ser.parity = serial.PARITY_NONE self.ser.stopbits = serial.STOPBITS_ONE self.ser.timeout = 0.1 self.ser.xonxoff = False self.ser.rtscts = False self.ser.dsrdtr = False self.ser.dtr = False self.ser.rts = False self.ser.open() print(f"[INFO] 串口已打开: {self.port} @ {self.baudrate}") print("[INFO] 等待控制板稳定...") time.sleep(3.0) self.ser.reset_input_buffer() self.ser.reset_output_buffer() def close(self): if self.ser and self.ser.is_open: self.ser.close() print("[INFO] 串口已关闭") @staticmethod def build_frame(cmd: int, params: bytes = b"") -> bytes: length = 2 + len(params) return bytes([0x55, 0x55, length, cmd]) + params def send_frame(self, cmd: int, params: bytes = b""): frame = self.build_frame(cmd, params) print("TX:", frame.hex(" ")) self.ser.write(frame) self.ser.flush() def read_frames(self, wait_s: float = 1.0): deadline = time.time() + wait_s buf = bytearray() while time.time() < deadline: n = self.ser.in_waiting if n > 0: buf.extend(self.ser.read(n)) time.sleep(0.01) print("RAW RX:", bytes(buf).hex(" ")) return self.split_frames(bytes(buf)) @staticmethod def split_frames(buf: bytes): frames = [] i = 0 while i <= len(buf) - 3: if buf[i] == 0x55 and buf[i + 1] == 0x55: length = buf[i + 2] frame_len = 2 + length if i + frame_len <= len(buf): frames.append(buf[i:i + frame_len]) i += frame_len continue i += 1 return frames @staticmethod def parse_fw_query(frame: bytes): if len(frame) != 6 or frame[:4] != b"\x55\x55\x04\x01": return None servo_type = frame[4] fw_ver = frame[5] return { "servo_type": servo_type, "servo_type_text": {1: "PWM舵机", 2: "总线舵机"}.get(servo_type, f"未知({servo_type})"), "firmware_version": fw_ver, } @staticmethod def parse_servo_readback(frame: bytes): if len(frame) < 6 or frame[0] != 0x55 or frame[1] != 0x55 or frame[3] != 0x0D: return None payload = frame[4:] items = [] if len(payload) % 3 != 0: usable = len(payload) - (len(payload) % 3) payload = payload[:usable] for i in range(0, len(payload), 3): sid = payload[i] pulse = payload[i + 1] | (payload[i + 2] << 8) items.append({"id": sid, "pulse": pulse}) return items def query_firmware(self): self.ser.reset_input_buffer() self.send_frame(0x01) frames = self.read_frames(wait_s=1.0) ok = False for f in frames: print("FRAME:", f.hex(" ")) info = self.parse_fw_query(f) if info: ok = True print("[OK] 固件查询成功") print("舵机类型:", info["servo_type_text"]) print("固件版本号:", info["firmware_version"]) if not ok: print("[WARN] 未解析到有效固件查询返回") def read_servo_angles(self): self.ser.reset_input_buffer() self.send_frame(0x0D) frames = self.read_frames(wait_s=1.0) ok = False for f in frames: print("FRAME:", f.hex(" ")) items = self.parse_servo_readback(f) if items is not None: ok = True print("[OK] 舵机角度回读成功") for item in items: print(f" 舵机ID={item['id']}, 脉宽={item['pulse']}") if not ok: print("[WARN] 未解析到有效舵机角度回读返回") def set_init_pose(self): self.ser.reset_input_buffer() self.send_frame(0x0C) time.sleep(1.5) print("[OK] 已发送初始位姿设置命令") def move_servos(self, servo_pairs, move_time_ms=800): """ servo_pairs: [(servo_id, pulse), ...] 协议: 长度 = 舵机个数*3 + 5 params: 个数(1B), time_low(1B), time_high(1B), [id, pulse_low, pulse_high] * N """ if not servo_pairs: print("[WARN] 空舵机列表") return params = bytearray() params.append(len(servo_pairs)) params.append(move_time_ms & 0xFF) params.append((move_time_ms >> 8) & 0xFF) for sid, pulse in servo_pairs: pulse = int(pulse) params.append(int(sid) & 0xFF) params.append(pulse & 0xFF) params.append((pulse >> 8) & 0xFF) self.ser.reset_input_buffer() self.send_frame(0x03, bytes(params)) time.sleep(max(move_time_ms / 1000.0, 0.5)) print("[OK] 已发送多舵机控制命令") def interactive(self): menu = """ ================ LeArm 串口测试 ================= 1. 固件查询 2. 舵机角度回读 3. 初始位姿设置 4. 控制单个舵机 5. 控制多个舵机 q. 退出 舵机范围 (1夹爪,200-700;2腕部,125-875;) (3号,125-875;4号,125-875;) (5号,125-875;6旋转,125-875;) ================================================= """ while True: print(menu) choice = input("请选择: ").strip().lower() if choice == "1": self.query_firmware() elif choice == "2": self.read_servo_angles() elif choice == "3": self.set_init_pose() elif choice == "4": sid = int(input("舵机ID: ").strip()) pulse = int(input("目标位置(例如 500): ").strip()) t = int(input("运行时间ms(例如 800): ").strip() or "800") self.move_servos([(sid, pulse)], move_time_ms=t) elif choice == "5": print("输入格式示例: 1:500,2:600,3:450") raw = input("舵机列表: ").strip() t = int(input("运行时间ms(例如 800): ").strip() or "800") pairs = [] for item in raw.split(","): sid_s, pulse_s = item.split(":") pairs.append((int(sid_s), int(pulse_s))) self.move_servos(pairs, move_time_ms=t) elif choice == "q": break else: print("[WARN] 无效选项") def main(): arm = LeArmSerial(port="/dev/ttyUSB0", baudrate=9600) try: arm.open() arm.interactive() finally: arm.close() if __name__ == "__main__": main()