#はじめに

先日、Donkey Car をS.BUS対応ラジコン受信機経由で操作するという記事を書きました。

これは vJoySerialFeeder を利用することから、monoとGUI環境を必要とし、リソース食いであるという問題がありました。

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

#ハードウェア

ハードウェアは Donkey Car をS.BUS対応ラジコン受信機経由で操作すると同様です。

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

#ソフトウェア

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

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

pip install pyserial

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

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

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

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群の最後に、下記コードを追加してください。

    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つの設定を変更します。

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))の下に下記コードを追加します。


            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では試していませんが、原理的にはできると思います。

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