2026年11月20日 星期五

Controlling a Mitsubishi FX1S PLC from Python and the ATS Console: FX Programming Port Protocol | 用 Python 和 ATS 操作台控制三菱 FX1S PLC:FX 程式口協定實作

桌上有一台舊的三菱 MELSEC FX1S-14MR,加上一條 USB 程式線。平常改這種 PLC 要開 GX Developer/GX Works2,但我只想做三件事:看輸入有沒有進來、從電腦切輸出、讀寫幾個 D 暫存器。所以這次不靠三菱軟體,直接用 Python 講 FX 的「程式口協定」,先寫成命令列工具,再整合進自己的 ATS 網頁操作台,變成一個 PLC 控制頁。

圖說:FX1S-14MR 與 USB-SC09 類程式線(線上的標籤寫 Industrial Adapter and Interface link)

硬體

  • PLC:FX1S-14MR,AC85~264V 供電;輸入 X0~X7 共 8 點,繼電器輸出 Y0~Y5 共 6 點。
  • 傳輸線:一端是 Mini-DIN 8 pin,插 PLC 面板蓋子底下的程式口(RS-422);另一端是 USB。電腦上看到的是 USB-SERIAL CH340,接在 msi 筆電上是 COM40,裝好 CH340 驅動就能用。
  • 確認型號:連上之後讀特殊暫存器 D8001,回 22216。前兩碼 22 代表 FX1S,後面 216 是韌體 V2.16。

FX 程式口協定

這是 GX Developer 跟 FX 系列溝通用的協定,格式很單純:9600 bps、7 個資料位元、偶同位、1 個停止位元(7E1)。內容都是 ASCII,前面放 STX、後面放 ETX,最後加上 2 碼檢查碼。

圖說:讀取與強制兩種封包,檢查碼是 STX 之後到 ETX(含)的位元組加總,取低 8 位元

指令格式用途
ENQ05 → 回 06(ACK)確認 PLC 有沒有在線上
0 讀0+位址 4 碼+長度 2 碼讀 X、Y、M、D…(位元組位址)
1 寫1+位址+長度+資料寫 D 等暫存器,回 ACK
7/87(ON)或 8(OFF)+位元位址 4 碼強制單一個位元 ON/OFF

讀寫用的是位元組位址:X 從 0080、Y 從 00A0、M 從 0100、D 從 1000 開始(D 每個佔 2 bytes,低位元組在前)。M8000 這類特殊繼電器在 01E0,D8000 以上在 0E00。強制指令則用另一套位元位址:Y0=0500、X0=0400、M0=0800。

踩到的坑

  • 第一個 ENQ 沒回應:一開始 ping 失敗,改送原始封包卻正常。原因是 CH340 剛開埠時,第一個位元組會掉。解法是開埠後等 0.2 秒,ping 失敗再重試一次。
  • 強制指令的位址要反過來送:讀寫指令的位址照順序寫,強制指令卻是低位元組在前。Y0 的位元位址 0500 要送成 0005,送錯 PLC 會回 NAK。
  • X、Y 是八進位:FX 的 X、Y 編號沒有 8、9,X7 的下一個是 X10。程式裡算位址時要先當八進位轉換。
  • STOP 時強制會直接出力:我原本以為 STOP 時輸出都會關掉,強制只是改記憶體。實機一試,在 STOP 下強制 Y0、Y2,面板上 OUT 0、2 的燈就亮了。STOP 時沒有程式在跑,強制的值也不會被蓋掉,反而是最單純的測試方式。
  • RUN 時程式有用到的 Y 會被蓋掉:RUN 時 PLC 每次掃描都會依程式重算輸出,從電腦強制的值馬上被覆蓋。比較好的做法是在 PLC 程式寫 LD M0/OUT Y0,電腦去控 M0。這樣 PLC 裡的連鎖和保護也都還在。

命令列工具 fxplc.py

先用 pyserial 寫一支命令列工具,Windows、Linux 都能跑:

py -3 fxplc.py -p COM40 ping          # OK,PLC 有回應
py -3 fxplc.py -p COM40 status        # STOP | X0=□ … | Y0=□ …
py -3 fxplc.py -p COM40 on Y0         # 強制 Y0 ON
py -3 fxplc.py -p COM40 read D0 4     # [0, 0, 0, 0]
py -3 fxplc.py -p COM40 write D0 123
py -3 fxplc.py -p COM40 read D8001    # [22216] → FX1S V2.16

整合進 ATS:PLC 控制頁

ATS 主畫面上方多了一顆「PLC」按鈕,按下去會開一個獨立視窗。選好 COM 埠、按連線,標題會顯示型號和 RUN/STOP,然後每 0.5 秒更新一次:

  • 輸入 X0~X7:指示燈,唯讀。
  • 輸出 Y0~Y5、輔助繼電器 M0~M15:點一下切換,送的是強制 ON/OFF。
  • 資料暫存器 D:指定起點和數量讀成一張表,每列可以填新值寫入,也可以勾自動更新。
  • 任意元件:輸入 T0、M100、D8001 等,可以讀取或強制。
  • 依 RUN/STOP 顯示不同提示:STOP 時強制的 Y 會直接出力;RUN 時程式驅動的 Y 會被蓋掉,要改控 M。
  • 連著的時候 COM 埠被 ATS 占用;要用 GX Works 改程式前,先按「中斷」把埠放掉。

圖說:ATS 的 PLC 控制頁(這張是模擬模式的畫面,實機會顯示 COM40)

ATS 也加了一台模擬的 FX1S(--sim 時選「sim」),沒有接 PLC 也能測整個頁面。

實機:STOP 下強制 Y0、Y2

ATS 裝在 msi 筆電上,PLC 開關留在 STOP。在 PLC 頁點 Y0 和 Y2,畫面上兩個輸出亮起,PLC 面板上 OUT 的 0、2 兩顆紅燈也同時亮了(RUN 燈沒亮,確實是 STOP)。

圖說:msi 上的實機畫面:COM40、STOP,Y0 和 Y2 已強制 ON

圖說:同一時間的 PLC 面板:POWER 亮、RUN 不亮,OUT 0、2 亮

Y0 跟誰導通?

FX1S-14MR 是繼電器輸出,每個 Y 都是一顆繼電器的乾接點:Y0 ON 時,Y0 和它的共點 COM0 兩個端子接通。6 個輸出分成三組,實際接線請以端子蓋上的標示為準:

共點輸出
COM0Y0
COM1Y1
COM2Y2、Y3、Y4、Y5

接負載的方式就把它當成一個開關:電源一端接 COM0,經過負載後接到 Y0,交流、直流都可以,每點最多約 2 A。想先確認的話,把 PLC 撥到 RUN,三用電錶切到通路檔夾 COM0 和 Y0,在 ATS 上點 Y0,ON 時應該會嗶。

應用:用 Y0、Y1 控制兩顆 24V 電磁閥

圖說:FX1S-14MR 用 Y0、Y1 控制兩顆 24V 電磁閥的接法

這是很常見的氣動應用:外接一顆 24V 開關電源,+24V 接到兩顆電磁閥的一端,電磁閥另一端分別接 Y0、Y1,PLC 的 COM0 接電源 0V。Y0 ON 時繼電器把 Y0 和 COM0 接通,第一顆電磁閥通電動作;Y1 同理。換句話說,PLC 的繼電器只是串在電磁閥的 0V 那一側當開關,電磁閥的電源由開關電源提供,不是由 PLC 供電。

套用到 FX1S-14MR 時要注意兩件事:

  • Y1 的共點是 COM1,不是 COM0。很多網路上的接線圖是 Y0、Y1 共用 COM0,但 FX1S-14MR 的 Y0、Y1 各有自己的共點,所以圖中把 COM0 和 COM1 跳線接在一起,再接到 0V。Y2~Y5 則共用 COM2。
  • 電磁閥是電感性負載:直流線圈斷電時會產生反向突波,長期下來會讓繼電器接點燒蝕。所以圖中在每顆線圈兩端反向並聯一顆二極體(1N4007,陰極接 +24V 那一端);很多電磁閥接頭本身已經內建 LED 和保護元件,可以先確認一下。

目前進度與下一步

  • 實機已經確認:連線、型號(D8001)、讀 X/Y/M、讀 D、STOP 下強制 Y(面板 OUT 燈跟著亮)都正常。
  • 寫 D 只在模擬環境測過;繼電器接點還沒用三用電錶或實際負載量過。
  • 下一步:在 PLC 寫一段 M0~M5 → Y0~Y5 的梯形圖,讓電腦改控 M;也打算把 PLC 步驟加進 ATS 的測試流程,量測時順便切治具上的繼電器。

DIY Robot Cars EP2: Mecanum Wheels, from an ESP32-CAM Car to a Pi 5 Self-Driving Car | 自造機器人車 EP2:麥克納姆輪,從 ESP32-CAM 影像車到 Pi 5 自走車

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

English summary: Episode 2 of the "DIY Robot Cars" series merges three posts into one story about mecanum wheels: why 45-degree rollers let a car strafe and move diagonally (with the kinematics formula), a 2024 ESP32-CAM car that streams video and sends 4-bit commands to an Arduino Mega2560 driving two L298Ns and a servo arm, and a 2026 Raspberry Pi 5 rebuild with web joysticks, YOLO, gyro-assisted straight driving, distance and route maps, camera ground-plane ranging, scan-driven obstacle avoidance, and record-and-replay routes. The big lesson: a runaway caused by lgpio leaving the enable pins high, and how pinctrl in systemd, a PWM watchdog and an operation log fixed it.

這是「自造機器人車」系列的第二集,主角換成麥克納姆輪。把 2024 年的 ESP32-CAM 影像車和 2026 年 Pi 5 自走車的兩篇文章整理成一條故事線:輪子為什麼能橫著走 → ESP32-CAM 邊看邊開 → 換 Pi 5 網頁遙控 → 陀螺儀走直線與地圖 → 自動避障與一次暴衝 → 錄路線照軌跡走。影片約 20 分鐘,有章節可以跳著看;程式全文在各篇原文。

本集章節

  1. 麥克納姆輪為什麼能橫著走:45° 滾子、X 排列、四輪速度公式、鏡像安裝的 CW/CCW。
  2. 2024:ESP32-CAM 邊看邊開:ESP32-CAM 影像+網頁、4 支 GPIO 當 4 位元指令碼、Mega2560 推兩片 L298N 與 4 顆手臂伺服;從程式反推每顆輪子的位置。→ ESP32CAM + 麥克納姆倫 + Car(2024/06)
  3. 2026:換 Pi 5 網頁遙控:零件與接線、12.6 V 與 PWM 上限、雙搖桿網頁與 0.5 秒斷線停車、輪子 invert、實體腳號 13 ≠ GPIO13。→ Pi 5 麥克納姆輪自走車(一)(2026/11)
  4. 走直線、畫地圖、鏡頭測距:3 秒偏 10° → PD+學習量、兩個網頁互蓋、積分飽和、距離圖與路線圖、地面法測距與鏡頭校正。→ 同上+Pi 5 麥克納姆輪自走車(二)(2026/11)
  5. 避障與暴衝:避障一路改版、超聲波打到自己的鏡頭、掃描導向策略;lgpio tx_pwm(0) 不拉低+SIGKILL、tx_servo(0) 卡住別支腳 PWM(79/80 → 0/90);systemd pinctrl、PWM 看門狗、操作紀錄。→ 同上兩篇
  6. 照軌跡走與總整理:每 3 cm/5° 記點、麥克納姆斜走追點、搬到目前位置、對齊起點;實車 5.3 m、142 點、24 秒。

單篇影片

  • ESP32CAM Mecanum wheel CAR:https://youtu.be/qQoEmHxq1BA
  • Pi 5 麥克納姆輪自走車:https://youtu.be/Imtc1hFkkwQ

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

補充一:麥克納姆輪的運動學

每顆麥克納姆輪的外圈是一排斜 45° 的小滾子。輪子轉動時,地面推回來的力不是正前方,而是沿著滾子垂直的方向斜 45°;四顆輪子的斜向力加起來,前後分量和左右分量各自可以抵消或相加,所以車子能直走、平移、斜走、原地轉。由上往下看,四顆輪子的滾子要排成「X」(左前、右後一組,右前、左後一組),裝反的話平移方向會反。

vx:往前、vy:往左平移、w:逆時針旋轉(都在 -1~1)
FL = vx - vy - w      FR = vx + vy + w
RL = vx + vy - w      RR = vx - vy + w
任一輪的絕對值超過 1 時,四輪一起除以最大值(保持方向比例)

這就是 Pi 5 車 car/hw.py 的 mecanum()。例:vx=0、vy=1(往左平移)→ FL -1、FR +1、RL +1、RR -1,同一條對角線的兩顆輪子轉向一樣。只轉對角線上的兩顆(例如 FR 與 RL 往前),車子會往左前 45° 斜走。

補充二:從 2024 版程式反推每顆馬達的位置

Mega2560 程式裡的 Forward(CW、CCW、CCW、CW)和 S_Left(四顆全 CW)比較,變號的是 motors1 的馬達 2 和 motors2 的馬達 1,它們是同一條對角線;Xtrun(原地轉)變號的是 motors2 的兩顆,所以 motors2 管同一側。對照上面的公式可以推出:一片 L298N 管右側(右前、右後)、另一片管左側(左前、左後);Left() 只轉右前+左後,就是往左前斜走。左右兩側馬達是鏡像安裝,同樣讓車往前,一邊要正轉、另一邊要反轉,所以程式裡同一個動作會同時出現 CW 和 CCW。(依原文程式推論,非原作者說明)

補充三:ESP32-CAM 為什麼要再接一片 Mega2560

AI-Thinker ESP32-CAM 的鏡頭佔掉大部分 GPIO,排針上只拉出 GPIO 0、1、2、3、4、12、13、14、15、16,其中 16 在有 PSRAM 的板子上接著 PSRAM、 1/3 是序列埠、0 是開機模式腳、2/12/15 是開機 strapping 腳、2/4/12/13/14/15 同時接到 microSD 卡、GPIO4 還接著板上的閃光燈 LED。要同時推 4 顆馬達(8 支方向腳+PWM)和 4 顆手臂伺服,腳位根本不夠,所以原文用 ESP32-CAM 只負責影像和網頁,把 4 支 GPIO 當成 4 位元的指令碼送給 Mega2560,16 種組合剛好放得下 7 種移動、停止和 8 個手臂動作。

原文的 WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0) 是關掉欠壓偵測:馬達啟動拉低電壓時 ESP32-CAM 不會一直重開,代價是電壓真的太低時晶片可能跑出錯誤行為而不是乾淨地重開。比較好的做法仍是把馬達和 ESP32-CAM 的電源分開、加大電容。

補充四:TT 馬達的電壓與 PWM 上限

TT 黃色減速馬達常見額定 3~6 V。三顆 18650 串聯滿電 12.6 V,L298N(雙極電晶體 H 橋)大約吃掉 2 V,馬達端仍有 9~10 V,所以要用 PWM 上限把平均電壓壓低;原文從 60% 再降到 50%。PWM 只是平均值,峰值仍是全電壓,長時間高 PWM 會讓馬達發熱與壽命縮短。

補充五:接馬達的程式要先想「死掉時腳位停在哪」

  • 停車一定直接把 EN 腳寫成低電位,不要只「停掉 PWM」(lgpio 的 tx_pwm(…, 0) 會讓腳停在最後的電位)。
  • 服務啟動前、結束後用 pinctrl 把馬達腳全部拉低(ExecStartPre/ExecStopPost),程式被砍也一樣。
  • 網頁遙控要有「沒收到指令就停車」的逾時(原文 0.5 秒)。
  • 硬體上可在 EN 腳加下拉電阻,讓 Pi 開機、腳位還是輸入狀態時馬達也不會動。
  • Pi 5 的 GPIO12/13/18/19 是硬體 PWM 腳,換成硬體 PWM 可避開軟體 PWM 卡住的問題(原文列為待辦)。

補充六:只靠輪速推算位置會飄

沒有輪子編碼器時,位置是「送給馬達的指令 × 實測速度 × 時間」積分出來的(dead reckoning),方向用陀螺儀。輪子打滑、電池電壓下降、麥克納姆輪平移效率(原文假設 0.8)都會讓誤差一路累積,所以路線圖上「照著走」的綠線疊在黃線上,只代表控制照著計畫走,不代表地上的真實位置;要量真實偏差得用地上標記或俯拍。

自造機器人車系列

2026年11月19日 星期四

DIY Robot Cars EP1: ESP32 Bluetooth RC Car to Obstacle Avoidance | 自造機器人車 EP1:ESP32 藍牙遙控車到自動避障

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

English summary: Episode 1 of the "DIY Robot Cars" series turns four posts from summer 2023 into one story: spinning DC motors with an L298N dual H-bridge, controlling them with single letters over ESP32 Classic Bluetooth SPP, building a four-wheel skid-steer car, driving it with the Bluetooth RC Controller app, and finally adding an HC-SR04 on a servo with two FreeRTOS tasks for obstacle avoidance. Along the way: L298N voltage drop and jumpers, the Arduino-ESP32 3.x LEDC API change, a strstr bug where "VER" also triggers Right(), and why readString() waits one second.

這是「自造機器人車」系列的第一集,把部落格 2023 年夏天連續四篇 ESP32 小車文章整理成一條故事線:L298N 讓馬達轉起來 → 藍牙 SPP 用字母控制 → 組成四輪車 → 手機 App 遙控 → 超音波+伺服馬達自動避障。影片約 16 分鐘,有章節可以跳著看;程式全文在各篇原文。

本集章節

  1. 讓馬達轉起來:L298N 雙 H 橋、接線(ENA 4、IN1~IN4 16~19、ENB 5)、共地;壓降 2~3 V、ENA/5V 跳帽;Robojax 函式庫與 ESP32 PWM 通道;Arduino-ESP32 3.x 的 LEDC 改版。→ ESP32 Bluetooth SPP control -- DC motors with L298N module(2023/07)
  2. 藍牙 SPP:手機送一個字母:BluetoothSerial、F/B/L/R/S;strstr 讓 VER 也觸發右轉;只有原版 ESP32 有傳統藍牙、iPhone 不開放 SPP。→ 同上
  3. 組一台四輪車:車體套件、18650 電池、四顆馬達同側並聯、L298N +5V 供電給 ESP32、滑移轉向三種轉法。→ ESP32 Bluetooth Remote Control Car -DIY(2023/08)
  4. App 遙控:Bluetooth RC Controller 指令表(F B L R G I H J S、W/U/V/X、0~9 q);為什麼程式比對的是 FS、FGFS(readString 預設等 1 秒)與逐字元改法。→ ESP32 Bluetooth RC Car APP - Bluetooth RC Controller(2023/08)
  5. 自動避障:HC-SR04(duration/29/2、Echo 5 V 分壓)、ESP32Servo 50 Hz 500~2400 µs、35 cm 避障邏輯、兩個 FreeRTOS 任務與 App X/x 切換;pulseIn 逾時、伺服電流、volatile、斜面軟布四個坑。→ ESP32 Obstacle Avoiding Robot Car(2023/08)
  6. 總整理與下一集:四篇=一台車長大;EP2 麥克納姆輪(ESP32-CAM 影像車 → Pi 5 自走車)、EP3 步進平衡車。

單篇影片

  • DC motors with L298N module - ESP32 Bluetooth SPP control:https://youtu.be/F3gByuOwLIU
  • ESP32 Bluetooth Remote Control Car DIY:https://youtu.be/mcMax-saFSY
  • ESP32 Bluetooth RC Car APP - Bluetooth RC Controller:https://youtu.be/RWFrd_aHiwI
  • ESP32 Obstacle Avoiding Robot Car:https://youtu.be/ZjxEfAh5XB8

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

補充一、H 橋怎麼讓馬達正反轉

四個開關排成 H 字形,馬達在中間。L298 每個橋由 IN1/IN2 決定方向、EN 決定開關:

ENIN1IN2馬達
HHL一個方向(正轉)
HLH另一個方向(反轉)
HIN1=IN2快速煞車
L任意自由滑行

EN 接 PWM,開的時間比例=平均電壓=速度。所以每顆馬達要三條線:兩條管方向、一條管速度。

補充二、L298N 模組的三個坑

  • 壓降約 2~3 V:L298 是雙極電晶體輸出(datasheet 的飽和壓降上下橋合計在 1 A 時約 1.8~3.2 V),7.4 V 電池到馬達端可能只剩 5 V 左右。
  • ENA/ENB 跳帽:插著=EN 接 5 V,永遠全速;要用 PWM 調速必須拔掉,改接 ESP32。
  • 5V 穩壓跳帽:供電 12 V 以下才插著,此時 +5V 端子是輸出,可以供電給 ESP32(原文接線就是這樣接);超過 12 V 要拔掉、+5V 改成輸入。
  • 另外:ESP32 與 L298N 一定要共地;2 A 是單路上限,馬達堵轉時電流最大。

補充三、ESP32 的 PWM 與 Arduino-ESP32 3.x

ESP32 的 PWM 來自 LEDC 周邊(通道、頻率、解析度)。Arduino-ESP32 核心 3.0 起 LEDC API 改版:ledcSetup()+ledcAttachPin() 合併為 ledcAttach(pin, freq, resolution),ledcWrite() 改成用腳位而不是通道。2023 年依 2.0.x 寫的範例與函式庫(例如需要傳 PWM 通道的 L298N 函式庫)用新核心可能編譯失敗:在開發板管理員改裝 2.0.x,或改寫成 3.x 寫法。

// 3.x 寫法示意(未編譯)
ledcAttach(ENA, 1000, 8);    // 1 kHz、8 bit
ledcWrite(ENA, 180);         // 0~255
digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);

補充四、為什麼用傳統藍牙 SPP、它的限制

  • SPP(Serial Port Profile)把藍牙當序列埠,Android 上的終端機、遙控 App 最多,ESP32 用 BluetoothSerial 幾行就好。
  • 只有原版 ESP32(ESP32-WROOM/WROVER)有 BR/EDR 傳統藍牙;ESP32-S3、C3、C6 只有 BLE,BluetoothSerial 不能用。
  • iOS 不開放一般 App 使用 SPP(需 MFi),要支援 iPhone 就改用 BLE(例如 Nordic UART Service)或 WiFi。

補充五、strstr 的小坑:打 VER 車子會右轉

原文 USB 序列埠這一路用 strstr(data, "R") 判斷「字串裡有沒有 R」;查版本的 VER 剛好含 R,所以印出版本號的同時也會呼叫 Right()(依程式邏輯推論,原文沒有寫到這個現象)。改法:

if (strcmp(data, "VER") == 0) { Serial.println(F("ESP32_20230710")); return; }
switch (data[0]) {
  case 'F': Forward(); break;
  case 'B': Reverse(); break;
  case 'L': Left();    break;
  case 'R': Right();   break;
  case 'S': Stop();    break;
}

補充六、四輪車的電源與轉彎

  • 電源:電池 → L298N +12V(馬達);L298N +5V → ESP32 5V 腳(主控)。兩顆 18650 串聯滿電 8.4 V、標稱 7.4 V,扣掉 L298N 壓降,馬達約 5~6 V。
  • 四顆馬達同側並聯:左兩顆接 OUT1/OUT2、右兩顆接 OUT3/OUT4,程式仍只有 motor1/motor2。
  • 滑移轉向(skid steering):輪子不能轉向,只能靠兩側速度差。單邊停(原文 L/R)以停住那側為圓心畫弧;兩邊反向=原地旋轉,最快但輪胎側滑、耗電;兩邊同向不同速=差速轉彎,最順(原文 App 版 FGFS 等斜角就是 60%/100%)。
  • 原文三篇的 Left()/Right() 改過一次:2023/07、2023/08 第一版是單邊往後轉,App 版改成另一側單邊往前轉。

補充七、Bluetooth RC Controller 指令表與 readString 等一秒

按鍵送出
前/後/左/右F/B/L/R,放開送 S
左前/右前/左後/右後G/I/H/J
前燈/後燈W 開 w 關/U 開 u 關
喇叭/額外鍵V/v、X/x(原文用 X/x 切換自動避障)
速度滑桿0~9、q(100%)

原文 App 版比對的是 "FS"、"FGFS" 這種組合,原因是 SerialBT.readString() 會一直讀到「超過逾時都沒有新字元」才回傳,預設逾時 1000 ms:一秒內的按下、放開會黏成一串,而且每個指令都要等一秒才執行。改法是逐字元處理:

void loop() {
  while (SerialBT.available()) {
    char c = SerialBT.read();
    switch (c) {
      case 'F': Forward(); break;
      case 'B': Reverse(); break;
      case 'G': ForwardLeft(); break;   // 60% / 100%
      case 'I': ForwardRight(); break;
      case 'S': Stop(); break;
      case 'X': autoMode = false; Stop(); break;
      case 'x': autoMode = true; break;
    }
  }
}

(或至少 SerialBT.setTimeout(20);,讓 readString 只等 20 ms。)

補充八、避障車:超音波、伺服與常見坑

  • HC-SR04:Trig 送 10 µs 脈衝,Echo 高電位時間=聲波去回時間。聲速約 340 m/s,1 cm 約 29 µs,去回除以 2 → distance = duration / 29 / 2(例:2030 µs ≈ 35 cm)。
  • Echo 是 5 V 訊號,ESP32 GPIO 只能 3.3 V:建議 1 kΩ/2 kΩ 分壓。
  • 伺服:ESP32 要用 ESP32Servo 函式庫;50 Hz(20 ms),原文 attach(13, 500, 2400):0° 500 µs、180° 2400 µs,90° 約 1450 µs;程式用 90°(前)、20°(右)、160°(左)。
  • pulseIn() 預設逾時 1 秒:前方沒回波時任務會卡一秒,建議 pulseIn(ECHO, HIGH, 25000)(約 4 m);原文定義的 MAX_DISTANCE 200 可以用在這裡。
  • 伺服啟動電流大,電壓掉下來 ESP32 可能 brownout 重開:伺服另外供電、加 470 µF 以上電容、共地。
  • distance、HCSR04Flag 由兩個 FreeRTOS 任務共用:宣告 volatile 或改用 queue。
  • 斜面會把超音波彈走、軟布會吸收,量不到 → 可加紅外線或 ToF(如 VL53L0X)補強。

自造機器人車系列

Driving an ODrive Motor from ECU RPM: OBD-II PID 0C to Set_Input_Vel on One CAN Bus | ECU 轉速連動 ODrive:行車電腦 OBD 轉速換算後直接控制無刷馬達

上一篇〈ATS 操作台用 PCAN 控制 ODrive v3.6〉已經能在網頁上手動控制兩顆無刷馬達。這次要讓馬達「自己動」:從行車電腦模擬器用 OBD-II 讀引擎轉速,換算成馬達速度、加上限制,一直送給 ODrive 的 M1(A2212 1400KV,沒有編碼器的 sensorless 馬達)。引擎轉速升高,馬達就跟著轉快;熄火、斷線,馬達就停。

為了少一支 PCAN,ODrive 和模擬器接在同一條 CAN bus 上,全部交給 ATS 處理。功能一天就寫好、在模擬環境測過,真正花時間的是實機接上去之後的幾個坑,這篇都記下來。

圖說:整體架構:PCAN、行車電腦模擬器、ODrive 三個節點在同一條 500k 的 CAN bus 上

做法

1. 讀轉速:OBD-II PID 0C

ATS 原本就有 OBD 掃描頁。引擎轉速是 Mode 01 的 PID 0C:送功能定址 7DF 02 01 0C,ECU 從 7E8 回 04 41 0C A B,轉速=(A×256+B)/4 rpm。OBD 頁平常會輪流問儀表那 8 個 PID 再加一個其他的,轉速大約每 0.5 秒才更新一次,太慢。所以連動多了一個選項「OBD 只問轉速」,打勾時只輪詢 0C,實測每秒約 14 次,資料延遲約 50 ms。

2. 換算與限制

  • 線性換算:轉速下限 → 速度下限、轉速上限 → 速度上限之間拉一條直線,超出範圍就停在兩端。實測用 600~8000 rpm 對 5~40 turns/s。
  • 熄火就停:低於 500 rpm 送 0。速度下限設 5 turns/s 是因為 sensorless 太慢轉不穩。
  • 速度上限:不管換算出多少,最多 40 turns/s(ODrive 本身的 vel_limit 是 50)。
  • 加減速斜率:20 turns/s²,轉速跳動時馬達不會跟著猛拉。
  • ECU 逾時:500 ms 沒有新的轉速就把目標歸 0。
  • 反轉:勾了就送負的速度。

圖說:換算規則(藍線)和實機量到的點(橘點):轉速約 5150 rpm 時 M1 約 26.6 turns/s,落在線上

3. 送速度給 ODrive

ODrive 韌體 0.5.6 走 CANSimple,CAN ID=node×32+指令。M1 是 node 3,Set_Input_Vel 是指令 0x0D,所以 ID 是 0x06D,資料是兩個 little-endian float(速度 turns/s、力矩前饋)。連動開始時先送 Set_Controller_Mode 切成「速度控制+PASSTHROUGH」,斜率由 ATS 自己算,之後每 50 ms 送一次速度。

同一條 bus 上 ID 會不會打架?ODrive 兩軸(node 2、3)用的是 0x40~0x7F,OBD 用 0x7DF/0x7E8,不會衝突。

4. 網頁畫面

在 ATS 的 ODrive 頁下面加一張「ECU 轉速連動」卡片:轉速來源可以選同一條 bus 或另一支 PCAN,所有參數都能在畫面上改、按「套用設定」立即生效。下面四格即時顯示 ECU 轉速、目標速度、送出速度(含斜率)和轉速資料延遲。

圖說:實機畫面:ECU 5153 rpm → 目標 26.53 turns/s,轉速資料延遲 47 ms

圖說:同一時間的 M1:閉迴路、26.63 turns/s,沒有錯誤

實測結果

連動中從 ATS 的 WebSocket 錄 60 秒(約 300 點):模擬器的轉速在 5117~5245 rpm 之間小幅跳動,目標速度跟著在 26.36~26.97 turns/s 之間變,M1 實際速度和目標的差距平均 0.11 turns/s、最大 0.47。OBD 這段時間 4510 次查詢只逾時 4 次。

圖說:60 秒實錄:藍線是由轉速換算的目標,橘線是 M1 的 sensorless 估計速度(縱軸只放大 26~27 turns/s 這一段)

實機踩到的坑

坑 1:ODrive 還是 250k,bus 上全是錯誤框

模擬器是 500k,ODrive 出廠設定是 250k。PCAN 用 500k 一連上,2 秒就是 5 萬多個錯誤框,ATS 自動斷線。CAN 讀不到 ODrive 的設定,改走 ODrive 的 USB 序列埠(ASCII 協定):w can.config.baud_rate 500000 → ss 存檔 → sr 重開,再讀回確認是 500000。之後只接 ODrive 的 5 秒內收了 1106 框、零錯誤。

坑 2:零星錯誤框被當成錯誤風暴

模擬器接上 bus 後會有零星的接收錯誤框(平均幾秒一個),PCAN 的錯誤計數一直停在 1,OBD 也照樣有回應。但 ATS 原本的規則是「錯誤持續 2 秒就斷線、剛送過框又出錯 0.5 秒就重開通道」,把它當成風暴,結果一直斷線、或重開通道打斷 OBD。改成:一波至少 50 個送出時的錯誤才重開通道(接收錯誤是別人的框壞掉,重開自己沒用),至少 500 個且持續 2 秒才斷線;零星的只在紀錄裡報告。

坑 3:模擬器卡住後,OBD 不會自己再問

模擬器一度沒回應,OBD 掃描第一次問不到支援表就停在「沒有回應」,模擬器好了也不會再試,要手動重開 OBD。改成沒回應時每 3 秒自動重試(紀錄只寫第一次),模擬器一回應連動就自己接上。

坑 4:OBD 重開後「只問轉速」失效,馬達一頓一頓

「只問轉速」原本是開始連動時設在當下那個 OBD 掃描器上。OBD 被停掉重開後換了一個新的,又回到輪詢全部 PID,轉速約 0.5 秒才更新一次,剛好碰到 500 ms 的逾時,速度在「歸 0」和「恢復」之間一直跳。改成每次讀轉速時都把設定套到目前的掃描器。

坑 5:模擬器 bus-off

有兩次模擬器只回了 1~2 秒,接著來一波錯誤,然後完全不說話,bus 上也沒有錯誤框了。ODrive 照樣在回 ACK,表示 bus 是通的,是模擬器自己停了。研判是錯誤太多進入 bus-off,模擬器斷電重開、重新檢查接線後才恢復。其中一次是在模擬器的 GND 拉到 PCAN 共地之後發生的,零星錯誤框的根本原因(接線、地、或模擬器本身的位元時序)還沒查清楚,先記下來。

安全:打開 ODrive 看門狗

連動時馬達是一直轉的。按「停止連動」或停止 ODrive 控制,ATS 都會先送速度 0;但如果 ATS 當掉、被直接關掉,或 PCAN 被拔掉,ODrive 會一直維持最後的速度。所以把 ODrive 兩軸的看門狗打開:config.enable_watchdog = 1、config.watchdog_timeout = 1.0(秒),存檔。ATS 開著 ODrive 控制時每 0.1 秒會用 RTR 問一次狀態,連動時每 50 ms 送一次速度,這些 CAN 指令都會餵看門狗;1 秒沒收到,該軸就自己回 IDLE。

副作用是 ATS 沒在跑 ODrive 控制時,兩軸會帶著 WATCHDOG_TIMER_EXPIRED 錯誤,所以使用順序變成:ODrive 頁按「開始」→ 兩軸「清錯誤」→ M1「閉迴路」→「開始連動」。ATS 的「設定(USB)」也加了看門狗兩欄,可以直接看、改、存檔。

小結

  • OBD 讀轉速 → 線性換算、限制、斜率 → Set_Input_Vel,ECU 和 ODrive 共用一條 500k CAN bus 可行,ID 不衝突。
  • 只問 PID 0C 時轉速每秒約 14 次、延遲約 50 ms,M1 跟目標的差距平均 0.11 turns/s。
  • 沒有編碼器的 M1 用 CAN 切進 sensorless 閉迴路,這次在實機上確認可行。
  • 實機上最花時間的是實體層:鮑率、終端電阻、錯誤框的判讀,以及模擬器 bus-off。
  • 還沒做:零星錯誤框的根本原因、用示波器看 CAN-H/L 波形。