ラズパイ5のサーボモーターのジッター問題に対処した話

Raspberry pi4ではpigpioでサーボモーターを制御していてスムーズ

https://zenn.dev/jam0824/articles/dce67854564627
この記事でラズパイ4でおしゃべりAIロボット・ハロを作っている話をしました。
この時はpanとtiltのサーボモーターをpigpioで制御していました。
pigpioはDMA(DirectMemoryAccess)でPWMを作っているため安定した動きをしていました。
https://x.com/mty_mno/status/1964643843477062084

Raspberry pi5ではpigpioが使えなくなってしまった…

せっかくラズベリーパイ5が出ているのだから置き換えようと思いゲットしました。
しかしラズパイ4と入れ替えた結果、pigpioが弾かれてしまっていました。
原因としては、pigpioはBCM283x系のSoCでのライブラリだったのが、ラズパイ5ではアーキテクチャが一新され、GPIO周りを制御するRP1チップが導入されメモリアドレスマップが変わってしまったためです。

gpiozeroを使ったらジッター（サーボモーターのブルブル震え)が悲惨なことに

仕方がないので、pythonでgpiozeroを使ってコードを書き直したら……
サーボモータのビクビク、ジジジ、突如激しい動きなど、ジッターがひどすぎます(泣)
これはソフトウェアPWMであるためです。つまりソフトウェアベースのタイマー処理で動いています。
この場合、ソフトウェアの負荷や割り込み発生でサーボ信号(パルス幅)がブレます。
その結果、ビクビクとしたジッターの現象として現れます。
ソフトウェアPWMでは避けられそうにありません。

ソフトPWMが使えないならハードPWMを使えばいいじゃない

ラズパイにはSoC内蔵のPWMハードウェアブロックがあります。
GPIO18とGPIO19です。（対応するピンは限られている）
ハードPWMではハードウェアが自動でパルスを生成するので、割り込みやCPU負荷に影響されず、安定したパルス幅を維持できます。
これを使っていきます。

ハードPWMの使い方

GPIO18と19を使えるようにする

まずは /boot/firmware/config.txt に以下を追記し、再起動します。

dtoverlay=pwm-2chan

これでGPIO18とGPIO19が使えるようになります。
再起動後に ls /sys/class/pwm を実行してみてください。

pi@raspberrypi:~ $ ls /sys/class/pwm
pwmchip0 pwmchip1

このように表示されたらOKです。

サーボの信号線をGPIO18(19)に挿す

私の場合はpan用のサーボモーターを18番、tilt用のサーボモーターを19番につないでいます。
電源は5V使用です。

rpi-harware-pwmをインストールする

私はpythonを使っているのでpython前提です。
以下をインストールします。

pip install rpi-hardware-pwm

サンプルコード

pythonでのサンプルコードは以下です。

#!/usr/bin/env python3
import time
from rpi_hardware_pwm import HardwarePWM

def angle_to_duty(angle, min_angle=0, max_angle=180,
                  min_pulse=0.5e-3, max_pulse=2.5e-3, frame_width=20e-3):
    """角度を duty[%] に変換"""
    angle = max(min_angle, min(max_angle, angle))
    ratio = (angle - min_angle) / (max_angle - min_angle)
    pulse = min_pulse + (max_pulse - min_pulse) * ratio
    duty = (pulse / frame_width) * 100
    return duty

if __name__ == "__main__":
    # Pi 5 の場合: ch2=GPIO18, ch3=GPIO19
    pan = HardwarePWM(pwm_channel=2, hz=50)
    tilt = HardwarePWM(pwm_channel=3, hz=50)

    try:
        print("中央へ")
        pan.start(angle_to_duty(90))
        tilt.start(angle_to_duty(90))
        time.sleep(2)

        print("左/上へ")
        pan.change_duty_cycle(angle_to_duty(45))
        tilt.change_duty_cycle(angle_to_duty(45))
        time.sleep(2)

        print("右/下へ")
        pan.change_duty_cycle(angle_to_duty(135))
        tilt.change_duty_cycle(angle_to_duty(135))
        time.sleep(2)

        print("中央に戻す")
        pan.change_duty_cycle(angle_to_duty(90))
        tilt.change_duty_cycle(angle_to_duty(90))
        time.sleep(2)

    finally:
        pan.stop()
        tilt.stop()

最後に

この問題にぶつかって検索していたら「あきらめましょう！」といったことが出てきて、raspberry pi4に本当に戻そうかと思っていました。
自分の備忘録的な記事ですが、他の困っている皆様の助けになりましたら幸いです。