331 lines
11 KiB
Python
331 lines
11 KiB
Python
|
#!/usr/bin/env python3
|
|||
|
|
# -*- coding: utf-8 -*-
|
|||
|
|
|
|||
|
|
import time
|
|||
|
|
import struct
|
|||
|
|
import threading
|
|||
|
|
from dataclasses import dataclass
|
|||
|
|
from typing import Dict, List, Tuple, Optional
|
|||
|
|
|
|||
|
|
import can
|
|||
|
|
|
|||
|
|
TORQUE_CMD_ID = 0x280
|
|||
|
|
CMD_A1 = 0xA1
|
|||
|
|
|
|||
|
|
|
|||
|
|
def _clamp_int16(x: int) -> int:
|
|||
|
|
return max(-32768, min(32767, int(x)))
|
|||
|
|
|
|||
|
|
|
|||
|
|
def _pack_i16_le(x: int) -> Tuple[int, int]:
|
|||
|
|
x = _clamp_int16(x)
|
|||
|
|
u = x & 0xFFFF
|
|||
|
|
return (u & 0xFF, (u >> 8) & 0xFF)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def build_torque_broadcast(t1: int, t2: int, t3: int, t4: int) -> can.Message:
|
|||
|
|
d0, d1 = _pack_i16_le(t1)
|
|||
|
|
d2, d3 = _pack_i16_le(t2)
|
|||
|
|
d4, d5 = _pack_i16_le(t3)
|
|||
|
|
d6, d7 = _pack_i16_le(t4)
|
|||
|
|
return can.Message(
|
|||
|
|
arbitration_id=TORQUE_CMD_ID,
|
|||
|
|
is_extended_id=False,
|
|||
|
|
data=[d0, d1, d2, d3, d4, d5, d6, d7],
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def _arb_id_single(motor_id: int) -> int:
|
|||
|
|
return 0x140 + int(motor_id)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def parse_a1(data8: bytes) -> Dict:
|
|||
|
|
# A1 回包（你实测）
|
|||
|
|
# [0]=0xA1 [1]=temp(int8) [2..3]=iq/power(int16 LE) [4..5]=speed(int16 LE, dps) [6..7]=encoder(uint16 LE)
|
|||
|
|
if len(data8) != 8 or data8[0] != CMD_A1:
|
|||
|
|
raise ValueError("not A1")
|
|||
|
|
temperature = struct.unpack("<b", data8[1:2])[0]
|
|||
|
|
iq_or_power = struct.unpack("<h", data8[2:4])[0]
|
|||
|
|
speed_dps = struct.unpack("<h", data8[4:6])[0]
|
|||
|
|
encoder = struct.unpack("<H", data8[6:8])[0]
|
|||
|
|
return dict(
|
|||
|
|
temperature_C=temperature,
|
|||
|
|
iq_or_power_raw=iq_or_power,
|
|||
|
|
speed_dps=speed_dps,
|
|||
|
|
encoder=encoder,
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def dump_frame(tag: str, mid: int, fr) -> str:
|
|||
|
|
if fr.t <= 0:
|
|||
|
|
return f"{tag} M{mid}: (no reply yet)"
|
|||
|
|
d = fr.data
|
|||
|
|
arb = fr.arb
|
|||
|
|
cmd = d[0]
|
|||
|
|
if cmd == CMD_A1:
|
|||
|
|
s = parse_a1(d)
|
|||
|
|
return (f"{tag} M{mid}: arb=0x{arb:X} "
|
|||
|
|
f"T={s['temperature_C']}C iq/p={s['iq_or_power_raw']} "
|
|||
|
|
f"spd={s['speed_dps']}dps enc={s['encoder']}")
|
|||
|
|
return f"{tag} M{mid}: arb=0x{arb:X} cmd=0x{cmd:02X} raw={[hex(x) for x in d]}"
|
|||
|
|
|
|||
|
|
|
|||
|
|
@dataclass
|
|||
|
|
class RxFrame:
|
|||
|
|
t: float = 0.0
|
|||
|
|
arb: int = 0
|
|||
|
|
data: bytes = b""
|
|||
|
|
|
|||
|
|
|
|||
|
|
class RxCache4Ch(can.Listener):
|
|||
|
|
"""
|
|||
|
|
一个 Notifier 监听一个 “设备Bus(包含两个channel)”，按 msg.channel 分流到 ch0/ch1 的缓存。
|
|||
|
|
"""
|
|||
|
|
def __init__(self, ch_to_motor_ids: Dict[int, List[int]]):
|
|||
|
|
super().__init__()
|
|||
|
|
self.ch_to_motor_ids = {int(ch): set(mids) for ch, mids in ch_to_motor_ids.items()}
|
|||
|
|
self._lock = threading.Lock()
|
|||
|
|
self.latest: Dict[Tuple[int, int], RxFrame] = {}
|
|||
|
|
for ch, mids in self.ch_to_motor_ids.items():
|
|||
|
|
for mid in mids:
|
|||
|
|
self.latest[(ch, mid)] = RxFrame()
|
|||
|
|
|
|||
|
|
def on_message_received(self, msg: can.Message):
|
|||
|
|
# python-can Message.channel：对 canalystii 多通道会填 0/1
|
|||
|
|
ch = getattr(msg, "channel", None)
|
|||
|
|
if ch is None:
|
|||
|
|
# 没有 channel 信息就无法分流，直接忽略或当作 ch0
|
|||
|
|
ch = 0
|
|||
|
|
try:
|
|||
|
|
ch = int(ch)
|
|||
|
|
except Exception:
|
|||
|
|
ch = 0
|
|||
|
|
|
|||
|
|
if ch not in self.ch_to_motor_ids:
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
arb = msg.arbitration_id
|
|||
|
|
if arb < 0x141 or arb > 0x160:
|
|||
|
|
return
|
|||
|
|
mid = arb - 0x140
|
|||
|
|
if mid not in self.ch_to_motor_ids[ch]:
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
with self._lock:
|
|||
|
|
self.latest[(ch, mid)] = RxFrame(time.time(), arb, bytes(msg.data))
|
|||
|
|
|
|||
|
|
def snapshot(self) -> Dict[Tuple[int, int], RxFrame]:
|
|||
|
|
with self._lock:
|
|||
|
|
return dict(self.latest)
|
|||
|
|
|
|||
|
|
|
|||
|
|
class SenderThread(threading.Thread):
|
|||
|
|
"""
|
|||
|
|
每个“逻辑channel(共4个)”一个发送线程：
|
|||
|
|
- 等主线程 tick_event
|
|||
|
|
- barrier 同步放行
|
|||
|
|
- 加 bus_lock 后 send（同一个设备的两个channel共享一个bus对象，必须加锁）
|
|||
|
|
"""
|
|||
|
|
def __init__(
|
|||
|
|
self,
|
|||
|
|
name: str,
|
|||
|
|
bus: can.BusABC,
|
|||
|
|
bus_lock: threading.Lock,
|
|||
|
|
channel_index: int, # 0 or 1
|
|||
|
|
tick_event: threading.Event,
|
|||
|
|
barrier: threading.Barrier,
|
|||
|
|
stop_event: threading.Event,
|
|||
|
|
):
|
|||
|
|
super().__init__(name=name)
|
|||
|
|
self.bus = bus
|
|||
|
|
self.bus_lock = bus_lock
|
|||
|
|
self.channel_index = int(channel_index)
|
|||
|
|
self.tick_event = tick_event
|
|||
|
|
self.barrier = barrier
|
|||
|
|
self.stop_event = stop_event
|
|||
|
|
self.torque = (0, 0, 0, 0)
|
|||
|
|
|
|||
|
|
def run(self):
|
|||
|
|
while not self.stop_event.is_set():
|
|||
|
|
if not self.tick_event.wait(timeout=0.5):
|
|||
|
|
continue
|
|||
|
|
if self.stop_event.is_set():
|
|||
|
|
break
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
self.barrier.wait(timeout=0.05)
|
|||
|
|
except threading.BrokenBarrierError:
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
t1, t2, t3, t4 = self.torque
|
|||
|
|
msg = build_torque_broadcast(t1, t2, t3, t4)
|
|||
|
|
msg.channel = self.channel_index # ★ 指定发到该设备的哪个channel
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
with self.bus_lock:
|
|||
|
|
self.bus.send(msg)
|
|||
|
|
except can.CanError:
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
# 等主线程清 tick_event，避免同tick重复发
|
|||
|
|
while self.tick_event.is_set() and not self.stop_event.is_set():
|
|||
|
|
time.sleep(0)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def open_device_bus(device_idx: int, bitrate: int) -> can.BusABC:
|
|||
|
|
"""
|
|||
|
|
每块 CANalyst-II 只打开一次 Bus，channel=[0,1]
|
|||
|
|
"""
|
|||
|
|
# canalystii 后端一般支持 device=0/1；如果你环境不支持会 TypeError，这里做兼容
|
|||
|
|
try:
|
|||
|
|
return can.Bus(interface="canalystii", channel=[0, 1], bitrate=bitrate, device=device_idx)
|
|||
|
|
except TypeError:
|
|||
|
|
# 某些版本 canalystii 不暴露 device 参数（只能看到一块设备）
|
|||
|
|
return can.Bus(interface="canalystii", channel=[0, 1], bitrate=bitrate)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def main():
|
|||
|
|
bitrate = 1000000
|
|||
|
|
hz = 500
|
|||
|
|
dt = 1.0 / hz
|
|||
|
|
|
|||
|
|
# ========= 你需要按实际接线修改：每个“设备-channel”上有哪些电机ID =========
|
|||
|
|
# 逻辑4路：dev0-ch0, dev0-ch1, dev1-ch0, dev1-ch1
|
|||
|
|
motor_map = {
|
|||
|
|
("dev0", 0): [1, 2, 3, 4],
|
|||
|
|
("dev0", 1): [1, 2, 3, 4],
|
|||
|
|
("dev1", 0): [1, 2, 3, 4],
|
|||
|
|
("dev1", 1): [1, 2, 3, 4],
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
# ========= 打开两块设备（每块一次性打开2路channel） =========
|
|||
|
|
bus0 = open_device_bus(device_idx=0, bitrate=bitrate)
|
|||
|
|
bus1 = open_device_bus(device_idx=1, bitrate=bitrate)
|
|||
|
|
|
|||
|
|
# 过滤：只收我们关心的 arb_id（两设备相同过滤即可）
|
|||
|
|
def set_filters_for(bus: can.BusABC, all_motor_ids: List[int]):
|
|||
|
|
try:
|
|||
|
|
bus.set_filters([{"can_id": _arb_id_single(i), "can_mask": 0x7FF, "extended": False} for i in all_motor_ids])
|
|||
|
|
except Exception:
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
all_ids_dev0 = sorted(set(motor_map[("dev0", 0)] + motor_map[("dev0", 1)]))
|
|||
|
|
all_ids_dev1 = sorted(set(motor_map[("dev1", 0)] + motor_map[("dev1", 1)]))
|
|||
|
|
set_filters_for(bus0, all_ids_dev0)
|
|||
|
|
set_filters_for(bus1, all_ids_dev1)
|
|||
|
|
|
|||
|
|
# ========= 接收缓存：每个设备一个 Notifier，一个 cache（按 channel 分流） =========
|
|||
|
|
cache0 = RxCache4Ch({0: motor_map[("dev0", 0)], 1: motor_map[("dev0", 1)]})
|
|||
|
|
cache1 = RxCache4Ch({0: motor_map[("dev1", 0)], 1: motor_map[("dev1", 1)]})
|
|||
|
|
notifier0 = can.Notifier(bus0, [cache0], timeout=0.01)
|
|||
|
|
notifier1 = can.Notifier(bus1, [cache1], timeout=0.01)
|
|||
|
|
|
|||
|
|
# ========= 发送线程：4个逻辑channel =========
|
|||
|
|
stop_event = threading.Event()
|
|||
|
|
tick_event = threading.Event()
|
|||
|
|
barrier = threading.Barrier(4 + 1) # 4个sender + 主线程
|
|||
|
|
|
|||
|
|
bus0_lock = threading.Lock()
|
|||
|
|
bus1_lock = threading.Lock()
|
|||
|
|
|
|||
|
|
sender_dev0_ch0 = SenderThread("sender-dev0-ch0", bus0, bus0_lock, 0, tick_event, barrier, stop_event)
|
|||
|
|
sender_dev0_ch1 = SenderThread("sender-dev0-ch1", bus0, bus0_lock, 1, tick_event, barrier, stop_event)
|
|||
|
|
sender_dev1_ch0 = SenderThread("sender-dev1-ch0", bus1, bus1_lock, 0, tick_event, barrier, stop_event)
|
|||
|
|
sender_dev1_ch1 = SenderThread("sender-dev1-ch1", bus1, bus1_lock, 1, tick_event, barrier, stop_event)
|
|||
|
|
|
|||
|
|
senders = [sender_dev0_ch0, sender_dev0_ch1, sender_dev1_ch0, sender_dev1_ch1]
|
|||
|
|
for th in senders:
|
|||
|
|
th.start()
|
|||
|
|
|
|||
|
|
# ========= 你的控制输出：这里演示固定 torque =========
|
|||
|
|
def torque_for(tag: str, ch: int) -> Tuple[int, int, int, int]:
|
|||
|
|
# TODO：替换为你的控制算法输出
|
|||
|
|
return (30, 30, 30, 30)
|
|||
|
|
|
|||
|
|
k = 0
|
|||
|
|
next_tick = time.perf_counter()
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
while True:
|
|||
|
|
# 1) 写入4路 torque（主线程写共享变量）
|
|||
|
|
sender_dev0_ch0.torque = torque_for("dev0", 0)
|
|||
|
|
sender_dev0_ch1.torque = torque_for("dev0", 1)
|
|||
|
|
sender_dev1_ch0.torque = torque_for("dev1", 0)
|
|||
|
|
sender_dev1_ch1.torque = torque_for("dev1", 1)
|
|||
|
|
|
|||
|
|
# 2) 同步tick发布 + barrier放行（尽量同时发送）
|
|||
|
|
tick_event.set()
|
|||
|
|
try:
|
|||
|
|
barrier.wait(timeout=0.05)
|
|||
|
|
except threading.BrokenBarrierError:
|
|||
|
|
pass
|
|||
|
|
tick_event.clear()
|
|||
|
|
|
|||
|
|
# 3) 打印（每100ms一次）
|
|||
|
|
if k % int(hz * 0.1) == 0:
|
|||
|
|
print(f"\n==== tick {k} t={time.time():.3f} ====")
|
|||
|
|
|
|||
|
|
snap0 = cache0.snapshot()
|
|||
|
|
print("[dev0-ch0]")
|
|||
|
|
for mid in motor_map[("dev0", 0)]:
|
|||
|
|
print(" " + dump_frame("[dev0-ch0]", mid, snap0[(0, mid)]))
|
|||
|
|
print("[dev0-ch1]")
|
|||
|
|
for mid in motor_map[("dev0", 1)]:
|
|||
|
|
print(" " + dump_frame("[dev0-ch1]", mid, snap0[(1, mid)]))
|
|||
|
|
|
|||
|
|
snap1 = cache1.snapshot()
|
|||
|
|
print("[dev1-ch0]")
|
|||
|
|
for mid in motor_map[("dev1", 0)]:
|
|||
|
|
print(" " + dump_frame("[dev1-ch0]", mid, snap1[(0, mid)]))
|
|||
|
|
print("[dev1-ch1]")
|
|||
|
|
for mid in motor_map[("dev1", 1)]:
|
|||
|
|
print(" " + dump_frame("[dev1-ch1]", mid, snap1[(1, mid)]))
|
|||
|
|
|
|||
|
|
k += 1
|
|||
|
|
|
|||
|
|
# 4) 500Hz 固定周期（绝对时间推进）
|
|||
|
|
next_tick += dt
|
|||
|
|
now = time.perf_counter()
|
|||
|
|
if next_tick > now:
|
|||
|
|
time.sleep(next_tick - now)
|
|||
|
|
else:
|
|||
|
|
next_tick = now
|
|||
|
|
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
pass
|
|||
|
|
finally:
|
|||
|
|
# 停止发送线程
|
|||
|
|
stop_event.set()
|
|||
|
|
tick_event.set()
|
|||
|
|
for th in senders:
|
|||
|
|
th.join(timeout=0.5)
|
|||
|
|
|
|||
|
|
# 停止 Notifier（避免你之前那种 interpreter shutdown 崩溃）
|
|||
|
|
try:
|
|||
|
|
notifier0.stop()
|
|||
|
|
notifier1.stop()
|
|||
|
|
except Exception:
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
# 安全清零力矩（给每个设备两个channel都发一次）
|
|||
|
|
try:
|
|||
|
|
for ch in [0, 1]:
|
|||
|
|
m = build_torque_broadcast(0, 0, 0, 0)
|
|||
|
|
m.channel = ch
|
|||
|
|
with bus0_lock:
|
|||
|
|
bus0.send(m)
|
|||
|
|
with bus1_lock:
|
|||
|
|
bus1.send(m)
|
|||
|
|
except Exception:
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
bus0.shutdown()
|
|||
|
|
bus1.shutdown()
|
|||
|
|
except Exception:
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
main()
|