From 27a8fc179775ea171557ca50328379170eb5d026 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Mon, 21 Sep 2026 02:12:04 +0800 Subject: [PATCH] =?UTF-8?q?docs(compose):=20=E4=B8=8A=E4=BD=8D=E6=9C=BA?= =?UTF-8?q?=E4=BB=A3=E7=A0=81=E8=B4=A8=E9=87=8F=E8=AF=84=E4=BC=B0=20host-c?= =?UTF-8?q?ode-quality=20+=20=E7=A7=AF=E5=88=86=E8=80=A6=E5=90=88=E6=8A=A5?= =?UTF-8?q?=E5=91=8A=E4=BA=A7=E7=89=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 全仓冗余/复杂度审计（3.5/5），交付重构清单 R1–R16； 附带入库 ecat 积分耦合 HTML 报告与 N2/N3 成对测试脚本。 --- docs/compose/spec/host-code-quality.md | 216 +++++ lab/ecat_integral_report.html | 359 ++++++++ scripts/build_integral_report_html.py | 424 +++++++++ scripts/test_motor_pair_n2_n3.py | 1132 ++++++++++++++++++++++++ 4 files changed, 2131 insertions(+) create mode 100644 docs/compose/spec/host-code-quality.md create mode 100644 lab/ecat_integral_report.html create mode 100644 scripts/build_integral_report_html.py create mode 100644 scripts/test_motor_pair_n2_n3.py diff --git a/docs/compose/spec/host-code-quality.md b/docs/compose/spec/host-code-quality.md new file mode 100644 index 0000000..e2355b2 --- /dev/null +++ b/docs/compose/spec/host-code-quality.md @@ -0,0 +1,216 @@ +--- +feature: host-code-quality +status: delivered +updated: 2026-09-23 +branch: main +commits: working-tree（主检出 main，用户授权；独立评审 pass，无 critical） +--- + +# 上位机代码质量评估（冗余 / 复杂度 / 屎山风险） + +## Report + +**What was built** — 本仓（`G:/Robot_v2/DCU/tools/ESP32`）全量代码质量评估文档。范围覆盖生产路径（`pages/` `js/` `css/` `esp32_server.py` `tabs.html`）以及 `scripts/` `lab/` `_local/` 与根目录报告；`tools/ecat` 仅评估本仓耦合面。方法：体积/行数盘点 + 三路只读审计（电机前端、Python 服务端、脚本杂物）+ 行号证据核验 + 独立评审。 + +**结论**：全仓综合 **3.5/5**——尚未不可救药，但已是明显「改出来的屎山」。不是乱写：transport 抽象、app-lease、大量实测注释说明有人在治理；真正的债是上帝文件、重复状态机、服务端越界替 ecat 补课、杂物堆积与文档漂移。交付重构清单 **R1–R16**（P0 风险 → P1 拆文件 → P2 服务端边界 → P3 卫生），本回合**未改任何业务代码**。 + +**Verification** — +- 行数量化：`main.js` total=4647 / nonempty=4398 / 顶层 `function|async function` 声明=178；`esp32_server.py` total=1536；`connection.js`=1207；`lab/spi_test.html`=2325 +- 卫生量化：`scripts/`=128 文件，README 点名仅 12；`lab/`=114 文件 57.4MB；`_local/`=27 文件 +- 证据抽查：`BASE_DIR` 双定义 L56/L134；README 默认 I226 vs 代码 I210（`device-link.js:31`、`motor.html:39`、`esp32_server.py:638`）；`_ecat_proxy` L762–775 autostart 后同 body 重发，L1028–1030 注释自认非幂等风险 +- 独立评审：**pass**，无 critical；major（表重复/Report 空/区间过宽）已在本版修订 +- 本回合未跑业务测试（无业务代码变更） + +**Journey log** — +- `git worktree add` 被沙箱拦截 → 用户授权主检出 main 写评估文档（与 pdo-fast-send 同路径） +- PowerShell `Measure-Object -Line` 会丢空行，应用 `ReadAllLines().Length` 做 total 口径 +- 评估类 compose：Tasks 覆盖「产出评估」而非「实施重构」；R1–R16 是 backlog，不阻塞 delivered +- 屎山分不是「代码写得差」，是「边界跟不上迭代」：P0 先止血（非幂等重发），再拆文件 + +## [S1] Problem + +用户要求评估 `G:\Robot_v2\DCU\tools\ESP32` 这套 ESP32 调试上位机在持续迭代后，是否已经变成「屎山」：有没有冗余、过度复杂、功能堆叠和不可维护的结构。本回合只交付评估结论与重构清单，**不改业务代码**。 + +## [S2] Design + +### 评估范围与方法 + +- **范围**：本仓全量——生产路径（`pages/` `js/` `css/` `esp32_server.py` `tabs.html`）+ `scripts/` `lab/` `_local/` + 根目录报告。姊妹仓 `tools/ecat` 仅评估本仓对其的耦合面，不深挖 native 源码。 +- **方法**：文件体积与行数盘点 → 模块职责/函数热点扫描 → 三路只读审计（电机前端、Python 服务端、脚本杂物）→ 对照 README/spec/gitignore 与近期 churn 核验证据。 +- **判定标准**： + - **领域复杂度**：协议/双链路/实时性约束本身带来的必然复杂度，不可删。 + - **结构债**：上帝文件、重复状态机、越界补丁、文档漂移——可治理。 + - **卫生债**：一次性脚本、采样 dump、根目录报告——可归档删除。 + +### 总判定 + +| 维度 | 评分 (1=干净 … 5=屎山) | 一句话 | +|------|------------------------|--------| +| 电机前端 `js/motor/main.js` | **3.5** | 有意识治理中的上帝文件；再拖一轮需求会到 4+ | +| Python 服务端 `esp32_server.py` | **3.5** | 有纪律的补丁山：注释极好，职责爆炸 | +| 脚本 / lab / _local 杂物 | **4.0** | 策略（.gitignore）清楚，磁盘堆积严重 | +| **全仓综合** | **3.5 / 5** | **尚未不可救药，但已明显「改出来的屎山」；不是乱写，是架构边界跟不上迭代速度** | + +**结论**：领域复杂度真实存在（CAN FD 协议、WiFi/EtherCAT 双链路、PDO 实时约束、pywebview 双进程），transport 抽象与大量设计注释说明有人在治理。真正的屎山信号是：**上帝文件未拆分、连接/总线状态机多处同源重复、服务端替 tools/ecat 扛了大量进程治理、杂物与文档漂移**。属于「有纪律的腐化」，还能救，但优先重构必须先做。 + +### 关键发现（证据） + +#### A. 电机前端 —— 上帝文件 + 半截双链路 + +| 位置 | 问题 | 严重度 | +|------|------|--------| +| `js/motor/main.js` **4647 行（含空行）/ 219KB / 顶层 `function`+`async function` 声明 178 个** | 文件头自承「由 motor.html 内联脚本机械外提，逻辑未改」——只搬了位置，没拆职责 | major | +| 同文件六域同居 | UI 持久化 (~L203–526)、连接+轮询 (~L940–1768)、命令序列 (~L2503–2997)、版本双路径 (~L2998–3284)、OTA+IDB (~L3580–4211)、示波器 IIFE (~L4257–end) | major | +| 链路特判仍散落业务层 | `failThresholdFor` L157、probe 复核 L1291、在线语义 L1340、**版本等待双实现** L3156/L3244、**参数应答双通道** L3353/L3384 | major | +| 两套并行连接状态机 | main.js 自建 failCount/probe/pollGen（不 import `connection.js`）；`connection.js` 另有 updateConnectionUi/testConnection | major | +| ECAT 总线 bootstrap 重复 | `device-link.js` `bootstrapEcatBus` L167–231 注释自承与 `transport-ecat.js` `ensureOpen/ensureStarted`「同源」 | major | +| 小重复 | `toast/showModal` main.js L566 自实现，其他页用 `common/toast.js`；`setClsIfChanged`/`setTextIfChanged` 与 connection.js 重复；`fetchDeviceJson` ≈ transport-wifi `getJson` | minor | +| transport 抽象本身 | `transport.js` 契约清晰（L11–26），wifi/ecat 实现分离得当；**这是正确决策，不是债** | （正向） | + +#### B. Python 服务端 —— 单文件九职责 + 越界补丁 + +| 位置 | 问题 | 严重度 | +|------|------|--------| +| `esp32_server.py` **1536 行** | 静态服务、reload 注入、UI-state、ECAT 代理、autostart、ESF1 快发、GUI 桥、app-lease、窗口双进程/Job/单实例 —— 九类职责同居 | major | +| 模块级全局 14+ | `g_ecat_*` / `g_fast_*` / `g_app_*` / `g_win` / `g_job` 等（L136–156 起） | major | +| 同一后端三通道 | HTTP 代理 `_ecat_proxy` + GUI 桥 `_GuiApi.ecat_api` + ESF1 TCP `ecat_send_raw_frames`，路径归一化重复 | major | +| **代理失败后自动 autostart 并重发同一 POST** | L762–775；L1028–1030 注释自己承认「带副作用的请求假超时 = 再写一遍」；仅连接失败/超时异常路径触发，非任意错误码 | **critical**（设计风险） | +| 越界替 tools/ecat 补课 | 端口漂移探测、`--parent-pid`、taskkill 全杀、RealTime 提权、stdout 写到他仓、硬编码 `--substr I210 --mask 0x50`（L638） | major | +| 跨进程单实例不互斥 | GUI 进程与服务子进程**各自**可能 `_kill_all_ecat_native`；`g_mutex` 只在进程内（L592） | major | +| 文档漂移 | README L323 写默认网卡 **I226**，代码/页面/测试默认全是 **I210**（`device-link.js:31`、`motor.html:39`、`esp32_server.py:638`、`test_ecat_autostart.mjs`） | minor（但误导） | +| 冗余小点 | `BASE_DIR` 定义两次（L56 与 L134）；motor/lab ui-state 路由近重复；前台窗口逻辑三处复制；端口遍历两份 | minor | +| 正向 | app-lease 接口面干净；锁与原子写有意识；双进程是实测踩坑后的合理架构；注释质量极高 | （正向） | + +#### C. 脚本 / lab / _local —— 卫生债最重 + +| 路径 | 体量 | 判定 | +|------|------|------| +| `scripts/` | **128 个文件 / ~30MB** | 正式测试（README 点名）**仅 12 个**；`_tmp_`/`probe_`/`skip_`/`anal_`/`collect_` 约 **55+** 个属一次性排障残留 | +| `lab/` | **114 文件 / 57.4MB** | 大头是采样 dump（`ecat_passive_*.csv` 各 ~25MB 且疑似双份、`skip_meter/` 61 个 json）；tabs 真正用到的只有 `spi_test.html`、`lan9252_pm.html` | +| `_local/` | 27 文件 / 1.6MB | 整目录已 gitignore；截图 + dbg 脚本，一次性 | +| 根目录报告 | `ECAT_*.html/md`、`GD32_OTA_FLOW_COMPARE.html`、`REPORT_*.md` ~130KB+ | 无 tabs/README 页面表引用，应归 `docs/` 或删 | +| `lab/spi_test.html` | **2325 行全内联**（非空 2258） | 单文件复杂度高于 motor.html；README 已标「临时功能」 | +| css/ | 结构合理（base/layout/components/can/motor/responsive） | 跨文件重复选择器约 40 个（`.btn`/`.toast`/`.header-bar`）；can.css 内部 11 处重复 | + +`.gitignore` 策略本身是清楚的（采样/探针/`_local` 不入库）——问题不在策略，而在**磁盘未清理 + 部分实验产物仍可能被误跟踪 + 根目录报告未归档**。 + +#### D. 跨层架构观察 + +```mermaid +flowchart TB + subgraph pages["页面层"] + tabs["tabs.html 外壳"] + motor["pages/motor.html"] + other["index / can / lab"] + end + + subgraph motorJS["电机前端（上帝文件）"] + main["main.js 4647行
状态+UI+OTA+序列+波形+轮询"] + tp["transport.js 契约"] + wifi["transport-wifi"] + ecat["transport-ecat"] + end + + subgraph commonJS["common"] + dl["device-link ConnectionHub"] + conn["connection.js（OTA页用）"] + toast["toast.js"] + end + + subgraph server["esp32_server.py 九职责"] + static["静态+reload"] + proxy["/ecat/api 代理"] + gui["_GuiApi + ESF1"] + lease["app-lease WS"] + win["窗口/双进程/Job"] + end + + native["tools/ecat
ecat_native.exe"] + + tabs --> motor + motor --> main + main --> tp + tp --> wifi + tp --> ecat + main -. "不 import，自建状态机" .-> conn + main -. "自实现 toast" .-> toast + main --> dl + ecat -->|桥或HTTP| proxy + ecat -->|桌面桥| gui + dl -->|bootstrapEcatBus 与 transport 同源重复| proxy + proxy --> native + gui --> native + lease -->|停循环| proxy +``` + +**正确且该保留的决策**： +- 双链路 transport 抽象（业务共用、I/O 可换） +- tabs 多 iframe 保活 +- app-lease 关窗停总线 +- PDO 快发堵死慢发（pdo-fast-send） +- 丰富的实测注释与 compose 规格 + +**真正的屎山信号**： +1. `main.js` 只做了「机械外提」，六域仍同居 +2. 连接状态机 ×2、ECAT bootstrap ×2、DOM diff 助手 ×3、toast ×2 +3. `esp32_server.py` 替 ecat 扛进程治理，三通道同一后端，非幂等 POST 可重发 +4. 版本/节点参数回执未下沉 transport，业务层链路特判 ~16 处 +5. README（395 行 tribal knowledge）与代码默认值漂移（I226 vs I210） +6. scripts/lab 杂物 80%+ 与生产无关 + +### 重构清单（优先级，本回合不实施） + +> 下列为建议 backlog，不构成本 feature 的未完成任务。实施前请单独开 compose feature 或直接授权改码。 + +#### P0 — 正确性 / 风险（先做） + +| ID | 动作 | 验收 | +|----|------|------| +| R1 | `_ecat_proxy` 失败 autostart 后**禁止重发非幂等 POST**（`motor/start`/`flash`/`ota/ecat`/`eeprom/*` 等）；仅幂等读路由可重试一次 | 源码无副作用路径二次 POST；回归 `test_ecat_autostart.mjs` + 人工确认 flash 路径 | +| R2 | 统一 ECAT 网卡默认值文档与代码（I210 vs I226 二选一） | README、`device-link.js`、`motor.html`、`esp32_server.py`、相关测试默认一致 | +| R3 | 跨进程对 `ecat_native` 的 kill/autostart 加文件锁或让 native 侧唯一 | GUI 进程与服务子进程不会互相 kill 对方刚拉起的实例 | + +#### P1 — 拆上帝文件 / 收敛重复状态机 + +| ID | 动作 | 验收 | +|----|------|------| +| R4 | `main.js` 拆出 `motor/ota.js`、`motor/seq.js`、`motor/conn-poll.js`（或等价命名）；入口只做装配 | main.js total 行数 < 2000（目标）；`test_motor_transport` / `browser_check_*` 全绿；引用处 `?v=` 已 bump | +| R5 | 版本等待与节点参数回执**下沉 transport 能力接口**，删除 main.js 内 wifi/ecat 双实现分支 | `rg 'tpId\\s*===\\s*\"ecat\"' js/motor/main.js` 在版本/参数相关函数中无命中 | +| R6 | ECAT `bootstrapEcatBus` 只保留一处（建议 device-link 为唯一 Owner，transport 只调用） | 全仓仅一处 health→open→motor/start 策略代码 | +| R7 | 电机页连接状态机与 common 收敛：要么复用 device-link 健康快照，要么抽 `conn-policy.js`；删除重复 DOM diff / toast 实现 | main.js 不再自实现 toast；failCount/probe 语义单一 | + +#### P2 — 服务端边界 + +| ID | 动作 | 验收 | +|----|------|------| +| R8 | 从 `esp32_server.py` 拆出 `ecat_client.py`（端口探测 + 裸 socket + ESF1）与 `ecat_proc.py`（autostart/kill/parent-pid/RT） | server.py 只保留 HTTP/静态/lease/窗口；`test_esf_pack.py` 仍 PASS | +| R9 | 收敛 GUI 桥与 HTTP 代理的 path/body 归一化为同一函数 | 桥与代理共用路由表，无重复剥前缀逻辑 | +| R10 | 桌面 App 模式下设备状态/命令的 GUI 代理路径评估是否可统一（减少 desktop vs browser 双路径分支） | 文档明确两路径必要差异，或实现单一入口 | +| R11 | 删除/合并小冗余：`BASE_DIR` 双定义、ui-state 双路由参数化、前台焦点 helper 共用 | 源码 diff 可核对；无行为变化 | + +#### P3 — 卫生与文档 + +| ID | 动作 | 验收 | +|----|------|------| +| R12 | 清理 `scripts/`：保留 README/spec/gitignore 白名单引用的测试；`_tmp_`/`skip_`/`anal_`/无引用 `probe_` 移出或删除 | scripts 目录正式脚本 ≤ 25 个（保留清单可一页列完）；README 自测命令仍可执行 | +| R13 | 清理 `lab/` 采样 dump（csv/json/skip_meter）；保留 tabs 引用的实验页 | lab 总体积 < 5MB；`spi_test` / `lan9252_pm` 仍可从 tabs 打开 | +| R14 | `_local/` 与根目录报告归档：报告进 `docs/` 或删除；`_local` 保持 ignore | 仓库根无散落 REPORT/ECAT_*.html | +| R15 | README 瘦身或拆分：把长篇实测细节沉到 `docs/`，README 只留架构/入口/红线 | README 行数明显下降且默认值与代码一致 | +| R16 | CSS 去重：公共 chrome（`.btn`/`.toast`/`.header-bar`）收到 `components.css`；页面 CSS 只留本域 | 对 `.btn`/`.toast` 的重复定义处数可清点下降 | + +### [S3] Out of Scope + +- 本回合**不修改任何业务代码**（只产出本评估文档） +- 不深挖 `tools/ecat` 内部实现，只评估本仓耦合面 +- 不做性能基准测试（延迟数字引用 README/既有报告） +- 不实施 R1–R16 +- 不评估固件仓库（ESP32_COMM / GD32） + +## Tasks + +- [x] T1: 全仓体积/文件/churn 盘点 — acceptance: 产出 scripts/js/server/lab 量化摘要 (covers: S2) +- [x] T2: 电机前端结构审计 — acceptance: main.js 热点、双链路残留、重复状态机带 file:line (covers: S2) +- [x] T3: esp32_server 职责与耦合审计 — acceptance: 九职责分段、越界项、critical 风险带 file:line (covers: S2) +- [x] T4: scripts/lab/_local 卫生审计 — acceptance: 分类数量、可删清单、必须保留清单 (covers: S2) +- [x] T5: 汇总评估文档 + 优先级重构清单 R1–R16 — acceptance: 本文档可独立评审 (covers: S2) +- [x] T6: 独立评审 — acceptance: 评审者对照证据复核 critical/major 发现与清单可行性；pass 无 critical (covers: S2; depends: T1–T5) diff --git a/lab/ecat_integral_report.html b/lab/ecat_integral_report.html new file mode 100644 index 0000000..d1b8e31 --- /dev/null +++ b/lab/ecat_integral_report.html @@ -0,0 +1,359 @@ + + + + +对接电机 2/3 积分差归因报告 + + + +

对接电机 2 / 3：积分差是谁造成的？

+
+ 数据：ECAT 无源采集 lab/ecat_passive_latest.csv · 周期 0.25ms · + 理论：刚性镜像对接时 v2+v3≡0，∫(v2+v3)dt≡0，a2+a3=常数
+ 方法：对齐 cycles 后做梯形积分；先加后积与先积后加一致。 +
+ +
+

关键数字

+
+
巡航 |v| N2 / N3
+
199.38 / 199.38
+
巡航段 ∫(v2+v3) 平均
+
0.11°
+
停车段 ∫(v2+v3) |均值|
+
6.43°
+
干净起动 Δt 中位/均值
+
0.00 / 0.38 ms
+
+
+ + +
+

通俗结论：到底是谁慢？

+

先说人话：两台电机并不是“有一台全程都慢”。 + 你看到的积分差，主要不是巡航时谁转得慢，而是快停下 / 刚要走的那几十一到几百毫秒里，两台对不齐， + 把理论应为 0 的 ∫(v2+v3)dt 顶出 5～20° 左右的偏差。

+ + + +
+ 一句话：积分差的大头在 停车阶段。 + 对接约束要求 v2 = −v3；只要刹停瞬间一台还带着速度、另一台已经接近 0， + v2+v3 就会冒尖峰，积分就会被拉离理论 0。这属于动态不同步， + 不等于某台电机“标定转速低、一直慢”。 +
+ +
+
正常：同时启动 / 巡航一致 / 中段积分≈0
+
注意：停车沿残速、行程衔接重叠、积分被顶开
+
偏大：该行程 ∫(v2+v3) 或编码器 Δ(a2+a3) 明显偏离 0
+
+
+ + +
+

曲线（带标注）

+ + + + #0#1#2#3#4#5#6#7 + + + 速度 deg/s — 蓝 N2 / 橙 N3 + + + 合速度 v2+v3（理论 0）— 停车尖峰在这里 + + + 积分 ∫(v2+v3)dt（理论 0）— 每段末的偏离就是耦合误差 + + t_rel 0.1..60.1s · ECAT 0.25ms · 竖线为行程起点 + + + ↑ 巡航：两线贴合，都不慢 + ↓ 停车沿：v_sum 尖峰 → 积分爬升 + + +
+ + +
+

图上怎么看（对照上方曲线）

+
+
+
↑ 巡航段（两条速度曲线几乎贴合）
+

N2 蓝线、N3 橙线在 |v|≈200°/s 附近重合 → 都不慢。 + 下方红线 v2+v3 贴 0，积分曲线几乎水平 → 对接约束在稳态是成立的。

+
+
+
↓ 停车沿 / 换向点（红线尖峰）
+

接近终点时，一台速度先掉下来、另一台还在走 → v2+v3 出现 ±100～200°/s 尖峰。 + 积分曲线在这里爬上或爬下，行程结束就停在理论 0 以外几度到二十度。 + 这里是积分差的主要来源。

+
+
+
↓ 行程刚衔接时（起动窗）
+

若上一段没完全停稳就反向，会看到“N3 早就动了”的假象（表格里 onΔ 为 −800ms 量级）。 + 这不是启动指令不同步，而是速度还在反向爬坡 / 残速， + 也会把起动窗积分抬高一点。

+
+
+
↓ 紫色角度和 a2+a3
+

理论应为常数（理想为 0）。行程终点若偏到 ±10～25°，与该行程 ∫(v2+v3) 同量级 → + 说明偏差是真实位移耦合误差，不只是速度噪声。

+
+
+ +
+ 标定建议（若要压积分差）： + ① 先统一两台的减速段（同样斜率刹停），而不是去改巡航速度； + ② 检查 N2/N3 负载与制动是否对称； + ③ 控制器上对对接轴做 v2+v3 → 0 的纠偏，比单轴调 PID 更对症； + ④ 序列 hold 不要短于“两台都确认停稳”的时间，避免重叠行程把积分搅浑。 +
+
+ + +
+

各行程明细（积分从行程起点重算）

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
#方向起动Δ(N3-N2)巡航|v| N2/N3∫起动∫巡航∫停车停时仅N2在动停时仅N3在动停车归因Δ(a2+a3)
#0down+0.00 ms199.30 / 199.23+0.00-0.28+7.34+0.00+1.29N3 残速-21.32°
#1up-827.50 ms199.42 / 199.42+28.52+0.11-7.58+0.00-0.84N3 残速+25.81°
#2down-840.50 ms199.28 / 199.30-2.08+0.09-3.39+0.00+0.00两台接近同步刹停-1.12°
#3up+2.25 ms199.47 / 199.43+0.12+0.26+5.99+0.58+0.00N2 残速-0.14°
#4down-1.50 ms199.34 / 199.40+0.74+0.22-7.46-1.24+0.00N2 残速+0.19°
#5up+1.50 ms199.47 / 199.43+0.26+0.20+6.13+0.92+0.00N2 残速-0.19°
#6down+0.00 ms199.25 / 199.35+0.64+0.35-11.65-4.49+0.00N2 残速+0.21°
#7up+0.00 ms199.51 / 199.49+0.15-0.04+1.92+0.00+0.00两台接近同步刹停-13.38°
+

+ 单位：角度 deg（积分）· 速度 deg/s · 时间 ms。 + 「停时仅N2/N3在动」为该窗口内单边残速对 ∫(v2+v3) 的贡献。 +

+
+ +
+

原始结论脚本输出（备查）

+ 
{
+  "main_source_of_integral_error": "起动段",
+  "phase_Isum_mean": {
+    "on": 3.5436874809462364,
+    "cruise": 0.11441910352287721,
+    "stop": 1.0892628954283632
+  },
+  "who_lags_at_stop": "N2",
+  "stop_lag_ms_n3_minus_n2_mean": 0,
+  "cruise_speed": "巡航速度接近",
+  "cruise_bias_deg_s": 0.0004292367234076469,
+  "plain": [
+    "启动：两台几乎同时动（差约 0.x ms），启动不是积分变大的主因。",
+    "巡航：两台平均速度几乎一样，积分在中段也很小（贴合理论 0）。",
+    "停车：积分差主要在这里积出来；N3 相对 N2 平均晚停约 0 ms。",
+    "停止窗里「只剩一台在动」时：主要是 N2 还在走，把 ∫(v2+v3) 推离 0。",
+    "所以：不是谁「全程都慢」，而是 N3 停车更拖、在停机瞬间继续走，积分被顶起来。",
+    "N2 也有停车滞后，但幅度和样本都更小；巡航略快一点点时对积分贡献有限。"
+  ]
+}
+
+ + diff --git a/scripts/build_integral_report_html.py b/scripts/build_integral_report_html.py new file mode 100644 index 0000000..e275e67 --- /dev/null +++ b/scripts/build_integral_report_html.py @@ -0,0 +1,424 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""生成对接电机积分差归因 HTML（通俗说明 + 标注图）。""" +from __future__ import annotations + +import json +from pathlib import Path + +LAB = Path(__file__).resolve().parents[1] / "lab" +SRC = LAB / "ecat_who_slow.json" +OUT = LAB / "ecat_integral_report.html" + +data = json.loads(SRC.read_text(encoding="utf-8")) +pa = data["phase_attribution"] +strokes = data["strokes"] +sample = data.get("plot_sample") or [] +verdict = data.get("verdict") or {} + +# 用行程数据修正通俗结论（避免停车检测为 0 的误导） +on_vals = pa["on_ms_n3_minus_n2"]["values"] +# 剔除 |Δ|>50ms 的伪起动（上一段未停稳造成的重叠） +on_clean = [v for v in on_vals if abs(v) <= 50] +on_overlap = [v for v in on_vals if abs(v) > 50] +I_on = [s["I_on"]["Isum"] for s in strokes] +I_cr = [s["I_cruise"]["Isum"] for s in strokes] +I_st = [s["I_stop"]["Isum"] for s in strokes] +only2 = [s["stop_only_n2"]["Isum"] for s in strokes] +only3 = [s["stop_only_n3"]["Isum"] for s in strokes] +# 停止窗谁残速大：I_st>0 → 合速度偏正 → 镜像下更像 N3 仍正/N2 已减 +# 直接用 only_* 贡献绝对值 +sum_only2 = sum(abs(x) for x in only2) +sum_only3 = sum(abs(x) for x in only3) +n_only2 = sum(1 for x in only2 if abs(x) > 0.2) +n_only3 = sum(1 for x in only3 if abs(x) > 0.2) + +cruise_v2 = pa["cruise_mean_abs_v2"]["mean"] +cruise_v3 = pa["cruise_mean_abs_v3"]["mean"] +cruise_bias = pa["cruise_bias_v2_minus_v3"]["mean"] + +mean_on_clean = sum(on_clean) / len(on_clean) if on_clean else None +mean_Ion = sum(I_on) / len(I_on) if I_on else 0 +mean_Icr = sum(I_cr) / len(I_cr) if I_cr else 0 +mean_Ist = sum(I_st) / len(I_st) if I_st else 0 +abs_Ist = sum(abs(x) for x in I_st) / len(I_st) if I_st else 0 +abs_Ion = sum(abs(x) for x in I_on) / len(I_on) if I_on else 0 +abs_Icr = sum(abs(x) for x in I_cr) / len(I_cr) if I_cr else 0 + +# SVG 折线 +W, H = 960, 420 +PAD_L, PAD_R, PAD_T = 56, 20, 36 + + +def poly(xs, ys, y0, y1, top, h, color, w=1.3): + if not xs: + return "" + lo, hi = y0, y1 + if hi - lo < 1e-9: + hi = lo + 1 + + def X(t, t0, t1): + span = max(t1 - t0, 1e-6) + return PAD_L + (t - t0) / span * (W - PAD_L - PAD_R) + + def Y(v): + return top + (hi - v) / (hi - lo) * h + + pts = " ".join(f"{X(xs[i], xs[0], xs[-1]):.1f},{Y(ys[i]):.1f}" for i in range(len(xs))) + return f'' + + +if sample: + ts = [p["t"] for p in sample] + v2 = [p["v2"] for p in sample] + v3 = [p["v3"] for p in sample] + vsum = [p["vsum"] for p in sample] + Isum = [p["Isum"] for p in sample] + t0, t1 = ts[0], ts[-1] + vmin = min(min(v2), min(v3)) + vmax = max(max(v2), max(v3)) + imin = min(min(Isum), 0) + imax = max(max(Isum), 0) + padv = (vmax - vmin) * 0.08 or 10 + padI = (imax - imin) * 0.1 or 2 + + def X(t): + return PAD_L + (t - t0) / max(t1 - t0, 1e-6) * (W - PAD_L - PAD_R) + + def Yv(v): + return 48 + (vmax + padv - v) / (vmax - vmin + 2 * padv) * 100 + + def Ys(v): + return 190 + (vmax + padv - v) / (vmax - vmin + 2 * padv) * 80 + + def YI(v): + return 300 + (imax + padI - v) / (imax - imin + 2 * padI) * 80 + + def line(vals, yfn, color, w=1.3): + pts = " ".join(f"{X(ts[i]):.1f},{yfn(vals[i]):.1f}" for i in range(len(ts))) + return f'' + + # 行程分界竖线 + vlines = [] + for sm in data.get("stroke_marks") or []: + if sm["t0"] <= t1: + x = X(sm["t0"]) + vlines.append( + f'' + ) + vlines.append( + f'#{sm["i"]}' + ) +else: + ts = v2 = v3 = vsum = Isum = [] + t0 = t1 = 0 + vlines = [] + +# 标注箭头：在图上用文字+简单箭头路径 +notes_svg = [] +if sample: + # 找几个典型点 + # 1) 巡航：|v| 稳定处 + # 2) 停车：vsum 尖峰 + peak_i = max(range(len(sample)), key=lambda i: abs(sample[i]["vsum"])) + # 理论零线 + notes_svg.append( + f'' + f'对接电机 2/3：速度、合速度 v2+v3、积分 ∫(v2+v3)dt（虚线为理论 0）' + ) + +# 表格行 +rows_html = [] +for s in strokes: + cls_on = "bad" if abs(s["on_ms_n3_minus_n2"]) > 50 else "ok" + cls_cr = "ok" + cls_st = "warn" if abs(s["I_stop"]["Isum"]) > 3 else "ok" + who = "" + if abs(s["stop_only_n3"]["Isum"]) > abs(s["stop_only_n2"]["Isum"]) and abs(s["stop_only_n3"]["Isum"]) > 0.2: + who = "N3 残速" + elif abs(s["stop_only_n2"]["Isum"]) > 0.2: + who = "N2 残速" + else: + who = "两台接近同步刹停" + rows_html.append(f""" + + #{s['i']}{s['dir_n2']} + {s['on_ms_n3_minus_n2']:+.2f} ms + {s['cruise']['mean_abs_v2']:.2f} / {s['cruise']['mean_abs_v3']:.2f} + {s['I_on']['Isum']:+.2f} + {s['I_cruise']['Isum']:+.2f} + {s['I_stop']['Isum']:+.2f} + {s['stop_only_n2']['Isum']:+.2f} + {s['stop_only_n3']['Isum']:+.2f} + {who} + {s['da2_plus_da3']:+.2f}° + """) + +# 通俗结论文本 +if abs_Ist > abs_Icr * 3 and abs_Ist > abs_Ion: + main_src = "停车阶段" +elif abs_Ion > abs_Ist and abs_Ion > abs_Icr: + main_src = "行程衔接/起动窗口（含上一段未停稳的重叠）" +else: + main_src = "起动与停车都有贡献，巡航最干净" + +plain_html = f""" +
+

通俗结论：到底是谁慢？

+

先说人话：两台电机并不是“有一台全程都慢”。 + 你看到的积分差，主要不是巡航时谁转得慢，而是快停下 / 刚要走的那几十一到几百毫秒里，两台对不齐， + 把理论应为 0 的 ∫(v2+v3)dt 顶出 5～20° 左右的偏差。

+ +
    +
  • 启动 + 干净起动时（|Δt|≤50ms 的行程），N3−N2 约为 + {mean_on_clean if mean_on_clean is not None else '—'} ms 量级 → + 几乎是同时启动，ECAT 同帧下发没有问题。 + 有 {len(on_overlap)} 个行程测出上百毫秒的“假早启动”，那是上一段还没停稳、N3 仍在动， + 不是这一段命令发晚了。
    • +
  • 巡航 + 两边平均 |v|：N2 {cruise_v2:.2f}°/s，N3 {cruise_v3:.2f}°/s， + 差值约 {cruise_bias:.3f}°/s → 稳态速度几乎一样，谈不上谁慢。 + 巡航段 ∫(v2+v3) 平均只有约 {mean_Icr:.2f}°，贴合理论 0。
    • +
  • 停车 / 换向 + 行程末端 ∫(v2+v3) 平均约 {mean_Ist:.2f}°，绝对值平均约 {abs_Ist:.2f}°， + 明显大于巡航。停止窗口里： + 出现「只有 N2 还在走」的行程约 {n_only2} 次（积分贡献合计 {sum_only2:.2f}°）， + 「只有 N3 还在走」约 {n_only3} 次（合计 {sum_only3:.2f}°）。 + 两边都有刹车尾巴，不是单边永久变慢；有的行程 N2 拖尾更明显，有的行程 N3 更明显。
    • +
+ +
+ 一句话：积分差的大头在 {main_src}。 + 对接约束要求 v2 = −v3；只要刹停瞬间一台还带着速度、另一台已经接近 0， + v2+v3 就会冒尖峰，积分就会被拉离理论 0。这属于动态不同步， + 不等于某台电机“标定转速低、一直慢”。 +
+ +
+
正常：同时启动 / 巡航一致 / 中段积分≈0
+
注意：停车沿残速、行程衔接重叠、积分被顶开
+
偏大：该行程 ∫(v2+v3) 或编码器 Δ(a2+a3) 明显偏离 0
+
+
+""" + +rows_joined = "\n".join(rows_html) + +# 标注框（放在 SVG 下方用 HTML 箭头说明） +annot_html = f""" +
+

图上怎么看（对照上方曲线）

+
+
+
↑ 巡航段（两条速度曲线几乎贴合）
+

N2 蓝线、N3 橙线在 |v|≈200°/s 附近重合 → 都不慢。 + 下方红线 v2+v3 贴 0，积分曲线几乎水平 → 对接约束在稳态是成立的。

+
+
+
↓ 停车沿 / 换向点（红线尖峰）
+

接近终点时，一台速度先掉下来、另一台还在走 → v2+v3 出现 ±100～200°/s 尖峰。 + 积分曲线在这里爬上或爬下，行程结束就停在理论 0 以外几度到二十度。 + 这里是积分差的主要来源。

+
+
+
↓ 行程刚衔接时（起动窗）
+

若上一段没完全停稳就反向，会看到“N3 早就动了”的假象（表格里 onΔ 为 −800ms 量级）。 + 这不是启动指令不同步，而是速度还在反向爬坡 / 残速， + 也会把起动窗积分抬高一点。

+
+
+
↓ 紫色角度和 a2+a3
+

理论应为常数（理想为 0）。行程终点若偏到 ±10～25°，与该行程 ∫(v2+v3) 同量级 → + 说明偏差是真实位移耦合误差，不只是速度噪声。

+
+
+ +
+ 标定建议（若要压积分差）： + ① 先统一两台的减速段（同样斜率刹停），而不是去改巡航速度； + ② 检查 N2/N3 负载与制动是否对称； + ③ 控制器上对对接轴做 v2+v3 → 0 的纠偏，比单轴调 PID 更对症； + ④ 序列 hold 不要短于“两台都确认停稳”的时间，避免重叠行程把积分搅浑。 +
+
+""" + +# SVG 图 +if sample: + svg = f""" + + + {''.join(vlines)} + {line(v2, Yv, '#4da3ff')} + {line(v3, Yv, '#f0a030')} + 速度 deg/s — 蓝 N2 / 橙 N3 + + {line(vsum, Ys, '#ff5c5c')} + 合速度 v2+v3（理论 0）— 停车尖峰在这里 + + {line(Isum, YI, '#b48cff')} + 积分 ∫(v2+v3)dt（理论 0）— 每段末的偏离就是耦合误差 + + t_rel {t0:.1f}..{t1:.1f}s · ECAT 0.25ms · 竖线为行程起点 + + + ↑ 巡航：两线贴合，都不慢 + ↓ 停车沿：v_sum 尖峰 → 积分爬升 + +""" +else: + svg = "

（无绘图样本）

" + +html = f""" + + + +对接电机 2/3 积分差归因报告 + + + +

对接电机 2 / 3：积分差是谁造成的？

+
+ 数据：ECAT 无源采集 lab/ecat_passive_latest.csv · 周期 0.25ms · + 理论：刚性镜像对接时 v2+v3≡0，∫(v2+v3)dt≡0，a2+a3=常数
+ 方法：对齐 cycles 后做梯形积分；先加后积与先积后加一致。 +
+ +
+

关键数字

+
+
巡航 |v| N2 / N3
+
{cruise_v2:.2f} / {cruise_v3:.2f}
+
巡航段 ∫(v2+v3) 平均
+
{mean_Icr:.2f}°
+
停车段 ∫(v2+v3) |均值|
+
{abs_Ist:.2f}°
+
干净起动 Δt 中位/均值
+
{(sorted(on_clean)[len(on_clean)//2] if on_clean else float('nan')):.2f} / {(mean_on_clean if mean_on_clean is not None else float('nan')):.2f} ms
+
+
+ +{plain_html} + +
+

曲线（带标注）

+ {svg} +
+ +{annot_html} + +
+

各行程明细（积分从行程起点重算）

+ + + + + + + + + + {rows_joined} + +
#方向起动Δ(N3-N2)巡航|v| N2/N3∫起动∫巡航∫停车停时仅N2在动停时仅N3在动停车归因Δ(a2+a3)
+

+ 单位：角度 deg（积分）· 速度 deg/s · 时间 ms。 + 「停时仅N2/N3在动」为该窗口内单边残速对 ∫(v2+v3) 的贡献。 +

+
+ +
+

原始结论脚本输出（备查）

+ 
{json.dumps(verdict, ensure_ascii=False, indent=2)}
+
+ + +""" + +OUT.write_text(html, encoding="utf-8") +print("HTML ->", OUT) +print("main_src:", main_src) +print("on_clean", on_clean, "mean", mean_on_clean) +print("I_on/I_cr/I_st abs mean", abs_Ion, abs_Icr, abs_Ist) +print("only2", n_only2, sum_only2, "only3", n_only3, sum_only3) diff --git a/scripts/test_motor_pair_n2_n3.py b/scripts/test_motor_pair_n2_n3.py new file mode 100644 index 0000000..c9a81c2 --- /dev/null +++ b/scripts/test_motor_pair_n2_n3.py @@ -0,0 +1,1132 @@ +#!/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(" 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("= 13 else None, + "ucur": struct.unpack_from("= 6 else None, + "rt_cur": struct.unpack_from("= 25 else 0.0, + "motT": struct.unpack_from("= 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("= 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())