ラズベリーパイのラジコンをJoy-Conで動かしてみた

↑↑ 仮で配線を繋いだので雑です。。。

この記事について

ラズベリーパイのラジコンをJoy-Conで操作できるようにしたPythonの紹介です。
今回のラジコンはRaspberry Piで学ぶ電子工作を参考にしています。

回路図

Joy-Conをラズベリーパイへ登録

• Joy-Conのシンクロボタンを押下する(Joy-Conのランプが点滅している状態にする)
  • この先の手順はランプが点滅している状態で実施します
  • もう一度シンクロボタンを押下すると同期が解除されます（ランプが消灯する）

https://support.nintendo.co.jp/app/answers/detail/a_id/33820

• ラズベリーパイでスキャンする

$ bluetoothctl

こんな感じでJoy-Conが検出されます

[NEW] Device XX:XX:XX:XX:XX:XX Joy-Con (R)  # MACアドレスは端末ごとに異なる

• Joy-Conが検出されない場合

[bluetooth]# scan on
# 検出されたらスキャンを停止する
[bluetooth]# scan off

• Joy-Conを接続する
  • 登録完了後もJoy-Conのランプが点滅し続けていますが、問題ありません
    失敗していれば画面にエラーが表示されます
  • 2回目以上はconnectのみでOKです

[bluetooth]# pair XX:XX:XX:XX:XX:XX
[bluetooth]# connect XX:XX:XX:XX:XX:XX
[bluetooth]# trust XX:XX:XX:XX:XX:XX

コード

コードは参考書籍のjavascript/Pythonを基に作成しました。

joy-con.py

# !/usr/bin/env python
# -*- coding: utf-8 -*-

import time
import pygame
import webiopi
import logging

# GPIOライブラリの取得
GPIO = webiopi.GPIO

PWM1 = 25
PWM2 = 24
PWM3 = 23
PWM4 = 22

duty = 0.9

duty_befor = [0, 0, 0, 0]

def main():
    pygame.init()
    joys = pygame.joystick.Joystick(0)
    joys.init()

    try:
        while True:
            events = pygame.event.get()
            for event in events:
                if event.type == pygame.JOYHATMOTION:
                    logging.debug('Joy-con Stick event catched : ',event)
                    if event.value[0]==1: # 前進
                        pwm4Write(duty, 0, duty, 0)
                        logging.debug('Forward')
                    elif event.value[0]==-1: # 後退
                        pwm4Write(0, duty, 0, duty) 
                        logging.debug('Retreats')
                    elif event.value[0]==0 and  event.value[1]==-1: # 右旋回
                        pwm4Write(duty, 0, 0, duty-0.05)
                        logging.debug('Turn Right')
                    elif event.value[0]==0 and  event.value[1]==1: # 左旋回
                        pwm4Write(0, duty, duty-0.05, 0)
                        logging.debug('Turn Left')
                    else:
                        pwm4Write(0, 0, 0, 0) # 停止
                        logging.debug('Stopped')
            time.sleep(0.1)
    except KeyboardInterrupt:
        destroy()

# WebIOPiの起動時に呼ばれる関数
def setup():
    # GPIOのセットアップ
    GPIO.setFunction(PWM1, GPIO.PWM)
    GPIO.setFunction(PWM2, GPIO.PWM)
    GPIO.setFunction(PWM3, GPIO.PWM)
    GPIO.setFunction(PWM4, GPIO.PWM)
    # 初期のデューティー比を0%に（静止状態）
    GPIO.pwmWrite(PWM1, 0)
    GPIO.pwmWrite(PWM2, 0)
    GPIO.pwmWrite(PWM3, 0)
    GPIO.pwmWrite(PWM4, 0)
    print('Set up is Success. Enjoy!!')

# WebIOPi終了時に呼ばれる関数
def destroy():
    # GPIO関数のリセット（入力にセットすることで行う）
    GPIO.setFunction(PWM1, GPIO.IN)
    GPIO.setFunction(PWM2, GPIO.IN)
    GPIO.setFunction(PWM3, GPIO.IN)
    GPIO.setFunction(PWM4, GPIO.IN)
    logging.debug('destroy is success. GPIO sets IN.')
    print('Shutdown Process is Success. GoodBay!!')

# 4つのPWMにデューティー比をまとめてセット
def pwm4Write(duty1, duty2, duty3, duty4):
    if beforDutyCheck(duty1, duty2, duty3, duty4):
        GPIO.pwmWrite(PWM1, float(duty1))
        GPIO.pwmWrite(PWM2, float(duty2))
        GPIO.pwmWrite(PWM3, float(duty3))
        GPIO.pwmWrite(PWM4, float(duty4))
        logging.debug('PWM1:{0} PWM2:{1} PWM3:{2} PWM4:{3}'.format(duty1,duty2,duty3,duty4))

def beforDutyCheck(duty1, duty2, duty3, duty4):
    if duty1 == duty_befor[0] and duty2 == duty_befor[1] and duty3 == duty_befor[2] and duty4 == duty_befor[3]:
        return False
    else:
        duty_befor[0] = duty1
        duty_befor[1] = duty2
        duty_befor[2] = duty3
        duty_befor[3] = duty4
        return True

if __name__ == '__main__':
    print('Wait for Preparing...')
    setup()
    main()

実行

$ sudo python3 joy-con.py

コード解説

あまりに説明がざっくりしているので、コードを説明します。

import time
import pygame
import webiopi
import logging

• パッケージをインポートしています
  pygameはpythonでゲームを制作するためのモジュールです

PWM1 = 25
PWM2 = 24
PWM3 = 23
PWM4 = 22

duty = 0.9

duty_befor = [0, 0, 0, 0]

• PWMはGPIOピンの指定で使用します
• dutyはモーターの回転数のようなものです
  参考書籍によると、モーターの劣化を防ぐために「1」とはしないそうです
• duty_beforは直前のdutyを保存します
  例えば、右上にスティックを入力した場合も前進として処理していますが、上→右上と連続して入力された場合はモーターの回転を変更しないように処理します

def main():
    pygame.init()
    joys = pygame.joystick.Joystick(0)
    joys.init()

    try:
        while True:
            events = pygame.event.get()
            for event in events:
                if event.type == pygame.JOYHATMOTION:
                    logging.debug('Joy-con Stick event catched : ',event)
                    if event.value[0]==1: # 前進
                        pwm4Write(duty, 0, duty, 0)
                        logging.debug('Forward')
                    elif event.value[0]==-1: # 後退
                        pwm4Write(0, duty, 0, duty) 
                        logging.debug('Retreats')
                    elif event.value[0]==0 and  event.value[1]==-1: # 右旋回
                        pwm4Write(duty, 0, 0, duty-0.05)
                        logging.debug('Turn Right')
                    elif event.value[0]==0 and  event.value[1]==1: # 左旋回
                        pwm4Write(0, duty, duty-0.05, 0)
                        logging.debug('Turn Left')
                    else:
                        pwm4Write(0, 0, 0, 0) # 停止
                        logging.debug('Stopped')
            time.sleep(0.1)
    except KeyboardInterrupt:
        destroy()

• pygame.init()の3行でスティック入力を検知する準備を行います
• if event.type == pygame.JOYHATMOTIONでスティック入力があった場合のみ処理を行います
  右上/左上は上入力、右下/左下は下入力として処理しています
  ちなみにボタン入力はevent.type == pygame.JOYBUTTONDOWNとすることで検知できます
• pwm4Write関数でモーターの回転を設定します。-0.05となっているのは回転の微調整です
• Ctl-cが入力された場合はdestroy関数を実行します

def setup():
    # GPIOのセットアップ
    GPIO.setFunction(PWM1, GPIO.PWM)
    GPIO.setFunction(PWM2, GPIO.PWM)
    GPIO.setFunction(PWM3, GPIO.PWM)
    GPIO.setFunction(PWM4, GPIO.PWM)
    # 初期のデューティー比を0%に（静止状態）
    GPIO.pwmWrite(PWM1, 0)
    GPIO.pwmWrite(PWM2, 0)
    GPIO.pwmWrite(PWM3, 0)
    GPIO.pwmWrite(PWM4, 0)
    print('Set up is Success. Enjoy!!')

• 実行時の初期設定を行います
  4つのGPIOピンをPWMとして設定し、0を設定します

def pwm4Write(duty1, duty2, duty3, duty4):
    if beforDutyCheck(duty1, duty2, duty3, duty4):
        GPIO.pwmWrite(PWM1, float(duty1))
        GPIO.pwmWrite(PWM2, float(duty2))
        GPIO.pwmWrite(PWM3, float(duty3))
        GPIO.pwmWrite(PWM4, float(duty4))
        logging.debug('PWM1:{0} PWM2:{1} PWM3:{2} PWM4:{3}'.format(duty1,duty2,duty3,duty4))

• 4つのGPIOをまとめて制御します
  前回のdutyと違いがあった場合のみ実行します

def beforDutyCheck(duty1, duty2, duty3, duty4):
    if duty1 == duty_befor[0] and duty2 == duty_befor[1] and duty3 == duty_befor[2] and duty4 == duty_befor[3]:
        return False
    else:
        duty_befor[0] = duty1
        duty_befor[1] = duty2
        duty_befor[2] = duty3
        duty_befor[3] = duty4
        return True

• 前回のdutyと比較を行い、違いがあった場合はその値を前回の値として保存します

if __name__ == '__main__':
    print('Wait for Preparing...')
    setup()
    main()

• Pythonが実行された時に、setup関数とmain関数が実行されるようにしています

今後やりたいこと

ラズベリーパイにカメラモジュールを付けて、ヘッドマウントディスプレイ的なもので映像が見れたら面白そうだなと思いました。

参考

• https://qiita.com/taznica/items/a4f5123f84641e4cac40
• http://bluebacks.kodansha.co.jp/books/9784062579773/appendix/
• https://support.nintendo.co.jp/app/answers/detail/a_id/33820