ロボットをインターネット越しにブラウザで動かすプログラムをSkyWay WebRTC Gatewayで実装してみる

## はじめに
先日[フォロ](https://www.elekit.co.jp/product/MR-9107)を買いました。多脚でガシガシ歩いて可愛い子です。
この子はプラモデルのように組み立てられるようになっていて、足や胴を動かす部分は単三電池に直結してあげるだけで動くようになっていました。
これなら自分で自由に動かすプログラムが書けるんじゃないかと思い、[SkyWay WebRTC Gateway](https://github.com/skyway/skyway-webrtc-gateway)のARM版とRaspberry Piを利用して実装してみます。

完成したやつ

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">基盤が重くて可哀想だったので大八車を作ってやりました <a href="https://t.co/qTTniaAijV">pic.twitter.com/qTTniaAijV</a></p>&mdash; るんばにゃん (@arukakan) <a href="https://twitter.com/arukakan/status/1039029858079137793?ref_src=twsrc%5Etfw">2018年9月10日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

この記事では↑のロボットをどうやって動かしているのかを書いていきます。
基本的にはインターネット越しにラズパイを操作する話なので、大体のIoTデバイスにも応用できるはずです。

ラズパイカメラを付けたので、遠隔で映像を見ながら操縦できるようになりました。
電源も[モバイルバッテリー](https://panasonic.jp/battery/pocket/p-db/QE-PL101.html)で動いたので、
完全に遠隔で動き回れるようになりました。

## 全体構成
まず全体構成です。せっかくラジコンのように動かすので、PS4コントローラーで自由に動かせるようにします。
PS4コントローラーの信号を取り出し、WebRTCでロボットに送り、Raspberry Piのピンをコントロールすることで回路を動かしてロボットを動かします。
それをモジュール単位で何をしているのか書き出したのが下の流れです。
![kairo-Page-6.png](https://qiita-image-store.s3.amazonaws.com/0/104451/88dffa5e-1b54-c87c-6e5c-b70f312acef7.png)

これを順番に分けて記載します。

## PS4 Controllerのデータ取得

[GamePad API](https://developer.mozilla.org/ja/docs/Web/Guide/API/Gamepad)で簡単に取得できます。
基本的にはAPIの説明ページにあるプログラムをそのまま貼り付ければできます。
全体像はgithubにアップロードしています。

```javascript

// gamepadが繋がれると発火
window.addEventListener("gamepadconnected", connecthandler);

function connecthandler(e) {
    addgamepad(e.gamepad);
}

function addgamepad(gamepad) {
    // gamepadを管理
    controllers[gamepad.index] = gamepad;
    requestAnimationFrame(updateStatus);
}

function updateStatus() {
    for (key in controllers) {
        const controller = controllers[key];
        for (let i = 0; i < controller.axes.length; i++) {
            // PS4コントローラーの左スティックの値を取得
            const val = controller.axes[i].toFixed(4);
            // WebRTCで転送するプログラムに渡す
            callback(i, controller.axes[i].toFixed(4));
        }
    }

    // 定期的に値の確認をする
    requestAnimationFrame(updateStatus);
}

```
## WebRTC部分

実装内容は基本的に前回の記事(https://qiita.com/nakakura/items/a99c468c56db2a92b32f)と同じですので、
ここは今回は割愛します。そのうち汎用的な形で整理してアップロードしますが、
公式の[サンプルプログラム](https://github.com/skyway/skyway-webrtc-gateway/blob/master/samples/data_callee.rb)がほぼそのまま使えます。

## ロボットの操作
### 回路部分

このロボットは足を動かす部分と腰を回す部分で2つのモーターが動くようになっており、電池を繋いでやると回るようになっています。直流なので方向の概念があり、逆向きに流し込むと反対向けに動くようになっています。
例えば足なら、逆向きにつないでやるとバックします。
一方で電圧を変えてスピードを変化させるような機能はなさそうです。

従って制御は、ON(前進)、ON(後退)、OFFの3段階あればよさそうです。
これはリレーモジュールで切り替えをしてやることで実現します。

ここで私がハマった落とし穴ですが、Raspberry PiのGPIOピンは3.3Vで16mAまでしか出力できないので、Arduinoのピン(5V)で操作できるリレーでも動かない場合があります。
リレーは電磁石でスイッチを切りかえるような構造になっていますが、スイッチが切り替わるだけの出力が出せないと切り替えられないのです。
どうするかというと、電源ラインと制御ラインが別々になっているものを利用します。
この種のリレーは、スイッチを動かすのに電源ラインから入っている電力を使うため、切り返し時のための制御ラインは小さな出力でも切り替えられるような仕組みになっています。

今回は[ドクターラボのリレーモジュール](https://www.amazon.co.jp/Generic-%E3%83%89%E3%82%AF%E3%82%BF%E3%83%BC%E3%83%A9%E3%83%9C-%E3%83%AA%E3%83%AC%E3%83%BC%E3%83%A2%E3%82%B8%E3%83%A5%E3%83%BC%E3%83%AB5V%E7%94%A84%E8%84%9A/dp/B01N5MUBP8)を利用しました。
回路は以下のとおりです。

<img src="https://qiita-image-store.s3.amazonaws.com/0/104451/9cc30514-295e-e492-91a7-3b0df31184d0.png" width=400>

このモジュールは4つのリレーがついていて、足と腰を動かすためにそれぞれ2つずつ利用します。
リレーをうまく切り替えてやることで電流の方向切り替えとOFFにすることが可能です。
リレーの切り替えは、GPIOから3.3V電圧を出す出さないで切り替えます。以下出すときをOn、出さないときをOffと書きます。

前進, 腰を右回転する場合


<img src="https://qiita-image-store.s3.amazonaws.com/0/104451/9776524c-a0d6-eb6c-f852-9e3a21fa25f9.png" width=400>

バック, 腰を左回転する場合

<img src="https://qiita-image-store.s3.amazonaws.com/0/104451/869cd2bc-041b-4546-a99c-a07d48c06100.png" width=400>


止める場合

<img src="https://qiita-image-store.s3.amazonaws.com/0/104451/e7b0a981-5c6b-6f99-2f59-4696a664756a.png" width=400>

作用点(×)に流れる電気の向きが変わっているのがわかると思います。

上の図ではリレー1,2と3,4を同じ動かし方をしていますが、個別制御なので腰だけ止めて足だけ進めることももちろん可能です。

### ラズパイ部分

4つのリレーを4つのGPIOピンで制御します。
SkyWay WebRTC GatewayからPS4コントローラーの出力を受け取ったら、状態を制御してラズパイのGPIOを操作して回路を制御します。
例えばRubyだと[pi_piper](https://github.com/jwhitehorn/pi_piper)というgemで簡単に操作できます。

基本的には

```ruby
#26番ピンをoutに設定する場合
pin = PiPiper::Pin.new(:pin => 26, :direction => :out)
#onにする
pin.on
#offにする
pin.off
```

だけでOn/Offが切り替えられます。

```ruby
#!/usr/bin/ruby
require "socket"
require "observer"
require "pi_piper"

#(1)
class State
  NOT_OVERRRIDE = 'not override error'
  def pin()
    raise State::NOT_OVERRRIDE
  end

  def update(flag)
    raise DayOfTheWeek::NOT_OVERRRIDE
  end
end

#(2)
class Forward < State
  def initialize(pin1, pin2)
    @pin1 = pin1
    @pin2 = pin2
  end

  def pin()
    @pin1.off
    @pin2.on
  end

  def update(flag)
    if flag == 0
      Stop.new(@pin1, @pin2)
    elsif flag == -1
      Backward.new(@pin1, @pin2)
    end
  end
end

#(3)
class Backward < State
  def initialize(pin1, pin2)
    @pin1 = pin1
    @pin2 = pin2
  end

  def pin()
    @pin1.on
    @pin2.off
  end

  def update(flag)
    if flag == 0
      Stop.new(@pin1, @pin2)
    elsif flag == 1
      Forward.new(@pin1, @pin2)
    end
  end
end

#(4)
class Stop < State
  def initialize(pin1, pin2)
    @pin1 = pin1
    @pin2 = pin2
  end

  def pin()
    @pin1.off
    @pin2.off
  end

  def update(flag)
    if flag == 1
      Forward.new(@pin1, @pin2)
    elsif flag == -1
      Backward.new(@pin1, @pin2)
    end
  end
end

#(5)
class StateManager
  include Observable

  def initialize()
    gpio_in1 = PiPiper::Pin.new(:pin => 26, :direction => :out)
    gpio_in2 = PiPiper::Pin.new(:pin => 19, :direction => :out)
    gpio_in3 = PiPiper::Pin.new(:pin => 20, :direction => :out)
    gpio_in4 = PiPiper::Pin.new(:pin => 21, :direction => :out)

    @body_state = Stop.new(gpio_in1, gpio_in2)
    @leg_state = Stop.new(gpio_in3, gpio_in4)
  end

  def format(message)
    array = message.split(",")
    key = array[0].to_i
    value = array[1].to_f

    #(6)
    if value < -0.5
      [key, 1]
    elsif value > 0.5
      [key, -1]
    else
      [key, 0]
    end
  end

  #(7)
  def feed(message)
    (key, value) = self.format(message)

    if key == 0
      new_state = @body_state.update(value)
      if new_state
        changed
        @body_state = new_state
        notify_observers(key, new_state.pin())
      end
    elsif key == 1
      new_state = @leg_state.update(value)
      if new_state
        changed
        @leg_state = new_state
        notify_observers(key, new_state.pin())
      end
    end
  end
end

if __FILE__ == $0
  #(8)
  stateManager = StateManager.new

  #(9)
  udps = UDPSocket.open()
  udps.bind("0.0.0.0", 10000)
  loop do
    data = udps.recv(65535).chomp
    stateManager.feed(data)
  end

  udps.close
end
```

ステートモデルでプログラムを書きます。まず状態を定義するための親クラスを作ります(1) 

(2)が前進状態、(3)が後退状態、(4)が停止状態で、それぞれのpin()メソッドの中でピンの状態を切り替えています。それぞれピンのOn, Offの状態が異なりますが、↑の回路図に対応しています。

(5)のクラスで状態を切り替えています。
取得したPS4コントローラーのスティックの出力値は-1.0〜1.0なので、
- -1.0〜-0.5 => 前進
- -0.5〜0.5 => 停止
- 0.5〜1.0 => 後退
としてフラグを立てます。(6)

(7)で状態を実際に切り替え、別の状態に変わる時は該当するクラスのインスタンスを生成してpin()メソッドを呼び出しています。

状態の切り替えとピンの操作するクラスが定期できたら、そのインスタンスを作成し(8)、WebRTC Gatewayから受信するためのUDPポートを作成して、受信した信号を流し込み続けます(9)

これで完成です。