顯示具有 步進馬達平衡車 標籤的文章。 顯示所有文章
顯示具有 步進馬達平衡車 標籤的文章。 顯示所有文章

2026年11月21日 星期六

DIY Robot Cars EP3: Two-Wheel Self-Balancing, from Ball-and-Beam PID to an ESP32-S3 Stepper Robot | 自造機器人車 EP3:兩輪自平衡,從球與橫樑 PID 到 ESP32-S3 步進平衡車

📺 影片版(約 19 分鐘,有章節可跳著看):本集整理成一支影片,細節在下方文字與各篇原文。
YouTube:https://youtu.be/B4_qJp1G4Tc(winson-DIY 頻道)

English summary: The final episode of the "DIY Robot Cars" series merges five posts into one story about two-wheel self-balancing: a 2023 ball-and-beam PID rig (HC-SR04 plus a servo), stepper motors with the A4988 and microstepping, the MPU6050 and a complementary filter, and a 2026 ESP32-S3 stepper balancing robot with core 0 running WiFi/OTA/web tuning and core 1 running a 200 Hz interrupt-driven control loop. The tuning log goes from "can't read the IMU" to a 59-second stand within ±2°, with the key lessons: a stepper needs acceleration (not speed) from the angle loop, a "sudden loss of power" was the stepper losing steps at its speed cap, and a half-empty 2S battery and brownouts were the real bottleneck.

這是「自造機器人車」系列的第三集,也是最後一集:兩輪自平衡。把 2023 年的球與橫樑 PID、步進馬達與 A4988、MPU6050,和 2026 年 ESP32-S3 步進平衡車整理成一條故事線。影片約 20 分鐘,有章節可以跳著看;程式全文在各篇原文。

本集章節

  1. 先學 PID:球與橫樑:HC-SR04 量位置、MG995 轉橫樑、PID_v1(Kp 0.8/Ki 0.005/Kd 0.3、REVERSE、輸出 ±25°)、伺服不對稱基準。→ Balance Ball and Beam Control system(2023/11)
  2. 零件:步進馬達、A4988、MPU6050:微步表、3200 步一圈、AccelStepper vs FastAccelStepper、互補濾波。→ ESP32 control stepper motor(2023/12)、ESP32 I2C Bus Scanner – MPU6050(2023/07)、ESP32結合MPU6050用Processing做動態演示(2023/01)
  3. 平衡車架構:ESP32-S3 雙核:腳位重配、core 0 網路/core 1 200 Hz 控制、INT 驅動、狀態機、OTA 安全與回滾。→ ESP32-S3 步進平衡車(2026/11)
  4. 調參:從站不住到站一分鐘:主板沒上電、16 微步驗證、軸與正負號、PD 給加速度、kd 80→250、trim+速度環。
  5. 突然沒力:失步與電池:倒下前 0.1 秒輪速頂到 4000、vmax/amax、電池分壓與校正、半電低於 A4988 8 V、速度環減弱、59 秒 ±2°、brownout。
  6. 成果、坑與系列總整理:調參網頁、最終參數、extern "C" 回滾坑、CDCOnBoot、開機預設不平衡。

補充:原文沒寫到的原理與常見坑

補充一:球與橫樑、平衡車都是「放著就會倒」的系統

球放在稍微傾斜的橫樑上會越滾越快,平衡車放開手會越倒越快——兩者都是不穩定系統,控制器不出手,誤差只會變大。PID 的三項各管一件事:P 依「現在離目標多遠」出力;I 把長時間累積的誤差補掉(例如重心不在正中間);D 看「誤差變化多快」,提前踩煞車、壓住來回擺。球與橫樑的感測器是 HC-SR04,程式裡 distance = duration/29/2 存成整數公分,位置只有 1 公分的解析度,D 項算出來的變化量會一格一格跳,這也是 Kd 不能給太大的原因之一。

原文程式把 PID 輸出(±25°)對應到伺服角度時用了不對稱的基準(輸出為正 92+Output、為負 60+Output),等於手動補機構的死區與零點偏差;換機構時這兩個數字要重新找。

補充二:步進馬達與 A4988 的幾個規格

  • 1.8° 步進角=一圈 200 步;A4988 的 MS1~MS3 全接高是 1/16 微步,一圈 3200 步。平衡車 vmax 4000 steps/s = 1.25 轉/秒。
  • A4988 馬達電源 VMOT 規格 8~35 V(Allegro A4988 datasheet)。2S 18650 滿電 8.4 V、半電約 7.4 V,已經在規格邊緣以下。
  • 電流限制由板上的可變電阻設 Vref:Imax = Vref ÷ (8 × Rs),Rs 是板上的感測電阻(常見 0.05~0.1 Ω,看板子),調之前先查自己那片的電阻值。
  • 步進馬達轉得越快,線圈電流來不及建立(電感+反電動勢),扭力越小;供電電壓越低越明顯。需要的扭力超過當下能給的,就會「失步」:控制器以為輪子在轉,其實沒跟上。
  • AccelStepper 靠程式在迴圈裡不斷呼叫 run() 決定什麼時候出一個脈衝,迴圈被其他事情卡住脈衝就會不均勻;FastAccelStepper 在 ESP32 系列用 MCPWM/PCNT 硬體產生脈衝,控制程式只要更新目標速度。

補充三:互補濾波的直覺

加速度計算出來的傾角(atan2(accY, accZ))不會漂移,但車子一加減速就會被干擾;陀螺儀積分出來的角度短時間很準,但會慢慢漂。互補濾波每一圈做 angle = α·(angle + gyro·dt) + (1−α)·accAngle:短時間信陀螺儀、長時間被加速度計拉回來。時間常數約 τ = α·dt ÷ (1−α);α = 0.98、dt = 5 ms 時 τ ≈ 0.25 秒,也就是大約 0.25 秒以上的偏差會被加速度計慢慢修正。

補充四:為什麼步進平衡車要「PD 給加速度」

直流馬達給電壓,輸出的是力(扭力);步進馬達照指定速度轉,輸出的是速度。把傾斜 θ 的車身推回來,輪子需要的是約 g·tanθ 的水平加速度(倒單擺的近似)。所以角度環輸出加速度、再積分成輪速,才是對步進馬達「說對的語言」;直接把角度換成速度,輪子只會跟著傾角跑,車身持續偏一邊直到輪速飽和——這正是原文 ① 的現象。

自造機器人車系列

  • EP1 ESP32 藍牙遙控車到自動避障:文章/影片
  • EP2 麥克納姆輪:ESP32-CAM 影像車到 Pi 5 自走車:文章/影片
  • EP3 兩輪自平衡(本篇)

2026年11月2日 星期一

ESP32-S3 Stepper Balancing Robot: Dual-Core OTA and Step-by-Step Tuning | ESP32-S3 步進平衡車:雙核 OTA 架構與調適步驟

📺 影片版(約 6 分鐘):想先看實際操作的話可以直接看這支影片,完整步驟、設定與程式碼在下方文字版。
YouTube:https://youtu.be/rxiPyxSWuQc(winson-DIY 頻道)
⚡ Shorts 精華版(45 秒):https://youtube.com/shorts/b57YKF00674

手邊有一台步進馬達平衡車(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 電流
  • 站穩後再加前進/轉向遙控