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

![IMG_3051.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/502127/65e3286b-b5c2-5a30-c4ff-522ddc5880e0.jpeg)
↑↑ 仮で配線を繋いだので雑です。。。

## この記事について
ラズベリーパイのラジコンをJoy-Conで操作できるようにしたPythonの紹介です。
今回のラジコンは[Raspberry Piで学ぶ電子工作](https://gendai.ismedia.jp/list/books/bluebacks/9784062579773)を参考にしています。

## 回路図
![bitfig10-04-NewMD.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/502127/cd9b343d-c943-5726-ed4d-879466d79c1c.png)

https://raspibb1.blogspot.com/

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

![シンクロボタン.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/502127/05f98d04-c910-ece0-5282-caa25b0bc1d3.png)
https://support.nintendo.co.jp/app/answers/detail/a_id/33820

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

```bash
$ bluetoothctl
```

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

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

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

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

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

```bash
[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を基に作成しました。

```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()
```

## 実行

```bash
$ sudo python3 joy-con.py
```

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

```python
import time
import pygame
import webiopi
import logging
```

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

```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を保存します  
例えば、右上にスティックを入力した場合も前進として処理していますが、上→右上と連続して入力された場合はモーターの回転を変更しないように処理します

```python
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`関数を実行します

```python
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を設定します

```python
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と違いがあった場合のみ実行します

```python
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と比較を行い、違いがあった場合はその値を前回の値として保存します

```python
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