手邊有一台步進馬達平衡車（HJ-R3 主板：兩顆 A4988、MPU6050、2 顆 18650），原本接的是傳統 ESP32。這次把控制板換成 ESP32-S3 N16R8，改成 core 0 跑 WiFi／OTA／調參網頁、core 1 跑 200 Hz 平衡控制 的雙核架構。這篇把從燒錄到第一次站穩一分鐘的調適步驟、每一步的實測數據和踩到的坑整理下來。
一、架構：兩顆核心各做各的
| 核心 | 負責 |
|---|---|
| Core 1 | MPU6050 data-ready 中斷每 5 ms 觸發 → 互補濾波算傾角 → 角度環（200 Hz）＋速度環（40 Hz）→ FastAccelStepper 用 MCPWM 硬體產生步進脈衝 |
| Core 0 | WiFi、ArduinoOTA、調參網頁（即時看角度、改 PID 不用重燒、存 NVS） |
OTA 的安全設計：寫 Flash 時兩顆核心都會停，所以平衡中不收 OTA；OTA 一開始先關 A4988 的 ENABLE；新韌體開機後 IMU 正常跑 10 秒才確認，讀不到 IMU 就自動回滾到舊版。實測 OTA 寫入期間控制迴圈最長被卡住 426 ms，證明這個設計是必要的。
二、腳位：S3 有一半不能照搬
ESP32-S3 沒有 GPIO 22～25，N16R8 模組的 26～32 接 Flash、33～37 接 octal PSRAM，原本的腳位幾乎都要換。主板是用杜邦線接到開發板，所以只要改線、不用動主板。
| 訊號 | 原 ESP32 | ESP32-S3 |
|---|---|---|
| MPU6050 SDA / SCL / INT | 22 / 21 / 18 | 8 / 9 / 10 |
| 左輪 STEP / DIR | 32 / 33 | 4 / 5 |
| 右輪 STEP / DIR | 14 / 12 | 6 / 7 |
| A4988 ENABLE | 25 | 15 |
| 電池電壓（新加） | — | 1（ADC1） |
三、程式架構
韌體是 Arduino sketch（esp32 core 3.3.12＋FastAccelStepper 2.0.1），約 700 行，依「誰在哪顆核心跑」切檔案：
| 檔案 | 跑在哪 | 內容 |
|---|---|---|
config.h |
— | 腳位、MPU 安裝方向、迴圈頻率、PID 預設值（Params 結構） |
shared.h |
兩核共用 | 狀態列舉、遙測（Telemetry）、指令（Command）、spinlock |
imu.cpp |
core 1 | MPU6050 直接讀暫存器：±4g、±500°/s、DLPF 42 Hz、取樣 200 Hz、data-ready 中斷；讀不到時印診斷 |
control.cpp |
core 1 | 平衡控制任務、狀態機、步進輸出、OTA 健康檢查 |
net.cpp |
core 0 | WiFi、mDNS、ArduinoOTA、調參網頁與 REST、參數存 NVS |
balance_car_s3.ino |
core 1（loop） | 開機順序、每秒量電池＋低電量保護、序列埠輸出 |
任務與優先權
| 核心 | 任務（優先權） | 做什麼 |
|---|---|---|
| Core 0 | WiFi 協定堆疊（系統） | ESP32 的 WiFi 本來就跑在 core 0 |
| net（2） | WebServer（/status、/set…）、ArduinoOTA.handle()、斷線重連，每 5 ms 一輪 | |
| Core 1 | control（configMAX_PRIORITIES − 2） | MPU6050 INT 每 5 ms 喚醒一次，跑完整個控制迴圈 |
| FastAccelStepper 引擎 | 管理速度斜坡，實際脈衝由 MCPWM／PCNT 硬體產生 | |
| Arduino loop（最低） | 每秒量電池、低電量保護、序列埠輸出 |
兩顆核心之間只透過 g_params／g_cmd／g_tel 三個結構交換資料，用 portMUX spinlock 保護。
重點有三個：
- 控制迴圈由硬體中斷驅動：MPU6050 每 5 ms 拉一次 INT，ISR 只做
vTaskNotifyGiveFromISR()，控制任務用ulTaskNotifyTake()等它。這樣取樣和控制完全同步，不靠delay()。中斷 10 ms 沒來會退回輪詢並記一筆 imuErr。 - 步進脈衝不佔 CPU：FastAccelStepper 用 S3 的 MCPWM＋PCNT 硬體產生脈衝，控制任務每 5 ms 只更新一次目標速度，WiFi 中斷再忙也不會讓步進抖。
- 兩核之間只交換三個小結構，進出都用
portENTER_CRITICAL(&g_mux)整份複製，臨界區只有幾十個 byte 的 memcpy：
Params g_params; // net 寫（/set、NVS），control 讀：kp kd ksp ksi trim alpha vmax amax
Command g_cmd; // net 寫（/cmd、/test），control 讀：speed turn enabled testL testR
Telemetry g_tel; // control 寫，net／loop 讀：state angle gyro speed acc[3] loops overruns …
volatile bool g_otaRequested; // OTA 開始時 net 設 true，control 看到就關馬達控制迴圈（每 5 ms）
等 INT → 複製 Params/Command → 讀 MPU6050
→ 互補濾波：angle = α·(angle + gyro·dt) + (1−α)·atan2(accY, accZ)，α = 0.98
→ 狀態機決定要不要出力
→ 速度環（每 5 次＝40 Hz）：targetAngle = −(ksp·(v − v設定) + ksi·∫)
→ 角度環（200 Hz）：acc = kp·(angle − trim − targetAngle) + kd·gyro
speed += acc·dt（限制在 ±vmax，acc 限制在 ±amax）
→ 左輪 = speed + turn、右輪 = speed − turn → FastAccelStepper
→ 寫回遙測（含這圈花了幾 µs、有沒有超過 5 ms）狀態機
| 狀態 | 進入條件 | 馬達 |
|---|---|---|
| idle | 開機、網頁按「停用馬達」、馬達測試結束 | 斷電（EN 高） |
| running | 允許平衡，且扶正到 trim ±5° 內維持 0.5 秒 | 平衡控制 |
| fallen | 傾角超過 ±45° | 立刻 forceStop＋斷電；再扶正會自動回 running |
| test | /test 指令，非平衡中才准 | 照指定速度轉到時間到 |
| ota | OTA 開始（g_otaRequested） | 斷電，直到重開機 |
| fault | MPU6050 或步進引擎初始化失敗 | 斷電，網路照開方便看狀態、重新 OTA |
網頁與 API（core 0）
| 路徑 | 用途 |
|---|---|
/ |
調參頁：每 200 ms 更新狀態、改參數、允許／停用平衡、馬達測試按鈕 |
/status |
JSON：state、angle、gyro、target、speed、batt、battLow、acc[3]、gyro3[3]、loops、imuErr、overruns、maxLoopUs、rssi、reset（重開原因）、uptime、params |
/set?kp=..&kd=.. |
即時改參數（不寫 Flash，平衡中也能改） |
/save |
參數存 NVS；寫 Flash 會讓兩核暫停，所以平衡中回 409 拒絕 |
/cmd?en=1&speed=&turn= |
允許平衡、前進速度、轉向差速 |
/test?l=400&r=0&ms=1000 |
馬達測試（最多 3 秒、±1600 steps/s） |
OTA 流程
espota 送韌體
→ net：狀態是 running 就不呼叫 ArduinoOTA.handle()（上傳逾時，等於拒絕）
→ onStart：g_otaRequested = true，等 control 關馬達、進 ota 狀態（最多 0.5 秒）
→ 寫入另一個 app 分割區（app3M_fat9M_16MB：兩個 3 MB app 分割區）→ 重開機
→ 新韌體狀態是 PENDING_VERIFY（verifyRollbackLater() 回 true）
→ control：IMU 和中斷連續正常 10 秒 → esp_ota_mark_app_valid_cancel_rollback()
IMU 讀不到 → esp_ota_mark_app_invalid_rollback_and_reboot()參數調整的流程因此變成：網頁 /set 即時改 → 站穩後停用馬達 → /save 存 NVS；只有改程式才需要 OTA。
四、調適步驟
步驟 1：先確認 MPU6050 讀得到
第一次燒進去序列埠顯示「MPU6050 讀不到」。韌體裡加了診斷：讀 SDA／SCL 閒置電位、把其他腳開下拉找「被外部拉高的腳」、正反兩種接法各掃一次 I2C。結果兩條線都是 0、沒有任何腳被拉高——代表 MPU6050 模組根本沒電。原因是主板整個沒上電（S3 只吃 USB 的電）。上電後迴圈剛好每秒 200 次、單次最長 2.7 ms。
步驟 2：馬達測試（輪子架空）
網頁加了「左輪前／後、右輪前／後、兩輪前」按鈕，每次 400 steps/s 轉 1 秒。順便確認微步：1 秒轉 1/8 圈 → 400 ÷ (200 × 16) = 1/8，主板跳線是 16 微步。
步驟 3：確認傾角的軸和正負號
一開始前後傾只看到幾度的變化，懷疑軸設錯。在 /status 加上三軸原始加速度後，請人扶著車子前傾、後傾，錄到：
| 動作 | acc X | acc Y | acc Z |
|---|---|---|---|
| 直立 | -0.03 | +0.02 | 1.01 |
| 前傾 | -0.01 | -0.73～-0.89 | 0.48～0.69 |
| 後傾 | -0.02 | +0.85 | 0.53 |
前後傾是 Y 軸在變（原本讀的 X 其實是左右傾）。改成：傾角＝atan2(acc Y, acc Z)、陀螺儀用繞 X 軸，兩者都乘 -1，讓前傾為正。
步驟 4：第一次平衡——控制方式要對步進馬達
Pi 4 上跑一支小程式每秒抓 20 次 /status，把每次測試錄下來。下圖是四個階段的傾角曲線：
① 角度 PD 直接給輪速：最長站了 38 秒，但每次都會慢慢偏向一邊，輪速一路加到上限 4000 steps/s 追不回來而倒下。原因是步進馬達本身是「照指定速度轉」，要把傾斜 θ 的車身撐回來需要約 g·tanθ 的加速度；只給速度，輪子只是跟著傾角跑。
改法：角度環輸出加速度，再積分成輪速：
acc = kp × (angle − trim − targetAngle) + kd × gyroRate // steps/s²
speed += acc × dt // steps/s步驟 5：kd 調到阻尼夠
② kd=80：前後擺動約 1 秒一次、越擺越大，輪速在 ±4000 之間甩，矯枉過正倒向另一邊。kd/kp 只有 0.03 秒，阻尼太小。③ kd=250（kd/kp ≈ 0.1 秒）：第一次站超過一分鐘。
步驟 6：一直往後跑 → trim ＋ 速度環
kd=250 時平均傾角是 -3.9°：重心偏後，要往後傾才站得住，車子就一直往後開。處理方式：
- 速度環 PI：車速越快，目標傾角越往反方向（ksp=0.002 → 1000 steps/s 往回傾 2°；ksi=0.0001 消除長時間偏移）
- 開了 I 項之後實際平衡點穩定在約 -1.7°，trim 設 -1.8°
步驟 7：站 30 秒後「突然沒力」→ 失步
倒下那一刻的數據很清楚：程式在 0.15 秒內把輪速加到 4000，車身卻以 428°/s 自由倒下——指令有送出去，輪子沒跟上，是步進馬達失步。步進馬達轉越快扭力越小，電池只有 7～8V 更明顯。把輪速和加速度上限改成可調參數，降到 vmax=3000 steps/s、amax=30000 steps/s² 之後，④ 可以站到 68 秒。
步驟 8：加電池電壓監測
電池＋（開關之後）─ 8.3kΩ ─ GPIO1 ─ 1.5kΩ ─ GND，滿電 8.4V 時 GPIO1 只有約 1.3V。和三用電表對照：電表 7.48V、韌體讀 6.90V，乘上 1.084 校正後讀 7.41～7.45V。一開始設低於 6.8V 自動停止平衡，後來發現平衡中馬達抽電會把電壓拉低、誤停車，現在改成只偵測、不切馬達（低於 6.0V 只在網頁標 battLow 警告）。
⚠️ 8.3k 一定接電池端；接反 GPIO1 會吃到約 7V，S3 直接燒掉。
量出來才發現電池只有 7.48V（每顆 3.74V，約半電），已經低於 A4988 馬達電源規格的 8V——這也是失步的主因之一。
步驟 9：充飽電後反而大幅前後擺 → 速度環減弱
電池充到 8.09V 再測，3 次最久只站 29.6 秒，前後擺動變成每秒約 0.45 次的大慢擺，速度環把目標傾角一路推到 ±10° 上限。把速度環 ksp 0.002→0.0012、ksi 0.0001→0.00004，同時 vmax 3000→3500、amax 30000→45000，擺動回到每秒約 1 次，角度標準差降到 3.6°。
回頭把 bal4～bal7 全部 16 次倒下對一遍，模式完全一樣：車身晃到 7～15° → 輪速頂到 vmax → 0.2～1.5 秒內倒下。「突然沒力」其實多半是輪子已經全速、沒有餘裕再追。
步驟 10：角度環加硬 → 站 59 秒、晃 ±2°
kp 2500→3000、kd 250→300、vmax 4000，這次站了 59 秒、角度標準差 2.0°（目前最好），而且不是倒下，是被當時 6.5V 的低電壓保護停掉——平衡中電壓被拉低。參數已經存進 NVS。
再往下測，倒下後 /status 的 reset 變成 brownout：電池出力時電壓垮到 ESP32 都撐不住而重開，接著程式叫輪子全速、車子卻不動。閒置時讀 7.7V 看起來還好，負載下已經沒力，電池是現在的瓶頸。
五、實測影片與調參網頁
kp 3000／kd 300 這組參數的實測：站穩後用手推一下，車子會往前追、擺幾下再回到原地。
調參網頁在 core 0 跑，手機連同一個 WiFi 打開 http://車子IP/ 就能看。上半部每 200 ms 更新一次 /status：
- state／reset／uptime：目前狀態、上次重開原因（poweron、sw、brownout…）、開機秒數
- angle／gyro／target／speed：傾角、傾倒速度、速度環要的目標傾角、輪速
- batt／battLow：電池電壓和低電量警告
- loops／imuErr／overruns／maxLoopUs：控制迴圈跑了幾次、IMU 讀取錯誤、逾時次數、單次最長耗時（200 Hz 的預算是 5000 µs，實測約 2300 µs）
- params：目前生效的參數
下半部可以直接改參數按「套用」即時生效，「存進 NVS」寫進 Flash（平衡中會被拒絕，避免寫 Flash 時兩顆核心停住讓車倒），還有停用馬達、允許平衡和單輪馬達測試按鈕。
圖說：手機上的調參網頁
六、目前的參數
| 參數 | 值 | 意思 |
|---|---|---|
| kp | 3000 | 每 1° 傾角給多少加速度（steps/s²） |
| kd | 300 | 每 1°/s 傾倒速度給多少加速度 |
| ksp / ksi | 0.0012 / 0.00004 | 速度環 PI，輸出目標傾角 |
| trim | -1.8° | 機械平衡點 |
| vmax / amax | 4000 / 45000 | 輪速、加速度上限（16 微步：1.25 轉/s） |
七、踩到的坑
- OTA 回滾沒作用：Arduino core 的
verifyRollbackLater()是 C 函式，在 .ino 覆寫要寫extern "C"，不然新韌體一開機就被標成有效，永遠不會回滾。 - S3 開發板接 CH343 時，FQBN 不要加
CDCOnBoot=cdc，Serial 才會走 UART0 出到 CH343。 - 開機預設不平衡：要在網頁按「允許平衡」才會動馬達，避免一接上車子剛好直立就自己衝出去。
- 倒下後 S3 會重開機：reset reason 是 poweron 時懷疑電池座接點彈開，是 brownout 時就是電池出力時電壓垮掉。
八、下一步
- 換能放大電流的動力型 18650（或確認電池內阻），避免出力時 brownout
- 考慮 3S 電池讓 A4988 吃到 11V 以上（要先確認主板降壓電路吃得了 12.6V），或調高 A4988 電流
- 站穩後再加前進／轉向遙控
沒有留言:
張貼留言