第3回：RCカーをリモート操作、Raspberry PiにてTAMIYA1/12RC XBランチボックスのモーターとサーボを動かしてみよう

sudo nano joystick_controltest.py

import inputs
from gpiozero import PWMOutputDevice
from gpiozero.pins.pigpio import PiGPIOFactory
import time
import sys

# GPIOの設定（PiGPIOFactoryを使って精密制御）
factory = PiGPIOFactory()

# サーボとESCの初期化
try:
    # サーボ（GPIOピン12）
    servo = PWMOutputDevice(12, pin_factory=factory, frequency=50)  # 50HzのPWM信号
    # ESC（GPIOピン13）
    esc = PWMOutputDevice(13, pin_factory=factory, frequency=50)    # 50HzのPWM信号
except Exception as e:
    print(f"GPIOの初期化中にエラーが発生しました: {e}")
    sys.exit(1)

def set_servo_angle(angle):
    """
    サーボの角度を設定します。角度は-90度から90度の範囲です。
    """
    # 角度をデューティサイクルに変換
    min_pulse_width = 0.5 / 1000
    max_pulse_width = 2.4 / 1000
    frame_width = 1 / 50  # 50Hzのフレーム幅

    pulse_width = ((angle + 90) / 180) * (max_pulse_width - min_pulse_width) + min_pulse_width
    duty_cycle = pulse_width / frame_width
    servo.value = duty_cycle

def set_esc_speed(pulse_width_ms):
    """
    ESCの速度を設定します。パルス幅はミリ秒単位で指定します。
    """
    # パルス幅をデューティサイクルに変換
    pulse_width_s = pulse_width_ms / 1000  # パルス幅を秒単位に変換
    frame_width_s = 1 / 50  # フレーム幅（秒）
    duty_cycle = pulse_width_s / frame_width_s  # デューティサイクルの計算
    esc.value = duty_cycle

def initialize_devices():
    """
    サーボとESCを安全な初期状態に設定します。
    """
    set_servo_angle(0)       # サーボを中央に設定
    set_esc_speed(1.5)       # ESCを停止状態に設定
    time.sleep(1)            # 安定するまで待機

def shutdown_devices():
    """
    サーボとESCを安全な停止状態に設定します。
    """
    set_servo_angle(0)       # サーボを中央に設定
    set_esc_speed(1.5)       # ESCを停止状態に設定
    time.sleep(1)            # 安定するまで待機
    servo.close()
    esc.close()

def main():
    print("ジョイスティックを監視中...")

    # デバイスの初期化
    initialize_devices()

    # ゲームパッドの取得
    try:
        gamepad = inputs.devices.gamepads[0]
        print(f"ゲームパッドが接続されました: {gamepad}")
    except IndexError:
        print("エラー: ゲームパッドが接続されていません。プログラムを終了します。")
        sys.exit(1)

    try:
        while True:
            # イベントの取得
            events = gamepad.read()

            for event in events:
                if event.ev_type == "Absolute":
                    if event.code == "ABS_X":
                        # サーボ操作
                        axis_value = event.state / 32767  # -1 から 1 の範囲に正規化
                        angle = axis_value * 90  # -90度から90度の範囲に変換

                        # 角度を -90 から 90 に制限
                        angle = max(min(angle, 90), -90)

                        set_servo_angle(angle)
                        #print(f"サーボの角度を {angle:.1f} 度に設定")

                    elif event.code == "ABS_RY":
                        # モーター操作
                        axis_value = event.state / 32767  # -1 から 1 の範囲に正規化

                        # デッドゾーンの設定
                        dead_zone = 0.05
                        if abs(axis_value) < dead_zone:
                            axis_value = 0

                        # パルス幅を1.4msから1.6msにマッピング
                        pulse_width = 1.5 + (axis_value * 0.1)

                        # パルス幅を1.4msから1.6msに制限
                        pulse_width = max(min(pulse_width, 1.6), 1.4)

                        set_esc_speed(pulse_width)
                        #print(f"ESCパルス幅を {pulse_width:.2f} msに設定")

            # CPU使用率を下げるための短いスリープ
            time.sleep(0.001)

    except KeyboardInterrupt:
        print("\nプログラムを終了します...")

    except OSError:
        print("エラー: ゲームパッドが切断されました。プログラムを終了します。")

    except Exception as e:
        print(f"エラーが発生しました: {e}")

    finally:
        # デバイスを安全な状態にシャットダウン
        shutdown_devices()
        print("デバイスを安全な状態に設定しました。")

if __name__ == "__main__":
    main()

python3 joystick_controltest.py