ロボット開発日記6 メカナムホイールローバー製作2(PS4コントローラ連携編)

みなさん、おはようございます、横山です。

昨日はメカナムホイールローバーキットの基板、モーター、メカナムホイール、そしてラズパイの組立までを紹介しました。

本日はPS4コントローラ(DualShock4)とラズパイを連携して、リモートラジコンカーを作りたいと思います。

完成動画はこちらになります。

Raspberry Pi × Dual Shock (PS4 Controller) × Omni Wheel

ここに至るまでが、なかなかの道のりでした。
まあPython組んで動かしてみると、
なぜかラズパイがバグって再起動を繰り返してしまう始末(泣)

悪戦苦闘の内容を紹介したいと思います。

重要：このpythonコードは、Freenove社のライブラリを使用して動作させています。そのため前提条件としてFreenove社の本製品とライブラリ集がないと動作しないことをご理解ください。

不具合と対策

1. PS4コントローラとラズパイ接続

以前に紹介した記事で、Bluetooth経由でPS4コントローラとラズパイを接続する方法をご紹介しました。

どのタイミングでかは分かりませんが、下記コマンドでPS4コントローラを接続するとボタンを押したキーとPython上に表示されるキーが異なるようになってしまいました。

sudo ds4drv
#PS4コントローラのPSボタンとSHARE同時押し
#このコマンドを入力するとBluetooth接続される。

1. コントローラではXボタンを押しているのに、ラズパイ上だと△ボタンが押されていると表示されてしまう。

2. ラズパイをシャットダウンして再起動すると、Bluetooth接続をやり直ししないといけなくなる。※再度sudo ds4drvコマンド入力が必要。

そのため、普通にラズパイのBluetooth検出機能を使って、PS4コントローラと接続しました。

ただこの状態で下記pythonファイルを実行すると、やたらジョイスティック部の数値が更新されてしまうことが分かりました。

from pyPS4Controller.controller import Controller


class MyController(Controller):

    def __init__(self, **kwargs):
        Controller.__init__(self, **kwargs)


controller = MyController(interface="/dev/input/js0", connecting_using_ds4drv=False)
controller.listen()

1. 対策 デッドゾーン機能の追加

R3, L3のジョイスティックの数値がやたら500前後で更新されるため、下記デッドゾーンを導入して対策してみました。

ジョイスティックの絶対値が600を超えない限り、表示されない仕組みです。
想定通り細かい数値が表示されなくなりました。

class MyController(Controller):

    def __init__(self, **kwargs):
        super().__init__(**kwargs)
        self.joystick_dead_zone = 600  # ジョイスティックのデッドゾーン設定

    def on_L3_up(self, value):
        if abs(value) > self.joystick_dead_zone:
            print(f"Left joystick moved up with intensity: {value}")

    def on_L3_down(self, value):
        if abs(value) > self.joystick_dead_zone:
            print(f"Left joystick moved down with intensity: {value}")

    def on_L3_left(self, value):
        if abs(value) > self.joystick_dead_zone:
            print(f"Left joystick moved left with intensity: {value}")

    def on_L3_right(self, value):
        if abs(value) > self.joystick_dead_zone:
            print(f"Left joystick moved right with intensity: {value}")

    def on_R3_up(self, value):
        if abs(value) > self.joystick_dead_zone:
            print(f"Right joystick moved up with intensity: {value}")

    def on_R3_down(self, value):
        if abs(value) > self.joystick_dead_zone:
            print(f"Right joystick moved down with intensity: {value}")

    def on_R3_left(self, value):
        if abs(value) > self.joystick_dead_zone:
            print(f"Right joystick moved left with intensity: {value}")

    def on_R3_right(self, value):
        if abs(value) > self.joystick_dead_zone:
            print(f"Right joystick moved right with intensity: {value}")

controller = MyController(interface="/dev/input/js0", connecting_using_ds4drv=False)
controller.listen()

2. モーターが無限に動いてしまう。

これも沼にハマってしまった例です。
PS4コントローラとローバーキットを連携します。

Xボタンを押すと前進、Xボタンを離すと停止
△ボタンを押すと後退、△ボタンを離すと停止

というような制御を入れてみました。

最初はうまく動作していたのですが、途中からXボタンを押していないのにずっとモーターが動作してしまう始末。ラズパイを再起動しないと直らなくなってしまいました。

2. 対策 デバウンス機能、モーター停止する制御

デバウンス機能を追加し、200msを超えてボタンを押さない限り、ボタンを押したことを認識しない機能を追加しました。

またモーターが確実に停止するように、if any関数でどれかのボタンが押されていることが確定すればTrueを返す、押されていない場合はモーター停止の制御を入れました。

from pyPS4Controller.controller import Controller
from Motor import Motor
import time

class MyController(Controller):
    def __init__(self, **kwargs):
        super().__init__(**kwargs)
        self.joystick_dead_zone = 500  # ジョイスティックのデッドゾーン設定
        self.button_states = {'x': False, 'triangle': False, 'square': False, 'circle': False}
        self.last_press_time = time.time()
        self.debounce_interval = 0.2  # デバウンス間隔を200ミリ秒に設定

    def update_motor(self):
        if any(self.button_states.values()):
            print("モーターを動作させます。")
            if self.button_states['x']:
                PWM.setMotorModel(-2000, -2000, -2000, -2000)
            elif self.button_states['triangle']:
                PWM.setMotorModel(2000, 2000, 2000, 2000)
            elif self.button_states['square']:
                PWM.setMotorModel(-1500, -1500, 2000, 2000)
            elif self.button_states['circle']:
                PWM.setMotorModel(2000, 2000, -1500, -1500)
        else:
            print("モーターを停止します。")
            PWM.setMotorModel(0, 0, 0, 0)

    def check_debounce(self):
        current_time = time.time()
        if (current_time - self.last_press_time) >= self.debounce_interval:
            self.last_press_time = current_time
            return True
        return False

    # ボタン処理にデバウンス機能を追加
    def on_x_press(self):
        if self.check_debounce():
            print("Xボタンが押されました。")
            self.button_states['x'] = True
            self.update_motor()

    def on_x_release(self):
        print("Xボタンが離されました。")
        self.button_states['x'] = False
        self.update_motor()

    # ジョイスティックの動きを制御
    def on_L3_up(self, value):
        if abs(value) > self.joystick_dead_zone:
            print(f"左ジョイスティックが上に動かされました。強度: {value}")

    # 他のボタンとジョイスティックの処理も同様に追加

PWM = Motor()
controller = MyController(interface="/dev/input/js0", connecting_using_ds4drv=False)
controller.listen()

これによりモーターが無限に誤動作する不具合を解消することができました！

ラズパイ起動と同時にpythonファイルを実行

ラズパイの電源をONした後に、いちいちpythonファイルをThony等で開いて、実行するのではなく、自動で指定のpyファイルを実行できるプログラムを試しました。

1. サービスファイルの作成

まずプログラムを作成するためのサービスファイルを作成します。

sudo nano /etc/systemd/system/myprogram.service

2. 実行したpyファイルのパスを記述

[Unit]
Description=My program start at boot
After=multi-user.target

[Service]
Type=idle
ExecStart=/usr/bin/python3 /home/pi/path/to/your/motor_ps4_test.py

[Install]
WantedBy=multi-user.target

保存してエディタを閉じます（Ctrl+X, Y, Enter）

3. サービスファイルの有効化

作成したサービスを有効にして、システム起動時に自動で実行されるように設定します。

sudo systemctl enable myprogram.service

次にサービスを手動で実行します。

sudo systemctl start myprogram.service

サービスの状態を確認するには、これでエラーが発生しなければラズパイ起動後に自動でpyファイルが実行されます。

sudo systemctl status myprogram.service

4. サービスファイルの無効と停止

自動でpyファイルを実行しないようにするためには、下記コマンドを実行します。
これでサービスを無効化して、システム起動時に自動的に実行されなくなります。

sudo systemctl disable myprogram.service

無効化後、サービスファイルを停止すればOKです。

sudo systemctl stop myprogram.service

PSボタン長押しでラズパイシャットダウン機能を追加

最後にラズパイを自動で起動できるようにサービスファイルを設けましたが、ボタンを押せばラズパイをシャットダウンできる制御も入れたいな〜っと思いました。

PSボタンを3秒以上長押しすると、ラズパイがシャットダウンするpythonも作ってみました。

from pyPS4Controller.controller import Controller
import time
import os

class MyController(Controller):
    def __init__(self, **kwargs):
        Controller.__init__(self, **kwargs)
        self.ps_press_time = None

    def on_playstation_button_press(self):
        self.ps_press_time = time.time()

    def on_playstation_button_release(self):
        if self.ps_press_time:
            elapsed_time = time.time() - self.ps_press_time
            if elapsed_time >= 3:
                print("Shutting down Raspberry Pi...")
                os.system("sudo shutdown now")
            self.ps_press_time = None

controller = MyController(interface="/dev/input/js0", connecting_using_ds4drv=False)
controller.listen()

これでラズパイにモニターやVNCを接続しなくても、ボタンを使って、自動起動＆シャットダウンまでできるようになりました。

最後に

今回はかなりpythonのコードに関して情報をまとめましたが、本当に世の中は便利になっていて、chatgpt4に相談すれば大半のことは解決できてしまいました。

本当に便利な世の中で感動していました。

ここまでの長文を読んでいただきありがとうございました。

この記事が皆様の何かの参考になれたら幸いです☺️