1133 lines
44 KiB
Python
1133 lines
44 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""N2/N3 对接电机诊断发令（ECAT）。
|
||
|
||
前提：用户保证机构处于安全状态（无障碍、可运动）。
|
||
默认参数偏保守：span=200°，vel=80°/s，行程 hold 短，全程可 Ctrl+C 急停。
|
||
|
||
子命令:
|
||
status 只读：链路 / 状态 / CONFIG
|
||
align 下发统一 CONFIG（mode/limitV/PID）+ 双机 CMD
|
||
openloop 开环速度对比：双机同 v，采 速度/电流/积分
|
||
stop 巡航后同时刹停，测停车滞后与 ∫(v2+v3)
|
||
trip 位置行程（mode3）：同向或镜像，采全程积分
|
||
full 依次跑 align → openloop → stop → trip，并汇总 JSON
|
||
stopnow 立即发速度=0（急停）
|
||
|
||
示例:
|
||
python scripts/test_motor_pair_n2_n3.py status
|
||
python scripts/test_motor_pair_n2_n3.py align --limit-v 22.0 --pid 100
|
||
python scripts/test_motor_pair_n2_n3.py stop --vel 80 --cruise 1.2
|
||
python scripts/test_motor_pair_n2_n3.py full --mode-mirror --span 200
|
||
python scripts/test_motor_pair_n2_n3.py stopnow
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import csv
|
||
import http.client
|
||
import json
|
||
import os
|
||
import struct
|
||
import sys
|
||
import threading
|
||
import time
|
||
from collections import defaultdict
|
||
|
||
HOST, PORT = "127.0.0.1", 8090
|
||
NODES = (2, 3)
|
||
LAB = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "lab")
|
||
PERIOD = 0.25 # ms，以 motor/state 为准
|
||
|
||
_tls = threading.local()
|
||
_lock = threading.Lock()
|
||
_stop_flag = threading.Event()
|
||
_rows: list = []
|
||
_seen: dict = {}
|
||
|
||
|
||
# ───────────────────────── HTTP / ECAT ─────────────────────────
|
||
|
||
def post(path: str, body: bytes | None = None, timeout: float = 8.0) -> dict:
|
||
if body is None:
|
||
body = b"{}"
|
||
c = getattr(_tls, "c", None)
|
||
if c is None:
|
||
c = _tls.c = http.client.HTTPConnection(HOST, PORT, timeout=timeout)
|
||
for _ in range(2):
|
||
try:
|
||
c.request("POST", path, body=body,
|
||
headers={"Content-Type": "application/json"})
|
||
return json.loads(c.getresponse().read().decode("utf-8", "replace"))
|
||
except Exception:
|
||
try:
|
||
c.close()
|
||
except Exception:
|
||
pass
|
||
_tls.c = c = http.client.HTTPConnection(HOST, PORT, timeout=timeout)
|
||
raise RuntimeError(f"POST {path} failed")
|
||
|
||
|
||
def ensure_ecat() -> tuple[float, dict]:
|
||
st = post("/api/motor/state")
|
||
period = st.get("period_ms") or PERIOD
|
||
wkc = str(st.get("wkc") or "")
|
||
motors = st.get("motors") or []
|
||
if wkc not in ("3", "3/3") or len(motors) < 2:
|
||
print("ECAT 未就绪 wkc=%s motors=%d → motor/start" % (wkc, len(motors)))
|
||
r = post("/api/motor/start",
|
||
json.dumps({"period_ms": 0.25, "dc_cycle_us": 250}).encode(),
|
||
timeout=35.0)
|
||
print(" start:", json.dumps(r, ensure_ascii=False)[:240])
|
||
time.sleep(0.4)
|
||
st = post("/api/motor/state")
|
||
period = st.get("period_ms") or PERIOD
|
||
return period, st
|
||
|
||
|
||
# ───────────────────────── CMD 编码 ─────────────────────────
|
||
|
||
# ───────────────── 安全：已机械对接，硬联锁 ─────────────────
|
||
# 规则（不可绕过，除非命令行 --i-understand-coupled-risk 且仍受硬限位约束）：
|
||
# 1) 只允许 mode3 + 有限角度目标，两轴目标必须互为相反数（镜像）。
|
||
# 2) 禁止 mode2 开环持续转（对接后会一直拧）。
|
||
# 3) 速度只表示「走向目标的速率」，符号必须 = sign(target - current)。
|
||
# 4) 发令前若 |a2+a3| 过大（错位）→ 拒绝运动，要求人工回零。
|
||
# 5) 硬限位：|ang|、目标、单次行程超限 → 拒绝。
|
||
# 6) 运行中看门狗：一台已停（|v|<8）另一台仍 |v|>40 超过 300ms → 双机急停。
|
||
FORCE_MIRROR = True
|
||
MAX_ABS_ANGLE = 2500.0 # 编码器绝对角限（多圈保护）
|
||
MAX_STROKE = 1200.0 # 单次目标相对当前位置最大行程
|
||
MAX_ASUM_ERR = 40.0 # |a2+a3| 超过则判定错位，禁止运动
|
||
STOP_VEL_EPS = 8.0
|
||
SPIN_VEL_WARN = 40.0
|
||
ASYMMETRY_MS = 300.0
|
||
|
||
|
||
def _read_pair():
|
||
snap = snap_motors()
|
||
a2 = (snap.get(2) or {}).get("ang")
|
||
v2 = (snap.get(2) or {}).get("vel")
|
||
a3 = (snap.get(3) or {}).get("ang")
|
||
v3 = (snap.get(3) or {}).get("vel")
|
||
return a2, v2, a3, v3, snap
|
||
|
||
|
||
def safety_precheck_motion(target2, target3, speed, force=False):
|
||
"""运动前硬检查。返回 (ok, msg, items)。items 为带符号速度的 CMD。"""
|
||
a2, v2, a3, v3, snap = _read_pair()
|
||
if a2 is None or a3 is None:
|
||
return False, "N2/N3 状态缺失，拒绝运动", None
|
||
if abs(a2) > MAX_ABS_ANGLE or abs(a3) > MAX_ABS_ANGLE:
|
||
return False, ("角度超硬限位 a2=%.1f a3=%.1f（限±%.0f）——请人工回位"
|
||
% (a2, a3, MAX_ABS_ANGLE)), None
|
||
if abs(target2) > MAX_ABS_ANGLE or abs(target3) > MAX_ABS_ANGLE:
|
||
return False, "目标超硬限位", None
|
||
if abs(target2 + target3) > 1.0:
|
||
return False, "目标必须镜像：target2+target3=0（得到 %s/%s）" % (target2, target3), None
|
||
if abs(target2 - a2) > MAX_STROKE or abs(target3 - a3) > MAX_STROKE:
|
||
return False, ("单次行程过大 Δ2=%.1f Δ3=%.1f（限±%.0f）"
|
||
% (target2 - a2, target3 - a3, MAX_STROKE)), None
|
||
asum = a2 + a3
|
||
if abs(asum) > MAX_ASUM_ERR and not force:
|
||
return False, ("错位过大 a2+a3=%.1f°（a2=%.1f a3=%.1f）。"
|
||
"禁止电控继续跑，请先人工把机构回到对称位（建议 a2≈-a3，和≈0）再测。"
|
||
% (asum, a2, a3)), None
|
||
# 方向：必须朝向各自目标
|
||
v2s = 0.0 if abs(target2 - a2) < 0.5 else (speed if target2 > a2 else -speed)
|
||
v3s = 0.0 if abs(target3 - a3) < 0.5 else (speed if target3 > a3 else -speed)
|
||
# 对接刚性镜像：位移方向应相反。若当前接近对称却要求同向位移 → 拒绝
|
||
if abs(asum) <= MAX_ASUM_ERR:
|
||
d2, d3 = target2 - a2, target3 - a3
|
||
if abs(d2) > 1 and abs(d3) > 1 and d2 * d3 > 0:
|
||
return False, ("拒绝：镜像机构不允许同向位移 Δ2=%.1f Δ3=%.1f"
|
||
% (d2, d3)), None
|
||
# 速度也应反号
|
||
if abs(v2s) > 0.5 and abs(v3s) > 0.5 and v2s * v3s > 0:
|
||
return False, "拒绝：速度同号 v2=%s v3=%s" % (v2s, v3s), None
|
||
items = [(2, float(v2s), float(target2)), (3, float(v3s), float(target3))]
|
||
return True, ("precheck ok a2=%.2f a3=%.2f asum=%.2f cmd=%s"
|
||
% (a2, a3, asum, items)), items
|
||
|
||
|
||
def _check_pair(items, allow_same=False):
|
||
"""双机指令必须：角度目标镜像；速度与目标方向一致且反号。"""
|
||
if not FORCE_MIRROR:
|
||
return
|
||
d = {int(n): (float(v), float(a)) for n, v, a in items}
|
||
if 2 not in d or 3 not in d:
|
||
return
|
||
v2, t2 = d[2]
|
||
v3, t3 = d[3]
|
||
if abs(t2 + t3) > 1.0 and (abs(t2) > 1 or abs(t3) > 1):
|
||
raise RuntimeError("SAFETY: 角度目标必须镜像 t2=%s t3=%s" % (t2, t3))
|
||
if abs(v2) > 0.5 and abs(v3) > 0.5 and v2 * v3 > 0:
|
||
raise RuntimeError("SAFETY: 速度禁止同号 v2=%s v3=%s（会拧线）" % (v2, v3))
|
||
a2, cur_v2, a3, cur_v3, _ = _read_pair()
|
||
if a2 is not None and a3 is not None:
|
||
if abs(a2) > MAX_ABS_ANGLE or abs(a3) > MAX_ABS_ANGLE:
|
||
raise RuntimeError("SAFETY: 编码器超限 a2=%.1f a3=%.1f" % (a2, a3))
|
||
if abs(v2) > 0.5 and abs(t2 - a2) > 0.5:
|
||
if v2 * (t2 - a2) < 0:
|
||
raise RuntimeError("SAFETY: N2 速度方向与目标相反 v=%s a=%.1f t=%.1f"
|
||
% (v2, a2, t2))
|
||
if abs(v3) > 0.5 and abs(t3 - a3) > 0.5:
|
||
if v3 * (t3 - a3) < 0:
|
||
raise RuntimeError("SAFETY: N3 速度方向与目标相反 v=%s a=%.1f t=%.1f"
|
||
% (v3, a3, t3))
|
||
|
||
|
||
def _watchdog_stop(reason):
|
||
print("!! WATCHDOG STOP:", reason)
|
||
for _ in range(6):
|
||
try:
|
||
post("/api/motor/send", json.dumps({"frames": [
|
||
{"ch": 2, "id": hex((2 << 16) | 0x0200), "dlc": 32,
|
||
"data": pack_cmd(mode=3, voltage=24, velocity=0,
|
||
angle=(_read_pair()[0] or 0), pid=(100, 100, 100))},
|
||
{"ch": 2, "id": hex((3 << 16) | 0x0200), "dlc": 32,
|
||
"data": pack_cmd(mode=3, voltage=24, velocity=0,
|
||
angle=(_read_pair()[2] or 0), pid=(100, 100, 100))},
|
||
]}).encode())
|
||
except Exception:
|
||
pass
|
||
time.sleep(0.03)
|
||
a2, v2, a3, v3, _ = _read_pair()
|
||
print(" after stop a2=%s v2=%s a3=%s v3=%s" % (a2, v2, a3, v3))
|
||
|
||
|
||
def pack_cmd(mode=3, voltage=24.0, current=0.0, velocity=100.0,
|
||
angle=0.0, pid=(100, 100, 100)) -> str:
|
||
"""与 pages/motor.html buildCmdBytes 一致（DLC32）。"""
|
||
b = bytearray(32)
|
||
b[0] = int(mode) & 0xFF
|
||
struct.pack_into("<H", b, 1, int(round(voltage * 1000)) & 0xFFFF)
|
||
struct.pack_into("<f", b, 3, float(current))
|
||
struct.pack_into("<f", b, 7, float(velocity))
|
||
struct.pack_into("<f", b, 11, float(angle))
|
||
struct.pack_into("<h", b, 15, int(pid[0]))
|
||
struct.pack_into("<h", b, 17, int(pid[1]))
|
||
struct.pack_into("<h", b, 19, int(pid[2]))
|
||
return bytes(b).hex()
|
||
|
||
|
||
def pack_config(mode=3, limit_v=22.0, current=0.0, velocity=100.0,
|
||
angle=0.0) -> str:
|
||
"""CONFIG 帧与 CMD 同布局（回声/写参数用）。"""
|
||
return pack_cmd(mode=mode, voltage=limit_v, current=current,
|
||
velocity=velocity, angle=angle, pid=(0, 0, 0))
|
||
|
||
|
||
def burst_cmds(items, mode=3, voltage=24.0, pid=(100, 100, 100),
|
||
allow_same_dir=False):
|
||
"""items: [(node, velocity, angle)] 一次 POST 同一 seq 沿。"""
|
||
_check_pair(items, allow_same=allow_same_dir)
|
||
frames = []
|
||
for n, vel, ang in items:
|
||
frames.append({
|
||
"ch": 2,
|
||
"id": hex((int(n) << 16) | 0x0200),
|
||
"dlc": 32,
|
||
"data": pack_cmd(mode=mode, voltage=voltage, velocity=vel,
|
||
angle=ang, pid=pid),
|
||
})
|
||
return post("/api/motor/send", json.dumps({"frames": frames}).encode())
|
||
|
||
|
||
def send_config(cfg: dict, nodes=NODES):
|
||
"""下发统一 CONFIG。cfg: {mode, limit_v, velocity, angle}"""
|
||
frames = []
|
||
for n in nodes:
|
||
frames.append({
|
||
"ch": 2,
|
||
"id": hex((int(n) << 16) | 0x0200),
|
||
"dlc": 32,
|
||
"data": pack_config(mode=cfg.get("mode", 3),
|
||
limit_v=cfg.get("limit_v", 22.0),
|
||
velocity=cfg.get("velocity", 100.0),
|
||
angle=cfg.get("angle", 0.0)),
|
||
})
|
||
return post("/api/motor/send", json.dumps({"frames": frames}).encode())
|
||
|
||
|
||
# ───────────────────────── 状态 ─────────────────────────
|
||
|
||
def decode_status(raw: bytes):
|
||
if len(raw) < 21:
|
||
return None
|
||
ang = struct.unpack_from("<f", raw, 13)[0]
|
||
vel = struct.unpack_from("<f", raw, 17)[0]
|
||
if ang != ang or vel != vel:
|
||
return None
|
||
d = {
|
||
"ang": ang,
|
||
"vel": vel,
|
||
"pwr": struct.unpack_from("<H", raw, 11)[0] * 0.001 if len(raw) >= 13 else None,
|
||
"ucur": struct.unpack_from("<h", raw, 4)[0] if len(raw) >= 6 else None,
|
||
"rt_cur": struct.unpack_from("<f", raw, 21)[0] if len(raw) >= 25 else 0.0,
|
||
"motT": struct.unpack_from("<h", raw, 2)[0] * 0.01 if len(raw) >= 4 else None,
|
||
"b10": raw[10] if len(raw) > 10 else 0,
|
||
}
|
||
return d
|
||
|
||
|
||
def snap_motors() -> dict:
|
||
out = {}
|
||
st = post("/api/motor/state")
|
||
for m in st.get("motors") or []:
|
||
n = m.get("node")
|
||
if n not in NODES:
|
||
continue
|
||
dec = decode_status(bytes.fromhex(m.get("data") or ""))
|
||
if dec:
|
||
dec["online"] = bool(m.get("online"))
|
||
dec["seq"] = m.get("seq")
|
||
out[n] = dec
|
||
out["_cycles"] = st.get("cycles")
|
||
out["_period_ms"] = st.get("period_ms")
|
||
return out
|
||
|
||
|
||
def read_configs_from_rx() -> dict:
|
||
cfgs = {}
|
||
try:
|
||
rx = post("/api/motor/rx")
|
||
except Exception:
|
||
return cfgs
|
||
for f in (rx.get("frames") or rx.get("parsed") or []):
|
||
if not isinstance(f, dict):
|
||
continue
|
||
fid = f.get("id", 0)
|
||
try:
|
||
if isinstance(fid, str):
|
||
fid = int(fid, 0)
|
||
except Exception:
|
||
continue
|
||
if (fid & 0xFFFF) != 0x0101:
|
||
continue
|
||
n = (fid >> 16) & 0xFF
|
||
if n not in NODES:
|
||
continue
|
||
try:
|
||
raw = bytes.fromhex(f.get("data") or "")
|
||
except Exception:
|
||
continue
|
||
if len(raw) >= 21:
|
||
cfgs[n] = {
|
||
"mode": raw[0],
|
||
"limit_V": struct.unpack_from("<H", raw, 1)[0] * 0.001,
|
||
"cur": struct.unpack_from("<f", raw, 3)[0],
|
||
"vel": struct.unpack_from("<f", raw, 7)[0],
|
||
"ang": struct.unpack_from("<f", raw, 11)[0],
|
||
"pid": [struct.unpack_from("<h", raw, o)[0] for o in (15, 17, 19)],
|
||
"hex": raw[:24].hex(),
|
||
}
|
||
return cfgs
|
||
|
||
|
||
# ───────────────────────── 采集 ─────────────────────────
|
||
|
||
def poller():
|
||
while not _stop_flag.is_set():
|
||
try:
|
||
d = post("/api/motor/state")
|
||
except Exception:
|
||
time.sleep(0.008)
|
||
continue
|
||
cyc = d.get("cycles", 0) or 0
|
||
wall = time.time()
|
||
for m in d.get("motors") or []:
|
||
n = m.get("node")
|
||
if n not in NODES:
|
||
continue
|
||
sq = m.get("seq", 0) or 0
|
||
with _lock:
|
||
if _seen.get(n) == sq:
|
||
continue
|
||
_seen[n] = sq
|
||
dec = decode_status(bytes.fromhex(m.get("data") or ""))
|
||
if not dec:
|
||
continue
|
||
with _lock:
|
||
_rows.append({
|
||
"wall": wall, "cyc": cyc, "node": n, "seq": sq,
|
||
"ang": dec["ang"], "vel": dec["vel"],
|
||
"rt_cur": dec["rt_cur"], "motT": dec["motT"],
|
||
})
|
||
|
||
|
||
def start_poll(n=6):
|
||
del _rows[:]
|
||
_seen.clear()
|
||
_stop_flag.clear()
|
||
ths = [threading.Thread(target=poller, daemon=True) for _ in range(n)]
|
||
for t in ths:
|
||
t.start()
|
||
return ths
|
||
|
||
|
||
def stop_poll(ths):
|
||
_stop_flag.set()
|
||
for t in ths:
|
||
t.join(timeout=2)
|
||
|
||
|
||
def aligned_series():
|
||
"""返回 [(cyc,t,a2,v2,a3,v3,vsum,asum)] 以 N2 cycles 对齐。"""
|
||
by = defaultdict(list)
|
||
for r in _rows:
|
||
by[r["node"]].append(r)
|
||
for n in by:
|
||
by[n].sort(key=lambda x: x["cyc"])
|
||
s2, s3 = by.get(2, []), by.get(3, [])
|
||
if not s2 or not s3:
|
||
return []
|
||
|
||
def interp(n, cyc):
|
||
s = by[n]
|
||
if cyc <= s[0]["cyc"]:
|
||
return s[0]["ang"], s[0]["vel"]
|
||
if cyc >= s[-1]["cyc"]:
|
||
return s[-1]["ang"], s[-1]["vel"]
|
||
lo, hi = 0, len(s) - 1
|
||
while lo + 1 < hi:
|
||
mid = (lo + hi) // 2
|
||
if s[mid]["cyc"] <= cyc:
|
||
lo = mid
|
||
else:
|
||
hi = mid
|
||
c0, a0, v0 = s[lo]["cyc"], s[lo]["ang"], s[lo]["vel"]
|
||
c1, a1, v1 = s[hi]["cyc"], s[hi]["ang"], s[hi]["vel"]
|
||
if c1 == c0:
|
||
return a1, v1
|
||
f = (cyc - c0) / (c1 - c0)
|
||
return a0 + (a1 - a0) * f, v0 + (v1 - v0) * f
|
||
|
||
out = []
|
||
for r in s2:
|
||
a3, v3 = interp(3, r["cyc"])
|
||
out.append((r["cyc"], r["wall"], r["ang"], r["vel"], a3, v3,
|
||
r["vel"] + v3, r["ang"] + a3, r["rt_cur"]))
|
||
return out
|
||
|
||
|
||
def trapz_vel(series, period_ms, key_v2=3, key_v3=5):
|
||
"""series 对齐列表 → (I2, I3, Isum) 全程梯形积分。"""
|
||
I2 = I3 = 0.0
|
||
for i in range(1, len(series)):
|
||
dt = (series[i][0] - series[i - 1][0]) * period_ms / 1000.0
|
||
if dt < 0:
|
||
dt = 0
|
||
I2 += 0.5 * (series[i][key_v2] + series[i - 1][key_v2]) * dt
|
||
I3 += 0.5 * (series[i][key_v3] + series[i - 1][key_v3]) * dt
|
||
return I2, I3, I2 + I3
|
||
|
||
|
||
def write_series_csv(path, series, meta=None):
|
||
with open(path, "w", newline="", encoding="utf-8") as fp:
|
||
w = csv.writer(fp)
|
||
w.writerow(["cycles", "wall", "a2", "v2", "a3", "v3", "vsum", "asum", "rt_cur2"])
|
||
for row in series:
|
||
w.writerow(row)
|
||
if meta:
|
||
fp.write("# meta " + json.dumps(meta, ensure_ascii=False) + "\n")
|
||
|
||
|
||
def analyze_pair(series, period_ms, label=""):
|
||
if len(series) < 40:
|
||
return {"label": label, "error": "too_few_samples", "n": len(series)}
|
||
mov = [r for r in series if abs(r[3]) > 15 or abs(r[5]) > 15]
|
||
cr = [r for r in series if abs(r[3]) > 80 and abs(r[5]) > 80]
|
||
I2, I3, Isum = trapz_vel(series, period_ms)
|
||
out = {
|
||
"label": label,
|
||
"n": len(series),
|
||
"I2": I2, "I3": I3, "I_sum_v2_plus_v3": Isum,
|
||
"asum_start_end": [series[0][7], series[-1][7]],
|
||
"asum_delta": series[-1][7] - series[0][7],
|
||
"a2_start_end": [series[0][2], series[-1][2]],
|
||
"a3_start_end": [series[0][4], series[-1][4]],
|
||
}
|
||
if cr:
|
||
out["cruise_mean_abs_v2"] = sum(abs(r[3]) for r in cr) / len(cr)
|
||
out["cruise_mean_abs_v3"] = sum(abs(r[5]) for r in cr) / len(cr)
|
||
out["cruise_mean_vsum"] = sum(r[6] for r in cr) / len(cr)
|
||
out["cruise_rms_vsum"] = (sum(r[6] ** 2 for r in cr) / len(cr)) ** 0.5
|
||
if mov:
|
||
out["mov_mean_abs_v2"] = sum(abs(r[3]) for r in mov) / len(mov)
|
||
out["mov_mean_abs_v3"] = sum(abs(r[5]) for r in mov) / len(mov)
|
||
out["mov_rms_vsum"] = (sum(r[6] ** 2 for r in mov) / len(mov)) ** 0.5
|
||
out["mov_max_abs_vsum"] = max(abs(r[6]) for r in mov)
|
||
return out
|
||
|
||
|
||
# ───────────────────────── 连续发令 ─────────────────────────
|
||
|
||
def drive(items, duration, rate=25.0, mode=3, voltage=24.0, pid=(100, 100, 100),
|
||
allow_same_dir=False):
|
||
"""连续发令 + 看门狗。已对接：仅 mode3 + 镜像角度目标 + 低速优先。"""
|
||
if mode != 3:
|
||
raise RuntimeError("SAFETY: 已对接仅允许 mode3，禁止 mode=%s" % mode)
|
||
_check_pair(items, allow_same=allow_same_dir)
|
||
t_end = time.perf_counter() + duration
|
||
dt = 1.0 / max(rate, 1.0)
|
||
n_ok = 0
|
||
last_err = None
|
||
spin_t0 = None
|
||
while time.perf_counter() < t_end:
|
||
t0 = time.perf_counter()
|
||
try:
|
||
burst_cmds(items, mode=mode, voltage=voltage, pid=pid,
|
||
allow_same_dir=allow_same_dir)
|
||
n_ok += 1
|
||
except Exception as e:
|
||
last_err = str(e)
|
||
print(" send err:", e)
|
||
if "SAFETY" in str(e):
|
||
_watchdog_stop(str(e))
|
||
break
|
||
try:
|
||
a2, v2, a3, v3, _ = _read_pair()
|
||
if v2 is not None and v3 is not None:
|
||
s2, s3 = abs(v2) < STOP_VEL_EPS, abs(v3) < STOP_VEL_EPS
|
||
spin = abs(v2) > SPIN_VEL_WARN or abs(v3) > SPIN_VEL_WARN
|
||
if (s2 != s3) and spin:
|
||
if spin_t0 is None:
|
||
spin_t0 = time.perf_counter()
|
||
elif (time.perf_counter() - spin_t0) * 1000.0 >= ASYMMETRY_MS:
|
||
_watchdog_stop("一台已停另一台仍在转 v2=%.1f v3=%.1f" % (v2, v3))
|
||
break
|
||
else:
|
||
spin_t0 = None
|
||
if a2 is not None and a3 is not None and (
|
||
abs(a2) > MAX_ABS_ANGLE or abs(a3) > MAX_ABS_ANGLE):
|
||
_watchdog_stop("角度超限 a2=%.1f a3=%.1f" % (a2, a3))
|
||
break
|
||
except Exception:
|
||
pass
|
||
pad = dt - (time.perf_counter() - t0)
|
||
if pad > 0:
|
||
time.sleep(pad)
|
||
return n_ok, last_err
|
||
|
||
|
||
def hold_position(pid=(100, 100, 100), voltage=24.0, rate=20.0, seconds=0.4,
|
||
targets=None):
|
||
snap = snap_motors()
|
||
if targets is None:
|
||
targets = []
|
||
for n in NODES:
|
||
if n in snap:
|
||
targets.append((n, 0.0, snap[n]["ang"]))
|
||
else:
|
||
targets.append((n, 0.0, 0.0))
|
||
return drive(targets, seconds, rate=rate, mode=3, voltage=voltage, pid=pid)
|
||
|
||
|
||
# ───────────────────────── 子命令 ─────────────────────────
|
||
|
||
def cmd_status(_args):
|
||
period, st = ensure_ecat()
|
||
print("链路 wkc=%s period=%sms cycles=%s motors=%s"
|
||
% (st.get("wkc"), period, st.get("cycles"),
|
||
[(m.get("node"), m.get("online"), m.get("seq"))
|
||
for m in st.get("motors") or []]))
|
||
print("\n--- live STATUS ---")
|
||
snap = snap_motors()
|
||
for n in NODES:
|
||
d = snap.get(n)
|
||
if not d:
|
||
continue
|
||
print(" N%s ang=%.3f vel=%.2f pwr=%s motT=%s b10=%s online=%s"
|
||
% (n, d["ang"], d["vel"], d["pwr"], d["motT"], d["b10"], d["online"]))
|
||
print("\n--- CONFIG from /api/motor/rx ---")
|
||
cfgs = read_configs_from_rx()
|
||
if not cfgs:
|
||
print(" (rx 暂无 0x0101 CONFIG，可先发一帧或等电机上报)")
|
||
for n, c in sorted(cfgs.items()):
|
||
print(" N%s %s" % (n, c))
|
||
return 0
|
||
|
||
|
||
def cmd_align(args):
|
||
period, st = ensure_ecat()
|
||
print("align: limitV=%.2f PID=%s mode=%s" % (args.limit_v, args.pid, args.mode))
|
||
print("before CONFIG:", json.dumps(read_configs_from_rx(), ensure_ascii=False))
|
||
r = send_config({"mode": args.mode, "limit_v": args.limit_v,
|
||
"velocity": args.vel, "angle": 0.0})
|
||
print("config send:", r)
|
||
time.sleep(0.15)
|
||
# 用统一参数发 CMD（速度 0，保持）
|
||
snap = snap_motors()
|
||
items = []
|
||
for n in NODES:
|
||
ang = snap.get(n, {}).get("ang", 0.0)
|
||
items.append((n, 0.0, ang))
|
||
for _ in range(int(args.hold * 25)):
|
||
burst_cmds(items, mode=args.mode, voltage=args.limit_v + 2,
|
||
pid=tuple(args.pid))
|
||
time.sleep(0.04)
|
||
print("after CONFIG:", json.dumps(read_configs_from_rx(), ensure_ascii=False))
|
||
print("live:", json.dumps(
|
||
{str(n): snap_motors().get(n) for n in NODES if snap_motors().get(n)},
|
||
ensure_ascii=False)[:400])
|
||
return 0
|
||
|
||
|
||
def cmd_openloop(args):
|
||
"""已对接：禁止 mode2 开环。只做短时 mode3 镜像微动（±30°）测响应。"""
|
||
period, _ = ensure_ecat()
|
||
print("SAFETY: 对接后禁止 open-loop mode2。改为 mode3 镜像小行程 ±20° @ vel≤50")
|
||
speed = min(abs(args.vel), 50.0)
|
||
span = 20.0
|
||
a2, _, a3, _, _ = _read_pair()
|
||
if a2 is None or a3 is None:
|
||
print("无状态")
|
||
return 2
|
||
# 在当前位置附近做镜像微动：目标 = 当前 ± 小量，并强制 target2=-target3
|
||
# 仅当当前已接近对称时允许
|
||
if abs(a2 + a3) > MAX_ASUM_ERR:
|
||
print("拒绝：a2+a3=%.1f 错位过大，先人工回位" % (a2 + a3))
|
||
return 2
|
||
t2 = span if a2 < 0 else -span
|
||
t3 = -t2
|
||
# 若当前位置离目标太近/太远则改用以 0 为中心
|
||
t2, t3 = span, -span
|
||
ok, msg, items = safety_precheck_motion(t2, t3, speed)
|
||
print(msg)
|
||
if not ok:
|
||
return 2
|
||
ths = start_poll(args.threads)
|
||
t0 = time.time()
|
||
drive(items, args.cruise, args.rate, mode=3, voltage=args.voltage,
|
||
pid=tuple(args.pid))
|
||
# 回到对称位附近 0/0 或停在当前位置
|
||
ok2, msg2, items2 = safety_precheck_motion(0.0, 0.0, speed)
|
||
print(msg2)
|
||
if ok2:
|
||
drive(items2, args.settle, args.rate, mode=3, voltage=args.voltage,
|
||
pid=tuple(args.pid))
|
||
t1 = time.time()
|
||
stop_poll(ths)
|
||
series = aligned_series()
|
||
period = post("/api/motor/state").get("period_ms") or period
|
||
tag = "micro_mirror"
|
||
csv_path = os.path.join(LAB, "pair_test_%s.csv" % tag)
|
||
meta = {"test": "micro_mirror_mode3", "span": span, "vel": speed,
|
||
"t0": t0, "t1": t1}
|
||
write_series_csv(csv_path, series, meta)
|
||
an = analyze_pair(series, period, label=tag)
|
||
an["csv"] = csv_path
|
||
an["command"] = meta
|
||
jpath = os.path.join(LAB, "pair_test_%s.json" % tag)
|
||
with open(jpath, "w", encoding="utf-8") as fp:
|
||
json.dump(an, fp, ensure_ascii=False, indent=2)
|
||
print(json.dumps(an, ensure_ascii=False, indent=2))
|
||
print("CSV", csv_path, "JSON", jpath)
|
||
return 0
|
||
|
||
|
||
def cmd_stop(args):
|
||
"""镜像 mode3：短行程后停在目标，禁止开环刹停测法。"""
|
||
period, _ = ensure_ecat()
|
||
speed = min(abs(args.vel), 80.0)
|
||
print("stop-test：对接机构仅允许 mode3 镜像行程后自然停靠（禁止 mode2）")
|
||
a2, _, a3, _, _ = _read_pair()
|
||
if a2 is None:
|
||
return 2
|
||
# 从对称位附近：先到 ±span，再回 0（行程用 stop.hold）
|
||
span = min(args.span if hasattr(args, "span") else 150.0, 200.0)
|
||
ok, msg, items = safety_precheck_motion(span, -span, speed)
|
||
print("leg1", msg)
|
||
if not ok:
|
||
return 2
|
||
ths = start_poll(args.threads)
|
||
t0 = time.time()
|
||
drive(items, args.cruise + 1.5, args.rate, mode=3,
|
||
voltage=args.voltage, pid=tuple(args.pid))
|
||
t_cmd_stop = time.time()
|
||
ok2, msg2, items2 = safety_precheck_motion(0.0, 0.0, speed)
|
||
print("leg2→0", msg2)
|
||
if ok2:
|
||
drive(items2, args.hold + 1.5, args.rate, mode=3,
|
||
voltage=args.voltage, pid=tuple(args.pid))
|
||
t1 = time.time()
|
||
stop_poll(ths)
|
||
series = aligned_series()
|
||
period = post("/api/motor/state").get("period_ms") or period
|
||
tag = "stop_mirror_mode3"
|
||
csv_path = os.path.join(LAB, "pair_test_%s.csv" % tag)
|
||
meta = {"test": "stop_mirror_mode3", "span": span, "vel": speed,
|
||
"t0": t0, "t_mid": t_cmd_stop, "t1": t1}
|
||
write_series_csv(csv_path, series, meta)
|
||
an = analyze_pair(series, period, label=tag)
|
||
an["csv"] = csv_path
|
||
an["command"] = meta
|
||
an["live_end"] = {str(n): snap_motors().get(n) for n in NODES if snap_motors().get(n)}
|
||
jpath = os.path.join(LAB, "pair_test_%s.json" % tag)
|
||
with open(jpath, "w", encoding="utf-8") as fp:
|
||
json.dump(an, fp, ensure_ascii=False, indent=2)
|
||
print(json.dumps(an, ensure_ascii=False, indent=2))
|
||
print("CSV", csv_path, "JSON", jpath)
|
||
return 0
|
||
hold_position(seconds=0.35, pid=tuple(args.pid), voltage=args.voltage)
|
||
ths = start_poll(args.threads)
|
||
t0 = time.time()
|
||
print("cruise...")
|
||
n1, e1 = drive(cruise_items, args.cruise, args.rate, mode=2,
|
||
voltage=args.voltage, pid=tuple(args.pid))
|
||
t_cmd_stop = time.time()
|
||
print("STOP → 0")
|
||
n2, e2 = drive(stop_items, args.hold, args.rate, mode=2,
|
||
voltage=args.voltage, pid=tuple(args.pid))
|
||
t1 = time.time()
|
||
stop_poll(ths)
|
||
series = aligned_series()
|
||
period = post("/api/motor/state").get("period_ms") or period
|
||
csv_path = os.path.join(LAB, "pair_test_%s.csv" % tag)
|
||
meta = {"test": "stop", "vel": vel, "mirror": args.mirror,
|
||
"t0": t0, "t_stop_cmd": t_cmd_stop, "t1": t1,
|
||
"sends_cruise": n1, "sends_stop": n2, "err": [e1, e2]}
|
||
write_series_csv(csv_path, series, meta)
|
||
|
||
# 停止沿分析：以发 STOP 时刻为界
|
||
if len(series) < 20:
|
||
print("samples too few")
|
||
return 2
|
||
# 找 wall 接近 t_cmd_stop 的索引
|
||
idx_stop = 0
|
||
for i, row in enumerate(series):
|
||
if row[1] >= t_cmd_stop:
|
||
idx_stop = max(0, i - 1)
|
||
break
|
||
pre = series[:idx_stop + 1]
|
||
post_s = series[idx_stop:]
|
||
|
||
def win_stats(seg):
|
||
if len(seg) < 5:
|
||
return None
|
||
v2 = [r[3] for r in seg]
|
||
v3 = [r[5] for r in seg]
|
||
vs = [r[6] for r in seg]
|
||
I2, I3, Isum = trapz_vel(seg, period)
|
||
return {
|
||
"n": len(seg),
|
||
"mean_v2": sum(v2) / len(v2),
|
||
"mean_v3": sum(v3) / len(v3),
|
||
"mean_abs_v2": sum(abs(x) for x in v2) / len(v2),
|
||
"mean_abs_v3": sum(abs(x) for x in v3) / len(v3),
|
||
"rms_vsum": (sum(x * x for x in vs) / len(vs)) ** 0.5,
|
||
"max_abs_vsum": max(abs(x) for x in vs),
|
||
"I2": I2, "I3": I3, "Isum": Isum,
|
||
}
|
||
|
||
# 各轴 |v| 穿越 20 的时刻
|
||
def cross(seg, node_i, thr=20.0, falling=True):
|
||
vi = 3 if node_i == 2 else 5
|
||
for i in range(1, len(seg)):
|
||
v0, v1 = abs(seg[i - 1][vi]), abs(seg[i][vi])
|
||
if falling and v0 >= thr > v1:
|
||
return seg[i - 1][0] + (v0 - thr) / (v0 - v1 + 1e-12) * (seg[i][0] - seg[i - 1][0])
|
||
if (not falling) and v0 < thr <= v1:
|
||
return seg[i - 1][0] + (thr - v0) / (v1 - v0 + 1e-12) * (seg[i][0] - seg[i - 1][0])
|
||
return None
|
||
|
||
c2 = cross(post_s, 2)
|
||
c3 = cross(post_s, 3)
|
||
off_delta_ms = None
|
||
if c2 is not None and c3 is not None:
|
||
off_delta_ms = (c3 - c2) * period
|
||
|
||
an = analyze_pair(series, period, label=tag)
|
||
an.update({
|
||
"csv": csv_path,
|
||
"command": meta,
|
||
"pre_stop": win_stats(pre),
|
||
"post_stop": win_stats(post_s),
|
||
"off_cross20_ms_n3_minus_n2": off_delta_ms,
|
||
"verdict_hint": {
|
||
"cruise": "比 pre_stop 的 mean_abs_v2/v3 谁大",
|
||
"stop": "比 post_stop 的 max_abs_vsum 与 off_delta_ms；Δ>0 表示 N3 晚停",
|
||
"integral": "post_stop.Isum 绝对值 = 停车段被顶开的积分",
|
||
},
|
||
})
|
||
jpath = os.path.join(LAB, "pair_test_%s.json" % tag)
|
||
with open(jpath, "w", encoding="utf-8") as fp:
|
||
json.dump(an, fp, ensure_ascii=False, indent=2)
|
||
print(json.dumps(an, ensure_ascii=False, indent=2))
|
||
print("CSV", csv_path, "JSON", jpath)
|
||
return 0
|
||
|
||
|
||
def cmd_trip(args):
|
||
"""mode3 镜像行程：n2=+span, n3=-span（已对接强制镜像）。"""
|
||
period, _ = ensure_ecat()
|
||
span = abs(args.span)
|
||
pos = [(2, span), (3, -span)]
|
||
zero = [(2, 0.0), (3, 0.0)]
|
||
tag = "trip_mirror"
|
||
print("trip %s span=±%.1f vel=%.1f hold=%.2f（强制镜像）"
|
||
% (tag, span, args.vel, args.hold))
|
||
snap = snap_motors()
|
||
a2 = snap.get(2, {}).get("ang", 0.0)
|
||
# 选择更远的端点作为第一目标
|
||
if abs(a2 - span) < abs(a2 - 0) and abs(a2) > span * 0.5:
|
||
first, second = zero, pos
|
||
elif abs(a2) < span * 0.5:
|
||
first, second = pos, zero
|
||
else:
|
||
first, second = zero, pos
|
||
|
||
ths = start_poll(args.threads)
|
||
t0 = time.time()
|
||
print("leg1 →", first)
|
||
snap = snap_motors()
|
||
t2t, t3t = first[0][1], first[1][1]
|
||
drive(_mirror_cmd_items(t2t, t3t, abs(args.vel), snap), args.hold,
|
||
args.rate, mode=3, voltage=args.voltage, pid=tuple(args.pid))
|
||
t_mid = time.time()
|
||
print("leg2 →", second)
|
||
snap = snap_motors()
|
||
t2t, t3t = second[0][1], second[1][1]
|
||
drive(_mirror_cmd_items(t2t, t3t, abs(args.vel), snap), args.hold,
|
||
args.rate, mode=3, voltage=args.voltage, pid=tuple(args.pid))
|
||
t1 = time.time()
|
||
stop_poll(ths)
|
||
series = aligned_series()
|
||
period = post("/api/motor/state").get("period_ms") or period
|
||
csv_path = os.path.join(LAB, "pair_test_%s.csv" % tag)
|
||
meta = {"test": "trip_mirror", "span": span, "vel": args.vel,
|
||
"t0": t0, "t_mid": t_mid, "t1": t1, "coupled": True}
|
||
write_series_csv(csv_path, series, meta)
|
||
an = analyze_pair(series, period, label=tag)
|
||
an["csv"] = csv_path
|
||
an["command"] = meta
|
||
an["live_end"] = {str(n): snap_motors().get(n) for n in NODES if snap_motors().get(n)}
|
||
jpath = os.path.join(LAB, "pair_test_%s.json" % tag)
|
||
with open(jpath, "w", encoding="utf-8") as fp:
|
||
json.dump(an, fp, ensure_ascii=False, indent=2)
|
||
print(json.dumps(an, ensure_ascii=False, indent=2))
|
||
print("CSV", csv_path, "JSON", jpath)
|
||
return 0
|
||
|
||
|
||
def _mirror_cmd_items(target2, target3, speed, snap=None):
|
||
"""仅通过 safety_precheck 生成 CMD；禁止把速度强行改成相反符号。"""
|
||
ok, msg, items = safety_precheck_motion(target2, target3, abs(speed))
|
||
print(" precheck:", msg)
|
||
if not ok:
|
||
raise RuntimeError("SAFETY: " + msg)
|
||
return items
|
||
|
||
|
||
def cmd_lowspeed(args):
|
||
"""低速镜像试控：先只读检查，再极小行程。错位大则拒绝一切运动。"""
|
||
period, st = ensure_ecat()
|
||
a2, v2, a3, v3, snap = _read_pair()
|
||
asum = None
|
||
if a2 is not None and a3 is not None:
|
||
asum = a2 + a3
|
||
print("\n======== 低速试控前置检查 ========")
|
||
print("a2=%s a3=%s a2+a3=%s v2=%s v3=%s" % (a2, a3, asum, v2, v3))
|
||
print("硬限位: |ang|≤%.0f |a2+a3|≤%.0f 禁止 mode2 禁止同向" %
|
||
(MAX_ABS_ANGLE, MAX_ASUM_ERR))
|
||
if a2 is None or a3 is None:
|
||
print("结论: 状态缺失 → 不发运动指令")
|
||
return 2
|
||
if abs(a2) > MAX_ABS_ANGLE or abs(a3) > MAX_ABS_ANGLE:
|
||
print("结论: 编码器超限 → 不发运动指令。请人工把机构回到对称位。")
|
||
return 2
|
||
if asum is not None and abs(asum) > MAX_ASUM_ERR:
|
||
print("结论: 错位 a2+a3=%.1f° 过大 → 拒绝电控运动。" % asum)
|
||
print("请先人工盘车/松耦合，使 a2≈-a3（和接近 0），再跑 lows peed。")
|
||
return 2
|
||
speed = min(abs(args.vel), 20.0) # 低速硬顶 20°/s
|
||
span = min(abs(args.span), 15.0) # 小行程硬顶 15°
|
||
t2, t3 = span, -span
|
||
if abs(t2 - a2) > 3 * span:
|
||
# 当前离目标太远则以当前对称附近为中心：目标 = round 到对称
|
||
t2 = span if a2 < 0 else -span
|
||
t3 = -t2
|
||
ok, msg, items = safety_precheck_motion(t2, t3, speed)
|
||
print("试控目标 ±%.1f° @ %.1f°/s" % (span, speed))
|
||
print(msg)
|
||
if not ok:
|
||
print("结论: precheck 失败，不发令")
|
||
return 2
|
||
print("发令 items:", items)
|
||
ths = start_poll(args.threads)
|
||
t0 = time.time()
|
||
n_ok, err = drive(items, max(args.hold, 2.0), rate=10.0, mode=3,
|
||
voltage=args.voltage, pid=tuple(args.pid))
|
||
print("行程发送完成 n_ok=%s err=%s" % (n_ok, err))
|
||
# 回 0（若仍安全）
|
||
ok2, msg2, items2 = safety_precheck_motion(0.0, 0.0, speed)
|
||
print("回零:", msg2)
|
||
if ok2:
|
||
drive(items2, max(args.hold, 2.0), rate=10.0, mode=3,
|
||
voltage=args.voltage, pid=tuple(args.pid))
|
||
else:
|
||
# 保持当前位置
|
||
a2, _, a3, _, _ = _read_pair()
|
||
if a2 is not None and a3 is not None:
|
||
try:
|
||
drive([(2, 0.0, a2), (3, 0.0, a3)], 0.6, rate=10.0, mode=3,
|
||
voltage=args.voltage, pid=tuple(args.pid))
|
||
except Exception as e:
|
||
print("hold err", e)
|
||
t1 = time.time()
|
||
stop_poll(ths)
|
||
a2e, v2e, a3e, v3e, _ = _read_pair()
|
||
print("结束: a2=%s v2=%s a3=%s v3=%s asum=%s" %
|
||
(a2e, v2e, a3e, v3e, (None if a2e is None or a3e is None else a2e + a3e)))
|
||
series = aligned_series()
|
||
period = post("/api/motor/state").get("period_ms") or period
|
||
csv_path = os.path.join(LAB, "pair_test_lowspeed.csv")
|
||
meta = {"test": "lowspeed", "vel": speed, "span": span,
|
||
"t0": t0, "t1": t1, "items": items}
|
||
write_series_csv(csv_path, series, meta)
|
||
an = analyze_pair(series, period, label="lowspeed")
|
||
an["csv"] = csv_path
|
||
an["command"] = meta
|
||
an["end"] = {"a2": a2e, "v2": v2e, "a3": a3e, "v3": v3e,
|
||
"asum": None if a2e is None or a3e is None else a2e + a3e}
|
||
jpath = os.path.join(LAB, "pair_test_lowspeed.json")
|
||
with open(jpath, "w", encoding="utf-8") as fp:
|
||
json.dump(an, fp, ensure_ascii=False, indent=2)
|
||
print(json.dumps(an, ensure_ascii=False, indent=2)[:800])
|
||
print("CSV", csv_path, "JSON", jpath)
|
||
return 0
|
||
|
||
|
||
def cmd_fairtrip(args):
|
||
"""公平行程：先双机 mode3 停到 0（镜像速度），再同时去 +span/-span。"""
|
||
period, _ = ensure_ecat()
|
||
span = abs(args.span)
|
||
print("fairtrip: settle@0 → mirror ±%.0f @ vel=%.1f hold=%.2f（强制镜像）"
|
||
% (span, args.vel, args.hold))
|
||
|
||
ths = start_poll(args.threads)
|
||
t0 = time.time()
|
||
print("1) 同步到 0（速度按镜像指向 0）")
|
||
items0 = _mirror_cmd_items(0.0, 0.0, abs(args.vel))
|
||
print(" cmd", items0)
|
||
drive(items0, args.settle,
|
||
args.rate, mode=3, voltage=args.voltage, pid=tuple(args.pid))
|
||
snap = snap_motors()
|
||
print(" at0:", {n: (snap.get(n) or {}).get("ang") for n in NODES})
|
||
t_go = time.time()
|
||
print("2) 同时镜像 ±span")
|
||
items1 = _mirror_cmd_items(span, -span, abs(args.vel), snap)
|
||
print(" cmd", items1)
|
||
drive(items1, args.hold,
|
||
args.rate, mode=3, voltage=args.voltage, pid=tuple(args.pid))
|
||
t1 = time.time()
|
||
stop_poll(ths)
|
||
snap = snap_motors()
|
||
print(" end:", {n: (snap.get(n) or {}).get("ang") for n in NODES},
|
||
{n: (snap.get(n) or {}).get("vel") for n in NODES})
|
||
|
||
series = aligned_series()
|
||
period = post("/api/motor/state").get("period_ms") or period
|
||
csv_path = os.path.join(LAB, "pair_test_fairtrip.csv")
|
||
meta = {"test": "fairtrip", "span": span, "vel": args.vel,
|
||
"t0": t0, "t_go": t_go, "t1": t1}
|
||
write_series_csv(csv_path, series, meta)
|
||
|
||
# 只分析 t_go 之后的行程
|
||
seg = [r for r in series if r[1] >= t_go]
|
||
an = analyze_pair(seg, period, label="fairtrip")
|
||
an["csv"] = csv_path
|
||
an["command"] = meta
|
||
an["live_end"] = {str(n): snap.get(n) for n in NODES if snap.get(n)}
|
||
|
||
# 到达时刻：|ang - target| < 3 且 |v| < 10
|
||
def arrive(node, target):
|
||
vi_ang = 2 if node == 2 else 4
|
||
vi_vel = 3 if node == 5 else 5
|
||
vi_vel = 3 if node == 2 else 5
|
||
for r in seg:
|
||
if abs(r[vi_ang] - target) < 3.0 and abs(r[vi_vel]) < 10.0:
|
||
return r[1]
|
||
return None
|
||
t2 = arrive(2, span)
|
||
t3 = arrive(3, -span)
|
||
an["arrive_wall"] = {"n2": t2, "n3": t3}
|
||
if t2 and t3:
|
||
an["arrive_delta_ms_n3_minus_n2"] = (t3 - t2) * 1000.0
|
||
|
||
# 行程中段 20%-80% 的 vsum / 巡航
|
||
if len(seg) > 50:
|
||
w = seg[len(seg) // 5: max(len(seg) - len(seg) // 5, len(seg) // 5 + 2)]
|
||
I2, I3, Isum = trapz_vel(w, period)
|
||
an["mid_stroke"] = {
|
||
"n": len(w),
|
||
"mean_abs_v2": sum(abs(r[3]) for r in w) / len(w),
|
||
"mean_abs_v3": sum(abs(r[5]) for r in w) / len(w),
|
||
"mean_vsum": sum(r[6] for r in w) / len(w),
|
||
"rms_vsum": (sum(r[6] ** 2 for r in w) / len(w)) ** 0.5,
|
||
"max_abs_vsum": max(abs(r[6]) for r in w),
|
||
"I2": I2, "I3": I3, "Isum": Isum,
|
||
}
|
||
jpath = os.path.join(LAB, "pair_test_fairtrip.json")
|
||
with open(jpath, "w", encoding="utf-8") as fp:
|
||
json.dump(an, fp, ensure_ascii=False, indent=2)
|
||
print(json.dumps(an, ensure_ascii=False, indent=2))
|
||
print("CSV", csv_path, "JSON", jpath)
|
||
return 0
|
||
|
||
|
||
def cmd_stopnow(_args):
|
||
print("STOP NOW")
|
||
for _ in range(8):
|
||
try:
|
||
burst_cmds([(2, 0.0, 0.0), (3, 0.0, 0.0)], mode=2)
|
||
except Exception as e:
|
||
print(" ", e)
|
||
time.sleep(0.03)
|
||
print("live", json.dumps(
|
||
{str(n): snap_motors().get(n) for n in NODES if snap_motors().get(n)},
|
||
ensure_ascii=False)[:300])
|
||
return 0
|
||
|
||
|
||
def cmd_full(args):
|
||
"""对接安全套件：仅镜像 openloop / stop / fairtrip（禁止同向）。"""
|
||
results = {}
|
||
print("\n======== FULL-MIRROR: 1/4 align ========")
|
||
cmd_align(args)
|
||
|
||
print("\n======== FULL-MIRROR: 2/4 openloop mirror ========")
|
||
cmd_openloop(args)
|
||
p_ol = os.path.join(LAB, "pair_test_openloop_mirror_vel%s.json" % abs(args.vel))
|
||
if os.path.exists(p_ol):
|
||
results["openloop_mirror"] = json.loads(open(p_ol, encoding="utf-8").read())
|
||
|
||
print("\n======== FULL-MIRROR: 3/4 stop mirror ========")
|
||
cmd_stop(args)
|
||
p_st = os.path.join(LAB, "pair_test_stop_mirror.json")
|
||
if os.path.exists(p_st):
|
||
results["stop_mirror"] = json.loads(open(p_st, encoding="utf-8").read())
|
||
|
||
print("\n======== FULL-MIRROR: 4/4 fairtrip mirror ========")
|
||
cmd_fairtrip(args)
|
||
p_ft = os.path.join(LAB, "pair_test_fairtrip.json")
|
||
if os.path.exists(p_ft):
|
||
results["fairtrip"] = json.loads(open(p_ft, encoding="utf-8").read())
|
||
|
||
out = os.path.join(LAB, "pair_test_FULL_mirror_summary.json")
|
||
slim = {"coupled": True, "rule": "mirror_only_v2=-v3", "results": {}}
|
||
for k, v in results.items():
|
||
slim["results"][k] = {kk: v.get(kk) for kk in (
|
||
"label", "cruise_mean_abs_v2", "cruise_mean_abs_v3",
|
||
"cruise_mean_vsum", "mov_rms_vsum", "mov_max_abs_vsum",
|
||
"I_sum_v2_plus_v3", "asum_delta", "arrive_delta_ms_n3_minus_n2",
|
||
"off_cross20_ms_n3_minus_n2", "mid_stroke", "pre_stop", "post_stop",
|
||
) if kk in v}
|
||
with open(out, "w", encoding="utf-8") as fp:
|
||
json.dump(slim, fp, ensure_ascii=False, indent=2)
|
||
print(json.dumps(slim, ensure_ascii=False, indent=2))
|
||
print("summary ->", out)
|
||
return 0
|
||
|
||
|
||
def _who_slower(an):
|
||
v2 = an.get("cruise_mean_abs_v2")
|
||
v3 = an.get("cruise_mean_abs_v3")
|
||
if not v2 or not v3:
|
||
return None
|
||
if abs(v2 - v3) < 1.0:
|
||
return "almost_equal"
|
||
return "N2_slower" if v2 < v3 else "N3_slower"
|
||
|
||
|
||
# ───────────────────────── CLI ─────────────────────────
|
||
|
||
def build_parser():
|
||
p = argparse.ArgumentParser(
|
||
description="N2/N3 对接电机诊断发令（ECAT）— 请确认机构安全")
|
||
p.add_argument("--pid", type=int, nargs=3, default=[100, 100, 100],
|
||
help="PID current/vel/angle P，默认 100 100 100")
|
||
p.add_argument("--voltage", type=float, default=24.0)
|
||
p.add_argument("--rate", type=float, default=25.0, help="发令频率 Hz")
|
||
p.add_argument("--threads", type=int, default=6)
|
||
sub = p.add_subparsers(dest="cmd", required=True)
|
||
|
||
sub.add_parser("status", help="只读状态与 CONFIG")
|
||
slowp = sub.add_parser("lowspeed", help="低速镜像试控（错位大则拒绝）")
|
||
slowp.add_argument("--vel", type=float, default=15.0)
|
||
slowp.add_argument("--span", type=float, default=10.0)
|
||
slowp.add_argument("--hold", type=float, default=2.5)
|
||
|
||
sp = sub.add_parser("align", help="统一 CONFIG 并保持")
|
||
sp.add_argument("--limit-v", type=float, default=22.0)
|
||
sp.add_argument("--vel", type=float, default=100.0)
|
||
sp.add_argument("--mode", type=int, default=3)
|
||
sp.add_argument("--hold", type=float, default=0.8)
|
||
|
||
so = sub.add_parser("openloop", help="开环速度对比 mode2")
|
||
so.add_argument("--vel", type=float, default=80.0)
|
||
so.add_argument("--cruise", type=float, default=1.5)
|
||
so.add_argument("--settle", type=float, default=0.6)
|
||
|
||
ss = sub.add_parser("stop", help="巡航→同时刹停")
|
||
ss.add_argument("--vel", type=float, default=80.0)
|
||
ss.add_argument("--cruise", type=float, default=1.2)
|
||
ss.add_argument("--hold", type=float, default=0.8)
|
||
ss.add_argument("--mirror", action="store_true", help="v2=-v3 镜像巡航")
|
||
|
||
st = sub.add_parser("trip", help="mode3 位置行程")
|
||
st.add_argument("--span", type=float, default=200.0)
|
||
st.add_argument("--vel", type=float, default=80.0)
|
||
st.add_argument("--hold", type=float, default=2.5)
|
||
st.add_argument("--mirror", action="store_true")
|
||
|
||
sfair = sub.add_parser("fairtrip", help="先到0再同时±span，公平对比")
|
||
sfair.add_argument("--span", type=float, default=400.0)
|
||
sfair.add_argument("--vel", type=float, default=200.0)
|
||
sfair.add_argument("--hold", type=float, default=2.5)
|
||
sfair.add_argument("--settle", type=float, default=2.0)
|
||
|
||
sub.add_parser("stopnow", help="急停速度=0")
|
||
|
||
sf = sub.add_parser("full", help="align+openloop+stop+trip 全套")
|
||
sf.add_argument("--vel", type=float, default=80.0)
|
||
sf.add_argument("--cruise", type=float, default=1.2)
|
||
sf.add_argument("--hold", type=float, default=2.2)
|
||
sf.add_argument("--settle", type=float, default=0.6)
|
||
sf.add_argument("--span", type=float, default=200.0)
|
||
sf.add_argument("--limit-v", type=float, default=22.0)
|
||
sf.add_argument("--mode", type=int, default=3)
|
||
sf.add_argument("--mirror", action="store_true")
|
||
return p
|
||
|
||
|
||
def main(argv=None):
|
||
args = build_parser().parse_args(argv)
|
||
print("⚠ 请确认电机2/3机构处于安全状态；Ctrl+C 后请手动 stopnow")
|
||
print("target:", args.cmd, "pid", args.pid)
|
||
handlers = {
|
||
"status": cmd_status,
|
||
"lowspeed": cmd_lowspeed,
|
||
"align": cmd_align,
|
||
"openloop": cmd_openloop,
|
||
"stop": cmd_stop,
|
||
"trip": cmd_trip,
|
||
"fairtrip": cmd_fairtrip,
|
||
"stopnow": cmd_stopnow,
|
||
"full": cmd_full,
|
||
}
|
||
try:
|
||
return handlers[args.cmd](args)
|
||
except KeyboardInterrupt:
|
||
print("\n!! 中断 → 急停")
|
||
try:
|
||
cmd_stopnow(args)
|
||
except Exception:
|
||
pass
|
||
return 130
|
||
|
||
|
||
if __name__ == "__main__":
|
||
raise SystemExit(main())
|