cbb/DCU_DesktopController
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
27a8fc179775ea171557ca50328379170eb5d026
DCU_DesktopController/scripts/test_motor_pair_n2_n3.py
T

1133 lines
44 KiB
Python
Raw Normal View History

docs(compose): 上位机代码质量评估 host-code-quality + 积分耦合报告产物
2026-09-21 02:12:04 +08:00
#!/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())
Reference in New Issue Copy Permalink