242 lines
7.2 KiB
Python
242 lines
7.2 KiB
Python
#!/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()
|