117 lines
3.7 KiB
Python
117 lines
3.7 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
|
||
import argparse
|
||
import math
|
||
import struct
|
||
import can
|
||
|
||
|
||
CMD_READ_MULTITURN_ANGLE = 0x92
|
||
|
||
|
||
def arb_id_single(motor_id: int) -> int:
|
||
# 单电机命令/回包 arbitration id
|
||
return 0x140 + int(motor_id)
|
||
|
||
|
||
def build_read_multiturn_angle(motor_id: int) -> can.Message:
|
||
"""
|
||
读取多圈角度命令：
|
||
data[0]=0x92，其余填0
|
||
"""
|
||
data = [CMD_READ_MULTITURN_ANGLE, 0, 0, 0, 0, 0, 0, 0]
|
||
return can.Message(arbitration_id=arb_id_single(motor_id), is_extended_id=False, data=data)
|
||
|
||
|
||
def sign_extend(value: int, bits: int) -> int:
|
||
"""
|
||
对 value 做符号扩展，使其成为 Python int（有符号）
|
||
"""
|
||
sign_bit = 1 << (bits - 1)
|
||
mask = (1 << bits) - 1
|
||
value &= mask
|
||
return (value ^ sign_bit) - sign_bit
|
||
|
||
|
||
def parse_multiturn_angle_reply_int56(msg: can.Message) -> int:
|
||
"""
|
||
兼容你贴的文档：DATA[1..7] 是角度的低->高字节（共7字节）
|
||
=> 解析为有符号 int56
|
||
单位：0.01 deg / LSB
|
||
"""
|
||
d = bytes(msg.data)
|
||
if len(d) != 8 or d[0] != CMD_READ_MULTITURN_ANGLE:
|
||
raise ValueError("not 0x92 reply")
|
||
raw_u56 = int.from_bytes(d[1:8], byteorder="little", signed=False) # 7 bytes
|
||
raw_s56 = sign_extend(raw_u56, 56)
|
||
return raw_s56
|
||
|
||
|
||
def parse_multiturn_angle_reply_int64(msg: can.Message) -> int:
|
||
"""
|
||
如果你确认实际回的是 8 字节 motorAngle（int64）：
|
||
常见做法是 DATA[0..7] 都是 motorAngle 的 8 字节（但会与“命令字节=0x92”冲突）
|
||
或者命令字节在别处。
|
||
这个函数留作你确认后切换用。
|
||
"""
|
||
d = bytes(msg.data)
|
||
# 这里假设 d[0] 仍是 0x92，则 motorAngle 应该在 d[1:9] 不存在
|
||
# 所以只有当你的实际报文不是“d[0]=0x92”时才用它
|
||
raise NotImplementedError("Need exact 8-byte layout confirmation.")
|
||
|
||
|
||
def main():
|
||
ap = argparse.ArgumentParser()
|
||
ap.add_argument("--interface", default="canalystii", help="python-can interface: canalystii / socketcan / ...")
|
||
ap.add_argument("--channel", default="0", help="channel: canalystii 0/1, socketcan can0/can1 ...")
|
||
ap.add_argument("--bitrate", type=int, default=1000000)
|
||
ap.add_argument("--motor-id", type=int, default=2)
|
||
ap.add_argument("--timeout", type=float, default=0.2, help="seconds")
|
||
ap.add_argument("--repeat", type=int, default=3, help="query times")
|
||
args = ap.parse_args()
|
||
|
||
motor_id = args.motor_id
|
||
rx_arb = arb_id_single(motor_id)
|
||
|
||
bus = can.Bus(interface=args.interface, channel=args.channel, bitrate=args.bitrate)
|
||
|
||
# 只收这个电机的回包（降低噪声）
|
||
try:
|
||
bus.set_filters([{"can_id": rx_arb, "can_mask": 0x7FF, "extended": False}])
|
||
except Exception:
|
||
pass
|
||
|
||
try:
|
||
for i in range(args.repeat):
|
||
# 发送 0x92
|
||
bus.send(build_read_multiturn_angle(motor_id))
|
||
|
||
# 等回包
|
||
msg = bus.recv(timeout=args.timeout)
|
||
if msg is None:
|
||
print(f"[M{motor_id}] timeout: no reply")
|
||
continue
|
||
if msg.arbitration_id != rx_arb or len(msg.data) != 8:
|
||
print(f"[M{motor_id}] unexpected frame: arb=0x{msg.arbitration_id:X} data={list(msg.data)}")
|
||
continue
|
||
|
||
# 解析（按你贴的文档：int56 in DATA[1..7]）
|
||
motor_angle_raw = parse_multiturn_angle_reply_int56(msg)
|
||
|
||
# 单位换算：0.01°/LSB
|
||
angle_deg = motor_angle_raw * 0.01
|
||
angle_rad = angle_deg * math.pi / 180.0
|
||
|
||
print(
|
||
f"[M{motor_id}] motorAngle_raw={motor_angle_raw} "
|
||
f"angle_deg={angle_deg:+.4f}° angle_rad={angle_rad:+.6f} rad"
|
||
)
|
||
|
||
finally:
|
||
bus.shutdown()
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|