Initial commit
This commit is contained in:
commit
a4e596c3aa
234 changed files with 81101 additions and 0 deletions
242
arm_plane_calib/learm_test.py
Normal file
242
arm_plane_calib/learm_test.py
Normal file
|
|
@ -0,0 +1,242 @@
|
|||
#!/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()
|
||||
Loading…
Add table
Add a link
Reference in a new issue