Pi 5 Mecanum Car Part 2: Record & Replay Routes, an lgpio PWM Bug, and Scan-Driven Obstacle Avoidance | Pi 5 麥克納姆輪自走車（二）：錄路線照軌跡走、lgpio PWM 卡住的坑、掃描導向避障

這篇接著 Pi 5 麥克納姆輪自走車（一）。上一篇把網頁遙控、陀螺儀走直線、距離圖、路線圖和自動避障做起來了；這次加了「手動開一段路線，再叫車子自己照著軌跡走一次」，順便把實車調校時遇到的幾個大坑整理起來：lgpio 軟體 PWM 會卡住（車子暴衝、或完全不動）、超聲波打到自己的鏡頭、自動避障的策略重寫。每一段都附上程式說明。

說明：第一節錄路線、照軌跡走的截圖和偏差數字是在開發機上用模擬模式（假房間）跑的；實車在測照軌跡走的時候剛好遇到下面的 PWM 卡住問題。11/13 補：實車已經完整跑過，畫面、路線圖和影片放在第七節；鏡頭重新調整後的校正放在第八節。

一、錄路線、照軌跡走

用法很簡單，都在網頁的「路線圖」頁籤：

1. 按「● 錄路線」，用搖桿開一段路（可以直走、轉彎、平移），開完按「■ 停止錄」。錄好的路線畫成黃色虛線，存在車上（data/route.json），重開程式也還在。
2. 把車子放回起點，按「▶ 照軌跡走」。勾「從目前位置開始」時，整條路線會搬到目前的位置和車頭方向，所以車子不用擺得很準。
3. 走的時候推搖桿、按停車、切自動避障都會馬上中斷；正前方小於 25 cm 會停下來等，超過 15 秒就放棄。

錄：每 3 cm 或轉 5° 記一個點

位置用的是路線圖本來就有的推算位置（輪速 × 時間做麥克納姆反運算，車頭方向用 MPU6050 陀螺儀）。錄的時候只要在推算迴圈裡多記點：

# car/route.py
def rec(self, x, y, th):
    lx, ly, lth = self.pts[-1]
    if math.hypot(x - lx, y - ly) >= self.cfg.get("rec_cm", 3) or abs(wrap(th - lth)) >= self.cfg.get("rec_deg", 5):
        self.pts.append((round(x, 1), round(y, 1), round(th, 1)))

停止錄的時候，最後停下來的位置也要補一個點，不然重走會少最後幾公分。

走：麥克納姆可以斜著走，不用先轉頭

一般兩輪車要「先轉向目標再前進」，麥克納姆輪可以往任何方向平移，所以做法是：把下一個目標點換到車身座標（前 fx、左 ly），直接往那個方向走，同時用比例控制修車頭角度。車頭差太多（錄的時候原地轉的那一段）就先原地轉：

# car/route.py：回 (新的 i, vx, vy, w, 完成？)
def step(self, pts, i, x, y, th):
    look, tol, tol_deg = c.get("look_cm", 10), c.get("tol_cm", 5), c.get("tol_deg", 6)
    dist = lambda p: math.hypot(p[0] - x, p[1] - y)
    # 往前找目標點：近的、而且車頭角度差不多的就跳過（原地轉的那幾個點角度差大，會停下來轉）
    while i < len(pts) - 1 and dist(pts[i]) < look and abs(wrap(pts[i][2] - th)) < 10:
        i += 1
    p = pts[i]
    d, err = dist(p), wrap(p[2] - th)
    if i == len(pts) - 1 and d < tol * 0.6 and abs(err) < tol_deg:   # 終點要求準一點
        return i, 0.0, 0.0, 0.0, True
    if (d < tol and i < len(pts) - 1) or abs(err) > 20:               # 車頭差太多：原地轉
        ...
        return i, 0.0, 0.0, math.copysign(c.get("turn_speed", 0.55), err), False
    a = math.radians(th)
    fx = (p[0] - x) * math.cos(a) + (p[1] - y) * math.sin(a)      # 目標在車身座標：前 fx、左 ly
    ly = -(p[0] - x) * math.sin(a) + (p[1] - y) * math.cos(a)
    w = max(-0.3, min(0.3, c.get("kp_deg", 0.03) * err))
    return i, sp * fx / d, sp * ly / d, w, False

「從目前位置開始」就是把整條路線做一次旋轉＋平移，讓錄的起點對到現在的位置和車頭：

x0, y0, th0 = self.pts[0]
a = math.radians(th - th0)
return [(x + (px - x0) * c - (py - y0) * s, y + (px - x0) * s + (py - y0) * c, th + wrap(pth - th0))
        for px, py, pth in self.pts]

伺服器端每 50 ms 呼叫一次 step()；只有「往前走」的時候才檢查超聲波（它只看得到正前方），前方太近就停車等：

# car/server.py play_loop()
i, vx, vy, w, done = self.route.step(p["pts"], p["i"], o.x, o.y, o.th)
if vx > 0.15 and self.front is not None and self.front < s["stop_cm"]:
    self.car.motors.stop()
    ...   # 等它讓開，超過 wait_s 秒就放棄
self.car.motors.set({n: v * self.limit for n, v in mecanum(vx, vy, w).items()})

模擬測試：錄一段 3 m 路線（直走、左轉 90°、直走、右平移），兩種開始方式都能走完，離原路線最多 4.5 cm、終點差約 2.5 cm。實車因為位置靠輪速推算，一定會比模擬飄，路線越長差越多。

放回起點：疊上起點畫面＋陀螺儀＋超聲波

車子被手拿起來搬的時候輪子沒轉，推算位置不會變，所以沒辦法用路線圖的位置來擺車。改成開始錄的時候記下三樣東西：攝影機畫面、陀螺儀原始角度（手搬也會跟著轉）、正前方超聲波距離。按「對齊起點」時，起點畫面半透明疊在即時影像上，下面即時提示車頭還要往哪邊轉幾度、前方距離要往前或往後挪幾公分：

"align": {"dyaw": round(((self.route.start["yaw"] - self.imu.yaw) + 180) % 360 - 180, 1),
          "front0": self.route.start.get("front"),
          "front": None if self.front0 is None else round(self.front0)}

二、暴衝和不動：lgpio 停伺服會卡住別支腳的 PWM

這是今天最大的坑。實車有時突然全速衝出去，有時推搖桿完全不動，一開始以為是電池沒電，換了電池量到 12.16 V 還是不動。後來直接看 L298N 的 EN 腳（調速的 PWM）：程式以為在送 50% PWM，實際電位卻一直是低的；方向腳（IN）倒是正常。

用一支空腳重現，終於找到原因：lgpio 0.2.2 用 tx_servo(pin, 0) 停掉伺服脈衝時，會讓「別支腳」的軟體 PWM 卡住。tx_busy() 還說它在跑，但電位停住不動：停在高電位就是暴衝，停在低電位就是不動。超聲波裝在伺服上，自動避障一直左右轉頭，程式每次轉完 0.4 秒就停掉伺服脈衝（不然伺服會抖），所以很容易踩到。

測試（另一個執行緒一直開／關伺服，主執行緒改馬達 PWM）	PWM 卡住次數
用 tx_servo(pin, 0) 停伺服	79／80
同上，所有 lgpio 呼叫加全域 lock	63／64（不是執行緒問題）
改用 tx_pwm(pin, 50, 0)＋gpio_write(pin, 0) 停伺服	0／90

# car/hw.py Servo
def _off(self):
    # 不能用 tx_servo(pin, 0) 停：lgpio 0.2.2 的 bug，停掉伺服會讓「別支腳」的軟體 PWM 卡住
    lg, h = self.g
    lg.tx_pwm(h, self.cfg["pin"], 50, 0)
    lg.gpio_write(h, self.cfg["pin"], 0)

另外加了一個 PWM 看門狗：每 0.5 秒直接讀 4 支 EN 腳的實際電位。該 PWM 的腳在 12 ms 內一定看得到高、低兩種電位；只看到一種就是卡住，先重送一次，還是不行就緊急停車、關掉自動模式。該停的腳卻是高電位就馬上拉低。發生的事情都記進 systemd journal。

# car/hw.py Motors
def _pwm_ok(self, en):
    lg, h = self.g
    seen = set()
    for _ in range(40):          # 1 kHz、duty 25~60%：12 ms 內一定看得到高低兩種
        seen.add(lg.gpio_read(h, en))
        if len(seen) == 2:
            return True
        time.sleep(0.0003)
    return False

修好之後實車驗證：讓伺服轉頭 18 次再轉輪子，EN 腳的 PWM 每次都正常；之後跑自動避障，看門狗一次都沒觸發。順便把 PWM 上限從 60% 降到 50%：滿電 3 顆 18650 約 12.6 V，經 L298N 壓降後給 TT 馬達還是偏高。

有人問 EN 能不能直接用跳帽接成一直 enable：不建議。那樣只能在 IN 腳打 PWM，用的還是同一套軟體 PWM；而且程式停掉時就少了「EN 拉低＝馬達一定停」這道保險。真的要更穩，Pi 5 的 GPIO12/13/18/19 剛好都是硬體 PWM 腳，之後可以不改線換成硬體 PWM。

三、操作紀錄：出事時查得到原因

暴衝那次完全查不到發生什麼事，所以加了操作紀錄，直接 print 進 systemd journal（journalctl -u car）：網頁連上／斷線和 IP、所有指令、搖桿推／放開、速度上限變化、自動模式每一步和當時的前方距離、每次掃描的讀值、照軌跡走結束的原因。自動模式的狀態改成 property，一變就記：

@auto_state.setter
def auto_state(self, v):
    if v != getattr(self, "_auto_state", None) and v:
        log(f"自動：{v}" + (f"（前方 {self.front:.0f} cm）" if self.front is not None else ""))
    self._auto_state = v

後面兩個問題都是靠這份紀錄找到的，例如：

19:15:05 掃描：右80 7、右40 37、前 20、左40 20、左80 7
19:15:05 自動：往右轉（右邊已轉 120°）（前方 20 cm）
19:15:09 掃描：右80 7、右40 51、前 39、左40 20、左80 6
19:15:09 自動：左右兩邊都試過不行，後退後迴轉（前方 38 cm）

四、超聲波打到自己的鏡頭

上面紀錄裡「右 80°」每次都是 5~8 cm，連迴轉 180° 之後也一樣，所以不是環境裡的東西。原來是鏡頭改裝到車子右前、又比車頭多出去 3 cm，超聲波往右轉到 70° 以上就打到鏡頭：

超聲波角度	右 75°	右 70°	右 65°	右 60°
量到	6 cm（鏡頭）	8 cm（鏡頭）	126 cm	25 cm

這個假讀值讓程式一直以為右前角貼著東西。修法：掃描角度右邊只到 60°（[-60, -30, 0, 30, 60, 80]），另外過濾「往右超過 68°、又在 12 cm 以內」的讀值，不畫進地圖：

def own_body(self, deg, d):
    """這個讀值是不是打到車子自己（鏡頭裝在右前，超聲波右轉太多會打到）"""
    u = self.cfg.get("ultrasonic", {})
    return d is not None and deg <= u.get("self_deg", -180) and d <= u.get("self_cm", 0)

五、鏡頭側裝：畫面旋轉、扶正、重新校正

為了讓鏡頭不會晃，改成側著鎖（離中心線 8.5 cm、離地 11 cm），畫面就變成躺著的。設定加 rotate（0／90／180／270）在讀進來時就轉正，YOLO、網頁、測距全部用轉正後的畫面；直的畫面寬高對調，水平視角從 60° 變成約 47°，測距的焦距要用原始寬度算：

def geometry(cam):
    """轉正後的畫面寬、高、焦距（像素）、水平視角"""
    W, H = cam.get("width", 640), cam.get("height", 480)
    f = (W / 2) / math.tan(math.radians(cam.get("hfov_deg", 60) / 2))
    if int(cam.get("rotate", 0)) % 180 == 90:
        W, H = H, W
    return W, H, f, round(2 * math.degrees(math.atan(W / 2 / f)), 1)

鎖好後畫面還歪了約 7.4°，再加一個 roll_deg 用 warpAffine 轉回水平（四角會有黑邊）。接著把箱子擺在車子中心線、離車頭 100 cm，用一張畫面同時算出上下（俯角）和左右（偏角）：

• 扶正後箱子底邊在 y≈413（畫面高 640）：照鏡頭高 11 cm、距離 97 cm 應該在水平線下 6.5°，實際在 9.5° → 鏡頭往上仰約 3°。
• 箱子中心在畫面偏左 8.2°，照位置（鏡頭在中心線右邊 8.5 cm）應該偏左 5.0° → 鏡頭往右轉約 3°。
• 驗算：箱子估在車子中心前方 112 cm（實際 113 cm）、偏離中心線 0.2 cm。

鏡頭只離地 11 cm，角度差 1° 在 1 m 外估距離就差約 17%，所以支架一定要鎖緊；之前鏡頭會晃的時候，同一個箱子兩張畫面就差了 2.6°。

六、自動避障：從「固定轉 30°」改成「照掃描結果走」

原本的規則是：被擋住就往一邊轉 30°，那一邊累積轉到 120° 還不行才換另一邊，兩邊都試過才迴轉。實車在擠的房間裡看路線圖就發現問題：累積角度的記憶常常蓋過眼前的掃描結果。例如右前方明明有 73 cm 空間，只因為右邊已經轉滿 120° 就強制轉向左邊（左邊全都只有 20 cm）；或是正前方 157 cm 是空的，卻因為兩邊都「試過」就直接迴轉。

新策略每次都照這次的掃描結果決定：

1. 前方路線空就直接走：不只看正前方，每個角度的讀值都換成「前方／側向距離」，落在車身寬度內、又在 40 cm 以內才算擋住。
2. 正前方有路，只是車角擦到旁邊的東西：麥克納姆輪往另一邊橫移 10 cm 再走，不轉彎。
3. 否則轉到最空、而且夠車寬通過的那個角度（分數＝距離－0.3×角度，少轉一點優先），兩邊差不多時選上次那一邊，避免左右來回擺。
4. 每個方向都不通：後退再掃一次 → 轉 90° 再掃（最多兩次）→ 最後才迴轉 180°。

def blocked_dir(self, seen, c, clear, shift=0.0):
    """往 c 度走、車子先往左平移 shift cm 時，路線（車寬左右各多 path_margin）上 clear 內有沒有東西"""
    half = self.cfg.get("body", {}).get("width_cm", 24) / 2 + self.cfg["auto"].get("path_margin_cm", 4)
    for x, d in seen.items():
        if d is None:
            continue
        r = math.radians(x - c)
        fwd, lat = d * math.cos(r), d * math.sin(r) - shift * math.cos(math.radians(c))
        if 0 < fwd < clear and abs(lat) <= half:
            return True
    return False

async def escape(self, seen, bumped, a, s):
    clear, far = s["stop_cm"] * 1.6, 300
    dist = lambda x: far if seen[x] is None else seen[x]
    # ① 前方路線空：直接走
    if not bumped and dist(0) >= clear and not self.blocked_dir(seen, 0, clear):
        return True
    # ② 正前方有路，只是車角擦到旁邊的東西：往另一邊橫移再走
    if not bumped and dist(0) >= clear:
        for sh in (sc, -sc):
            if not self.blocked_dir(seen, 0, clear, sh) and side_room >= sc + 15:
                await self.strafe(sh, a)
                return True
    # ③ 轉到最空、夠車寬的方向；跟上次同一邊加 15 分
    cands = [x for x in seen if x != 0 and dist(x) >= clear and not self.blocked_dir(seen, x, clear)]
    if not cands:
        return False          # 外面接著：後退再掃 → 轉 90° → 迴轉
    score = {x: min(dist(x), 200) - 0.3 * abs(x) + (15 if self.esc_dir and x * self.esc_dir > 0 else 0) for x in cands}
    best = max(score, key=score.get)
    await self.turn(1 if best > 0 else -1, abs(best), a)
    return True

開發機上做了一個 70 cm 寬、前面封死的假死巷測試（SIM_ROOM=deadend），車子約 53 秒走出來。實車換上新策略後跑了一段：照掃描轉向 13 次、後退再掃 8 次、卡住後退 4 次、橫移閃開 2 次，最後一步的迴轉一次都沒用到。

七、實車照軌跡走（11/13 補）

PWM 的問題修好之後，在家裡走廊實車錄了一段約 5.3 m、142 個點的路線：從起點往前直走約 3 m，左轉橫過，再往回走，繞一圈回到起點附近。接著把車子放回起點，按「▶ 照軌跡走」，車子自己走完全部 142 個點，花了 24 秒，速度上限 60%。

綠線幾乎疊在黃虛線上，轉彎處看起來只差幾公分，最後停在錄的起點旁邊。要注意的是：兩條線都是車子用輪速＋陀螺儀「自己推算」的位置，所以這張圖證明的是「控制有照著計畫走」，不等於地上的真實位置；輪子打滑、地板反光造成的誤差在圖上看不出來，真實偏差要另外用地上的標記或從上面俯拍來量。

走的時候網頁照常更新：鏡頭畫面（YOLO 每秒約 10 次，認得的東西會框出來，第一次跑時有認出冰箱）、正前方超聲波距離、路線圖上車子的位置，左下角顯示「照軌跡走 N/142」。下面是用 gx10 把整段網頁畫面錄下來的影片，實際速度：

網頁錄影是在另一台電腦用 Playwright 開同一個頁面、切到路線圖，等按鈕變成「走第 N 點」就開始連續截圖，走完 3 秒後停，再用 ffmpeg 接成影片（tools/capture_replay.py）。

八、鏡頭再調整後重新校正（11/13）

後來又把鏡頭重新固定了一次，位置變成：離地 10.5 cm、比車頭前緣往後 2 cm、在中心線右邊 8 cm。換了位置，俯角、偏角都要重校。這次用一張 Snoopy 凳子當目標，擺在車子正前方的中心線上，超聲波量到 99 cm：

這次改成用程式自動量：把凳子的藍色布面用 HSV 顏色挑出來，逐欄找上緣、下緣再擬合直線算歪斜角，木頭色的凳腳最下面當落地點，再代回和第五節一樣的公式：

• roll：7.4° → 6.7°（舊參數扶正後凳面還斜約 0.7°）。
• tilt：鏡頭到凳腳落地點約 103 cm，照高度 10.5 cm 應在水平線下 5.8°，畫面上在 8.8° → 往上仰 3.0°。
• yaw：凳子在鏡頭左邊 8 cm，應在左邊 4.5°，畫面上只在左 1.3° → 鏡頭往左轉 3.2°（之前是往右 3°，這次支架轉了約 6°）。
• 驗算：凳子估在車子中心前方 113.7 cm（實際約 114 cm）、離中心線 0.1 cm。

有一個小坑：改了 roll 之後，扶正的畫面裡凳腳的 y 會跟著移動（這次從 400.5 變 406），tilt 一定要用「新 roll 扶正後」的畫面再算一次，不然距離會差好幾公分。

還沒做的

• 照軌跡走已經實車跑完（第七節），但路線圖是車子自己推算的位置；還要用地上標記或俯拍量真實偏差。
• 換了滿電電池、PWM 上限改 50% 之後，速度（cm_per_s）要重量，推算位置才會準。
• 橫移時超聲波看不到側邊；平移用的 strafe_eff 0.8 也還是猜的。
• EN 腳改用 Pi 5 硬體 PWM，徹底避開軟體 PWM。