以 ESP32-S3 為運動控制器，3 個 MG90S 驅動三段活動臂（肩 / 肘 / 腕）， 1 個 MG90S 驅動抓夾，每段活動臂長 40 cm，控制器與執行器分處 兩條獨立的 RS485 總線。
┌───────────────────────────────────────┐
│ ESP32-S3 │
│ │
┌────────────┐ │ Bus A (UART1) Bus B (UART2) │
│ 上位機/PLC │◄────►│ 從站 115200 主站 250000 │
│ (PC) │ A/B │ │
└────────────┘ │ MCPWM ──► GPIO4/5/6/7 │
USB-RS485 │ （本地 PWM 後端時直接驅動舵機）│
└───────────────────────────────────────┘
│ A/B
┌──────────────┬───────────────┼──────────────┐
┌───┴───┐ ┌───┴───┐ ┌───┴───┐ ┌───┴───┐
│Node 1 │ │Node 2 │ │Node 3 │ │Node 4 │
│肩 J1 │ │肘 J2 │ │腕 J3 │ │抓夾 │
│MG90S │ │MG90S │ │MG90S │ │MG90S │
└───────┘ └───────┘ └───────┘ └───────┘
0x01 0x02 0x03 0x04
| Bus A（控制器總線） | Bus B（執行器總線） | |
|---|---|---|
| 角色 | ESP32-S3 作從站 | ESP32-S3 作主站 |
| 對端 | 上位機 / PLC / HMI | 4 個執行器節點 |
| 波特率 | 115200 | 250000 |
| 內容 | 「做什麼」：關節角、位姿、抓夾、使能、回零 | 「怎麼執行」：4 路脈寬 + 節點反饋 |
| 週期 | 事件驅動（上位機輪詢） | 100 Hz 廣播同步寫 + 輪詢讀 |
分開的理由：
- 實時性隔離——執行器側要 100 Hz 同步下發，不能因為上位機的長幀或 偶爾的通信阻塞而抖動。
- 波特率差異——上位機距離遠、速率低；執行器距離近、要求高吞吐。
- 終端與隔離——兩條總線可以各自加 120 Ω 終端電阻與隔離收發器， 一條短路不會拖垮另一條。
- 帶寬核算——Bus B 每週期廣播 1 幀（18 B）+ 輪詢 1 個節點（請求 8 B
- 應答 ~12 B），100 Hz 下約 7.6 kbps，250 kbps 上有充足裕量。
按目前的機械參數，這套臂在水平伸出時無法舉起自身重量。 這是結構性問題， 不是軟體能解決的，請務必先看這一節。
水平全伸（最惡劣工況）時肩關節的靜態力矩：
τ₁ = g · [ m₁·L₁/2 + m₂·(L₁+L₂/2) + m₃·(L₁+L₂+L₃/2) + m_p·(L₁+L₂+L₃) ]
當 L₁ = L₂ = L₃ = L、三桿等質量 m 時可化簡為：
τ₁ = g · L · (4.5·m + 3·m_p)
取默認參數 m = 0.1 kg、m_p = 0.1 kg（這兩個數值是估算，務必實測修正）：
| 單段臂長 L | 肩部所需力矩 | 肘部 | 腕部 | MG90S 連續可用 |
|---|---|---|---|---|
| 400 mm（本項目） | 30.0 kgf·cm | 16.0 | 6.0 | 0.59 |
| 300 mm | 22.5 kgf·cm | 12.0 | 4.5 | 0.59 |
| 200 mm | 15.0 kgf·cm | 8.0 | 3.0 | 0.59 |
| 100 mm | 7.5 kgf·cm | 4.0 | 1.5 | 0.59 |
MG90S 堵轉力矩約 1.8 kgf·cm（4.8 V），連續工作按 1/3 降額僅 0.59 kgf·cm。 也就是說 40 cm 臂在水平伸出時超載約 50 倍。
固件上電時會自動打印這份核算（arm_log_torque_report()），改完
ARM_LINK*_MASS_KG 後看串口日誌即可，不必自己手算：
W (1234) arm: ===== 靜力矩核算 (估算值，務必實測桿件後修正) =====
W (1234) arm: MG90S: 堵轉 1.80 kgf·cm，連續可用按 1/3 計 = 0.59 kgf·cm
W (1234) arm: 全伸水平 (0,0,0) 肩 30.0 / 肘 16.0 / 腕 6.0 kgf·cm <<< 超出 MG90S 能力
W (1234) arm: 待機姿態 肩 6.7 / 肘 13.7 / 腕 6.0 kgf·cm <<< 超出 MG90S 能力
W (1234) arm: 豎直停放 肩 0.0 / 肘 0.0 / 腕 0.0 kgf·cm
注意「豎直停放」(90°, 0°, 0°) 才是零重力矩姿態。若寫成 (90°, 0°, −90°)， 腕部連桿是水平的、末端甩出 400 mm，肩部仍有 6 kgf·cm—— 這是很容易踩的坑，
app_config.h裡已註明。
方案 A — 保留 40 cm，換大力矩舵機（推薦） 肩/肘換 20–35 kgf·cm 級數字舵機（如 DS3218、RDS3225、35 kg 級總線舵機）， MG90S 只留作腕部與抓夾。注意 20 kg 級舵機尺寸與 MG90S 不同，需重新設計法蘭。
方案 B — 保留 MG90S，大幅減重並加重力平衡
- 桿件改用碳纖管 + 薄壁 3D 打印件，做到每桿 ≤ 20 g；
- 肩部加拉簧或配重抵消靜態重力矩——這對靜態力矩極其有效，是性價比最高的做法；
- 運動範圍限制在接近豎直的區域（重力矩 ∝ cos θ）。
方案 C — 縮短臂長 臂長縮到 10–12 cm 且每桿 ≤ 20 g 時，MG90S 才勉強夠用（L=100 mm、 m=15 g、m_p=20 g 時肩部約 0.51 kgf·cm，接近 0.59 的連續可用值）。
無論選哪個方案，都請先實測桿件質量與質心位置，填回
main/include/app_config.h的ARM_LINK*_MASS_KG/ARM_PAYLOAD_MASS_KG， 讓過載告警（FAULT_OVERLOAD）有正確的判據。
完整硬件說明書（BOM / 接線 / 電源 / 標定 / 安全）見
docs/HARDWARE.md。
| 功能 | 引腳 | 說明 |
|---|---|---|
| 舵機 J1（肩） | GPIO4 | MCPWM，50 Hz |
| 舵機 J2（肘） | GPIO5 | MCPWM |
| 舵機 J3（腕） | GPIO6 | MCPWM |
| 舵機 抓夾 | GPIO7 | MCPWM |
| Bus A TXD | GPIO17 | UART1 → 收發器 DI |
| Bus A RXD | GPIO18 | UART1 ← 收發器 RO |
| Bus A DE | GPIO8 | 收發器 DE 與 ~RE 並接 |
| Bus B TXD | GPIO12 | UART2 → 收發器 DI |
| Bus B RXD | GPIO13 | UART2 ← 收發器 RO |
| Bus B DE | GPIO9 | 收發器 DE 與 ~RE 並接 |
| 舵機電源使能 | 可選 | SERVO_PWR_EN_GPIO，默認 −1（不使用） |
避開了 strapping 腳（GPIO0/3/45/46）、USB-JTAG（GPIO19/20）以及
Octal PSRAM/Flash 佔用的 GPIO26–37。改腳位請直接編輯
main/include/app_config.h。
ESP32-S3 GPIOx ──► 舵機信號線（橙/白）
外部 5 V 穩壓 ──► 舵機 VCC（紅） ← 千萬不要用開發板的 3V3/5V 引腳供電
共地 GND ──► 舵機 GND（棕） ← ESP32 與外部電源必須共地
4 個 MG90S 同時堵轉可達 2 A 以上，必須用獨立 5 V 電源（建議 ≥ 3 A）， 並在舵機側就近並聯 100–470 µF 電解電容。
GPIO(TXD) ──► DI
GPIO(RXD) ◄── RO
GPIO(DE) ──► DE ─┬─ 並接
~RE ─┘
VCC ──► 3V3（收發器邏輯電平）
A / B ──► 總線
- 總線兩端各接一個 120 Ω 終端電阻（中間節點不接）；
- 加失效保護偏置：A 上拉 4.7 kΩ 到 3V3、B 下拉 4.7 kΩ 到 GND， 避免總線空閒時 RX 收到亂碼；
- 兩條總線各自成鏈，不要交叉或形成星型分支。
ISDN2601_code/
├── CMakeLists.txt ESP-IDF 專案根
├── sdkconfig.defaults 目標晶片 / FreeRTOS 節拍等
├── docs/
│ └── HARDWARE.md 硬件說明書（BOM/接線/電源/標定/安全）
├── main/
│ ├── CMakeLists.txt
│ ├── app_main.c 入口：初始化兩條總線、建控制任務
│ ├── arm_controller.c/h 協議解析 + 狀態機 + 運動控制 + 安全監控
│ ├── kinematics.c/h 3R 正逆運動學、脈寬映射、靜力矩估算
│ ├── motion.c/h 逐軸梯形速度規劃
│ ├── protocol.c/h 幀格式、CRC16、流式解碼器
│ ├── rs485.c/h 半雙工總線驅動（DE 軟件控制）
│ ├── servo.c/h MG90S MCPWM 驅動
│ └── include/app_config.h 全部硬件引腳與機械參數
├── tests/
│ ├── Makefile 原生測試（不需要 ESP-IDF）
│ └── test_native.c
└── tools/
└── host_console.py 上位機調試工具
protocol.c / kinematics.c / motion.c 刻意不引用任何 ESP-IDF 頭文件，
因此可以直接在 PC 上編譯測試——這是整個項目能在沒有硬件的情況下被驗證的關鍵。
需要 ESP-IDF v5.0+（建議 v5.2 / v5.3）：
. $HOME/esp/esp-idf/export.sh # 或 source 你的 IDF 路徑
cd ISDN2601_code
idf.py set-target esp32s3
idf.py build
idf.py -p /dev/cu.usbmodem1101 flash monitor本工作區當前未安裝 ESP-IDF，因此固件未經實機編譯。已做的驗證見第 8 節。
切換執行器後端（默認本地 PWM）：
# 舵機由 Bus B 上的智能節點驅動，而不是直接接 ESP32-S3 的 MCPWM
idf.py build -DAPP_ACT_BACKEND=1或直接在 main/CMakeLists.txt 裡加：
target_compile_definitions(${COMPONENT_LIB} PRIVATE APP_ACT_BACKEND=1)┌──────┬──────┬─────┬─────┬────┬─────┬──────────┬───────┐
│ SOF0 │ SOF1 │ DST │ SRC │ FC │ LEN │ PAYLOAD │ CRC16 │
│ 0xAA │ 0x55 │ 1B │ 1B │ 1B │ 1B │ 0..64 B │ 2B LE │
└──────┴──────┴─────┴─────┴────┴─────┴──────────┴───────┘
CRC16 計算範圍（DST → PAYLOAD 末尾）
- CRC16 = CRC-16/MODBUS（多項式 0xA001，初值 0xFFFF，低字節在前），
標準向量
"123456789"→0x4B37； - 站號：
0x00廣播、0x01–0x04執行器節點、0x10機械臂控制器、0x40上位機； - 解碼器是逐字節狀態機，幀被拆包、前面有垃圾數據或出現假 SOF 都能自動重同步。
| 功能碼 | 名稱 | 請求載荷 | 應答載荷 |
|---|---|---|---|
| 0x01 | PING | — | err u8, uptime u32, seq u16, tick_hz u16 |
| 0x02 | READ_STATUS | — | state u8, fault u16, warn u16, grip_mode u8, grip u16, vin u16, uptime u32, seq u16, nodes u16 |
| 0x03 | WRITE_JOINTS | mask u16, j1/j2/j3 i16, grip u16, move_time u16 |
ACK/NACK |
| 0x04 | READ_JOINTS | — | j1/j2/j3 i16, load1..3 i16, grip u16, nodes u16 |
| 0x05 | WRITE_POSE | x i16, y i16, phi i16, grip u16, move_time u16 |
ACK/NACK |
| 0x06 | READ_POSE | — | x i16, y i16, phi i16, grip u16 |
| 0x10 | SET_ENABLE | enable u8 |
ACK/NACK |
| 0x11 | HOME | — | ACK/NACK |
| 0x12 | SET_GRIP | cmd u16, mode u8(可選) |
ACK/NACK |
| 0x13 | SET_LIMITS | axis u8, vmax u16, amax u16 |
ACK/NACK |
| 0x20 | READ_FAULT | — | fault u16, warn u16, state u8 |
| 0x21 | CLEAR_FAULT | — | ACK/NACK |
| 0x7E / 0x7F | ACK / NACK | err u8, uptime u32 |
— |
線路單位：
| 量 | 單位 | 編碼 |
|---|---|---|
| 笛卡爾座標 | 0.1 mm | i16 |
| 關節角 / 俯仰角 | 0.01 deg | i16 |
| 負載估算 | 0.1 %（堵轉力矩佔比） | i16 |
| 抓夾開合 | 0..1000 ‰ | u16 |
| 時間 | ms | u16 |
| 限速 | 0.1 單位/s、0.1 單位/s² | u16 |
所有多字節字段均為小端。
| 值 | 含義 |
|---|---|
| 0 | OK |
| 1 | 逆解失敗：目標不可達 |
| 2 | 超出關節限位 |
| 3 | 尚未回零 |
| 4 | 處於故障閉鎖態，需 CLEAR_FAULT |
| 5 | 舵機未使能，需 SET_ENABLE 1 |
| 6 | 載荷長度/格式錯誤 |
| 7 | 不支持的功能碼 |
fault 為閉鎖類（會停止運動，需 CLEAR_FAULT 解除）：
BUS_A_TIMEOUT(0x0004)、ESTOP(0x0080)。
warn 為告警類（不停止運動，僅上報）：
IK_UNREACHABLE(0x0001)、JOINT_LIMIT(0x0002)、BUS_B_TIMEOUT(0x0008)、
NODE_CRC(0x0010)、OVERLOAD(0x0020)。
OVERLOAD 只告警不閉鎖是有意的：按當前的機械參數它幾乎常亮，
若做成閉鎖機械臂將完全無法動作。
- 每 10 ms 廣播一次
WRITE_JOINTS（dst=0x00，mask=0x0F）， 4 個節點在同一時刻動作——這比逐節點點對點下發更實時、帶寬更省； - 每 10 ms 輪詢一個節點的
READ_JOINTS，4 個節點輪轉 → 每節點 25 Hz 反饋； - 節點無應答時置
BUS_B_TIMEOUT告警並標記離線。
pip install pyserial
python3 tools/host_console.py /dev/cu.usbserial-A50285BI ping
python3 tools/host_console.py PORT status
python3 tools/host_console.py PORT enable 1
python3 tools/host_console.py PORT joints 120 -80 -40 1000 1500
python3 tools/host_console.py PORT pose 500 600 0 500 2000
python3 tools/host_console.py PORT grip 800 mirror
python3 tools/host_console.py PORT home
python3 tools/host_console.py PORT watch # 20 Hz 連續監視
python3 tools/host_console.py PORT demo # 搬運演示若 USB-RS485 適配器會自環收到自己發出的字節，加 --echo。
MG90S 是無位置反饋的模擬舵機，不標定直接上電會直接撞限位。請按順序做：
- 先不裝臂，只接舵機，
ARM_HOME_*設成中位對應的關節角； enable 1使能，用joints逐軸小幅度點動，觀察實際轉向；- 某軸方向反了 → 把
app_config.h裡對應的SERVO_Jx_US_PER_DEG取負 （改符號即可，不必改機械或接線）； - 機械裝配的零位誤差 → 調
SERVO_Jx_TRIM_DEG； - 行程不匹配（脈寬 500–2500 µs 對應的角度不是 180°）→ 調
SERVO_MIN_US/SERVO_MAX_US； - 實測桿件質量與質心，填回
ARM_LINK*_MASS_KG/ARM_PAYLOAD_MASS_KG； - 抓夾：用
grip找出完全閉合與完全張開的脈寬，填入GRIP_CLOSED_US/GRIP_OPEN_US。
回零：MG90S 沒有絕對位置反饋，
HOME命令只是「規劃到待機姿態」， 不是真正意義上的回零。需要真正的回零請加限位開關或絕對編碼器。
cd tests && make會在 PC 上編譯 protocol.c / kinematics.c / motion.c 並運行 94 項斷言：
| 覆蓋範圍 | 驗證內容 |
|---|---|
| CRC16 | 標準向量 0x4B37 |
| 組幀/解幀 | 往返一致、單位翻轉被 CRC 攔下、垃圾前綴後能重同步 |
| 正運動學 | (0,0,0)→1200mm、(90,0,0)→1200mm 高等解析解 |
| 逆運動學 | 4 組姿態 × 2 個肘部分支，FK(IK(pose)) == pose 閉合且落在限位內 |
| 軟限位 | 越限判定與鉗位 |
| 靜力矩 | 水平全伸手算值 30/16/6 kgf·cm；並把超載固化成一條斷言 |
| 速度規劃 | 90° 長行程 = 梯形剖面 1.75 s、峰值速度 60°/s；5° 短行程 = 三角剖面 0.29 s；正反兩個方向都無過衝 |
| 脈寬映射 | 中位 1500 µs、±85° → 2444/556 µs、溢出鉗位、方向反轉 |
| 抓夾映射 | 三種模式的脈寬與雙顎鏡像張角 |
本機沒有 ESP-IDF 工具鏈，因此用一套 ESP-IDF 頭文件樁對全部 7 個源文件做了
gcc + clang 雙編譯器的 -fsyntax-only -Wall -Wextra 語法與類型檢查，
兩種執行器後端（LOCAL / RS485）均零警告通過。
Python 上位機生成的 WRITE_POSE 幀，餵給 C 固件解碼器解析出的字段
（x=500.0mm y=600.0mm phi=0.00deg grip=500 ms=2000）與 Python 端完全一致，
證明兩側的字節序、單位換算、CRC 實現對齊。
上述三項都不能替代實機聯調。上電前務必先做第 2 節的力矩核算與第 8 節的標定。
為什麼用 MCPWM 而不是 LEDC 驅動舵機 MCPWM 時基可設成 1 MHz，比較值直接就是脈寬微秒數，分辨率 1 µs（≈0.09°@MG90S）； LEDC 在 50 Hz 下受限於定時器位寬，脈寬分辨率只有 2–4 µs，且週期有 0.1%–0.4% 誤差。
為什麼 DE 用普通 GPIO 軟件控制
便於在發送前後插入收發轉換延時（RS485_DE_SETTLE_US），也便於換用不同收發器。
發送流程：DE=1 → uart_write_bytes → uart_wait_tx_done → 延時 → DE=0，
既避免總線衝突，也避免自己的發送被自己回收。
運動規劃為什麼這樣寫
逐軸梯形規劃，用「步末速度必須落在剎車曲線上」的隱式解限速
（v_new = (√(b² + 8ad − 4bv) − b) / 2，b = a·dt）。若改用「剎停距離 ≥ 剩餘距離
就減速」這種二值判據，離散時間下會先施加整步加速、下一步才發現要減速，
峰值速度衝過理論切換點，短行程可提前約 10%–13% 到位。配合梯形積分
(v_old+v_new)/2·dt，長短行程的到位時間都與連續時間解析解吻合。
抓夾的三種映射模式
DIRECT：開合指令 0–1000 ‰ 線性映射到脈寬；MIRROR（默認）：單舵機經連桿帶動雙顎對稱開合，左顎 +δ / 右顎 −δ 鏡像；LEVEL：映射補償——把腕部俯仰角 φ 映射進抓夾伺服，使顎口在世界座標中的 指向保持GRIP_LEVEL_REF_DEG不變。若只用一個舵機同時負責開合與調平， 兩者會互相耦合，需要按行程比例分配。
安全設計
- 控制器總線看門狗（默認 1500 ms）：超時凍結在當前設定值並閉鎖，需
CLEAR_FAULT； - 關節軟限位：笛卡爾指令逆解後會再校驗限位，超限直接 NACK，不下發；
- 失能時把所有 PWM 腳強制拉低（
mcpwm_generator_set_force_level(gen, 0, true)）， 舵機失去脈衝即不出力； - 故障閉鎖期只放行只讀命令與
CLEAR_FAULT； - 週期性主動遙測默認關閉（
CTRL_UNSOLICITED_TELEMETRY=0）——半雙工從站 主動上報會與上位機的請求撞車，僅點對點時才建議開啟。
- 未經實機驗證——本機沒有 ESP-IDF，固件只做了語法/類型檢查與數學驗證， 尚未在 ESP32-S3 上跑過。
- 無真實位置反饋——MG90S 是開環模擬舵機，
READ_JOINTS回傳的是規劃設定值， 不是實際角度。本地 PWM 後端下的「負載」是靜力矩估算值，不是實測電流。 - 無回零傳感器——
HOME只是規劃到待機姿態。 - 無基座回轉軸——3 個活動關節構成豎直平面內的 3R 臂（x, y, φ）， 工作空間局限在一個平面內。如需水平回轉需增加第 5 個舵機與第 4 個關節軸。
- 關節行程受舵機限制——MG90S 只有約 180°，單關節行程被限制在 170° 以內
（見
ARM_Jx_MIN/MAX_DEG）。 - Bus B 節點固件未提供——控制器側的主站邏輯（廣播同步寫 + 輪詢讀）已實現， 但執行器節點本身的固件需要另行開發（可用 ESP32-C3/C6 或 STM32， 協議見第 6 節）。默認使用本地 PWM 後端，不需要節點即可運行。
- 無電壓/電流採集——
vin_mV恆為 0。如需電池供電下的低壓保護， 需加 ADC 分壓採樣。