Donkey Car をS.BUS対応ラジコン受信機経由で操作する ～controller.py 改造版～

#はじめに

先日、[Donkey Car をS.BUS対応ラジコン受信機経由で操作する](https://qiita.com/rukihena/items/ed17c2245db504dd263a) という記事を書きました。

これは [vJoySerialFeeder](https://github.com/Cleric-K/vJoySerialFeeder) を利用することから、monoとGUI環境を必要とし、リソース食いであるという問題がありました。

Donkey Car の操作入力部分のソースである controller.py を読んだところ、ここに直接 S.BUS プロトコル解釈部分を入れられそうと思い、頑張ってみました！


#ハードウェア

ハードウェアは [Donkey Car をS.BUS対応ラジコン受信機経由で操作する](https://qiita.com/rukihena/items/ed17c2245db504dd263a)  と同様です。

反転回路の有効化と配線を行っておきましょう。

#ソフトウェア

##pySerial インストール
pythonでシリアルポートを読むので、pySerialが必要です。

私は下記コマンドでインストールしました。

```
pip install pyserial
```

pythonのモジュールインストール方法はいくつかあって、環境内で統一した方針でインストールしないと競合して大変なことになりがちですが、Donkey Carの環境で正当な方法がコレなのかどうかはよくわかりません・・・。

##S.BUSプロトコル解釈部分

```RcSbusController``` というクラス名でコントローラーを追加します。
```~/projects/donkeycar/donkeycar/parts/controller.py``` の ```def get_js_controller(cfg):``` の上あたりに下記コードを追加してください。

```python
class RcSbusController(JoystickController):
    def __init__(self, *args, **kwargs):
        super(RcSbusController, self).__init__(*args, **kwargs)
        import serial
        self.ser = serial.Serial("/dev/ttyUSB0", 100000, parity=serial.PARITY_EVEN, stopbits=2, timeout=1)

    def update(self):
        while self.running:
            startbyte = self.ser.read(1)
            if len(startbyte) != 1 :
                self.sbus_err("read timeout")
                continue
            if startbyte[0]!=0x0f :
                self.sbus_err("incorrect start byte")
                continue

            byteary = self.ser.read(24)
            if len(byteary) != 24 :
                self.sbus_err("incorrect frame length")
                continue
            if byteary[23] != 0 :
                self.sbus_err("incorrect end byte")
                continue

            ch1_raw_val = (byteary[0]>>0) + ((byteary[1] & 0x07)<<8)
            ch2_raw_val = (byteary[1]>>3) + ((byteary[2] & 0x3f)<<5)
            ch3_raw_val = (byteary[2]>>6) + ((byteary[3] & 0xff)<<2) + ((byteary[4] & 0x01)<<10)
            ch4_raw_val = (byteary[4]>>1) + ((byteary[5] & 0x0f)<<7)

            #failsafe = byteary[22]>>3 & 1

            self.set_steering(self.raw2axis(ch1_raw_val, False, 1024, 720, 720))
            self.set_throttle(self.raw2axis(ch2_raw_val, True,  1024, 720, 720))

    def raw2axis(self, raw, rev, neutral, neg_max, pos_max):
        val = raw-neutral
        if rev :
            val = -val
        if val < 0 :
            axis = val/neg_max
        else:
            axis = val/pos_max
        axis = min(axis, 1.0)
        axis = max(axis,-1.0)
        return axis

    def sbus_err(self, err_msg):
        #print("S.BUS Error : ", err_msg)
        self.set_steering(0)
        self.set_throttle(0)

    def init_trigger_maps(self):
        pass

    def shutdown(self):
        super(RcSbusController, self).shutdown()
        self.ser.close()

```

この```RcSbusController```を ```sbus```というタイプ名で使えるようにします。
同ファイルの ```def get_js_controller(cfg):``` の elif群の最後に、下記コードを追加してください。

```py
    elif cfg.CONTROLLER_TYPE == "sbus":
        cont_class = RcSbusController
```

```RcSbusController```クラスは```JoystickController``` クラスを継承しています。ジョイスティックデバイス ```/dev/input/js0``` を使うことが前提のクラスのようですが、使わないものでも動きました。

```update()```の中で S.BUSプロトコルの解釈をし、 ```self.set_steering()```, ```self.set_throttle()``` に値を渡せばオッケーのようです。

キャリブレーションは不要な感触でした。真面目にキャリブレーションする場合は raw2axis() に渡す引数で調整してみてください。


##設定

```~/mycar/myconfig.py``` の下記2つの設定を変更します。

```py
CONTROLLER_TYPE='sbus'
JOYSTICK_DEADZONE = 0.05
```

```JOYSTICK_DEADZONE``` はニュートラル幅を指定します。スロットルのノイズで AUTO_RECORD_ON_THROTTLE が上手く動かない問題を回避できます。（前回の記事ではvJoySerialFeederの設定でどうにかしてました）

あとは、いつものように

```
cd ~/mycar
python manage.py drive --js
```

で学習走行させてみましょう！

#機能追加 ～3chでモード切替～

プロポのスライドスイッチを3chに割り当てて、その操作からPilot modeの切替をできるようにしてみましょう。（プロポのスイッチの割り当て方法は、プロポの取扱説明書をご参照ください）

```self.set_throttle(self.raw2axis(ch2_raw_val, True,  1024, 720, 720))```の下に下記コードを追加します。

```py

            if ch3_raw_val < 664 :
                new_mode = "local"
            elif 1384 < ch3_raw_val :
                new_mode = "user"
            else :
                new_mode = "local_angle"
            
            if self.mode!=new_mode :
                self.mode = new_mode
                print('new mode:', self.mode)
```

これでコントローラーからの操作と学習走行の切り替えができるようになりました。

#発展

```/dev/input/js0``` を使わないことにより、linux依存でなくなりました。つまりどういうことかというと、Windowsのシミュレーター環境でも使えてしまいます！

ホストPCはminiconda環境ですので、pySerial のインストールは下記コマンドで行います。

```
conda activate donkey
conda install -c anaconda pyserial
```

また、 ```controller.py```に追加した中の```serial.Serial()``` に渡しているデバイス名を、お使いの環境でのデバイス名に書き換えましょう。
Windowsなら、```"COM3"```などになります。macOSでは試していませんが、原理的にはできると思います。

現実世界のラジコンのプロポで仮想のラジコンを操作するのはシュールで楽しいですｗ