Android Thingsでヒューマノイドロボット

# はじめに

[KHR-3HV ver2](https://kondo-robot.com/product/03070)は[近藤科学](https://kondo-robot.com/)より販売されている２足歩行ロボットで、入手性も高く、ROBO-ONE Light等での使用率も高い、非常にポピュラーなロボットです。
これをGoogle I/O 2018 の開催前日に正式版がリリースされたばかりのAndroid Thingsでガシャガシャしてみたいと思います。

![IMG_20180514_123937.jpg](https://qiita-image-store.s3.amazonaws.com/0/135868/e78e89c0-9f4b-32d4-9b14-af978f175819.jpeg)


※ 顔がどこかに行ってしまったので、リボテのダンボー載せておきました。~~キモい~~。

# 環境 & 必要なハードウェア
- Surface Pro 4
- Android Things 1.0
- Kotlin
- KHR-3HV ver2
- Raspberry Pi 3
- [ICS 変換基板](https://kondo-robot.com/product/03121)


# サーボの制御方法
KHR-3HV ver2は全身の17箇所にシリアルサーボ「[KRS-2552RHV ICS](https://kondo-robot.com/product/krs-2552rhv-ics)」が使用されています。
このサーボは「[ICS 3.5](https://kondo-robot.com/faq/serial-servo-method-tech-2)」というUARTを利用した近藤科学の独自規格のによって制御します。
複数のサーボをデイジーチェーンを接続するので、PWMでの制御と違い、ピンの本数が少なくて済みます。ハードウェアPWMが２つしかないRaspberryPiでは尚更うれしいですね。

### 角度の計算

KRS2552RHVの動作は[仕様](https://kondo-robot.com/w/wp-content/uploads/KRS_2552RHV_manual.pdf)より、最大動作角は270°で、3500〜11500の値(以下、ポジションデータ)で指定します。
ホーンの中心を0°と見た時、角度からポジションデータを求める式は、  

```
(目的の角度/270 + 0.5) * (11500 - 3500) + 3500
```
となります。


### ICSコマンドの作成
ICS規格に対応したコマンドを作成し、UARTで送信します。
[ICS 3.5の仕様](https://kondo-robot.com/faq/serial-servo-method-tech-2)より、サーボモータの回転角度を指定するには、  

```
第1バイト: 0x80 + サーボのID番号
第2バイト: ポジションデータの上位７ビット
第3バイト: ポジションデータの下位７ビット 
```
前述したようにポジションデータは最大11500なので、2バイトに分割して送信します。  
ここで注意したいのが、7ビットで分割しているという点です。ICSの規格上、第1バイト以外はMSBを0にする必要が有るそうなので、ミスって8ビットで分割しないように。

# ハードウェア

### ICS変換基板
Raspberry PiとKRS2552RHVとの接続は、[ICS変換基板](https://kondo-robot.com/product/03121)を用いります。
これを使用することで、KRS2552RHVなどのICS機器とマイコンのUART接続を別途の回路不要で行うことが出来ます。

以下のように配線してみました。

|ICS変換基板|Raspberry Pi|
----|----
|IOREF|5V|
|GND|GND|
|TX|BCM 15 (RX)|
|RX|BCM 14 (TX)|
|EN_IN|BCM 4|

[Raspberry Pi](https://developer.android.com/things/hardware/raspberrypi-io)と[ICS変換基板](https://kondo-robot.com/faq/ics_board_-tutorial1)のピン配置を参考にしてください。

`EN_IN`は送信と受信を切り替えるためのピンです。 

``` 
HIGH : TX (送信)
LOW  : RX (送信)
```
と、対応しているので、必要に応じてプログラムから切り替えます。

電源はKHR-3HV付属の10.8V 800mAhのニッケル水素バッテリーを使用しました。
電源の関係上一つのデイジーチェーンに8個ほどが限界らしいので、8個と9個に分けてICS変換基板に接続します。

# サーボモータにIDを割り振る
近藤科学の[ICS3.5 マネージャー](https://kondo-robot.com/faq/ics35mag_soft1003)を使用してサーボモータに一意のIDを割り振ります。
こういうのに有りがちなWindowsしか対応してない系のソフトウェアです。
![スクリーンショット (16).png](https://qiita-image-store.s3.amazonaws.com/0/135868/5bc2c35a-5401-82f6-e93e-fb8f725f21f1.png)

[Dual USBアダプターHS](https://kondo-robot.com/product/02116)でサーボとPCを接続し、ICSマネージャーを立ち上げます。
「接続ボタン」を押してID取得後、任意のIDに変更してください。

# プログラミング
## UARTのセットアップ
以下のようなクラスを作成しました。

```kotlin:ICSSerialServo.kt
class ICSSerialServo(manager: PeripheralManager, private val handler: Handler, private val context: Context) {

    private var servoChain: UartDevice? = null
    private var motionJson: JSONObject? = null
    private var enPin: Gpio = manager.openGpio("BCM4") //送受信切り替えピン HIGHで送信,LOWで受信
    private var uartThread: HandlerThread = HandlerThread("uartThread")
    private var uartHandler: Handler
    private val tag = ICSSerialServo::class.java.simpleName

    init {
        uartThread.start()
        uartHandler = Handler(uartThread.looper)
        motionJson = getMotionJson("motion.json")
        servoChain = try {
            manager.openUartDevice("UART0").also { servoChain ->
                servoChain.setBaudrate(BAUD_RATE) //通信速度
                servoChain.setDataSize(DATA_BITS) //ビット長
                servoChain.setParity(UartDevice.PARITY_EVEN) //偶数パリティ
                servoChain.setStopBits(STOP_BITS) //ストップビット
                servoChain.setHardwareFlowControl(UartDevice.HW_FLOW_CONTROL_NONE) //フロー制御なし
            }
        } catch (e: IOException) {
            Log.e(tag, "Unable to open UART device", e)
            handler.sendMessage(handler.obtainMessage(MSG_UART_IOEXCEPTION))
            null
        }
    }

    // ポジションデータからサーボを動作させる
    fun setPos(id: Int, PosData: Int) {
        try {
            if (servoChain != null) {

                uartHandler.post {
                    enPin.setDirection(Gpio.DIRECTION_OUT_INITIALLY_HIGH) // HIGHにセット(送信)
                    val cmd = ByteArray(3)
                    cmd[0] = 0x80.toByte() or id.toByte() // 0x80でポジションデータからサーボの動作
                    cmd[1] = ((PosData shr 7) and 0x007f).toByte() //POS_H
                    cmd[2] = (PosData and 0x007F).toByte() // POS_L

                    servoChain?.write(cmd, cmd.size)
                    servoChain?.flush(UartDevice.FLUSH_OUT)

                    // Log 出力
                    val afPos = (cmd[1].toInt() shl 7) or cmd[2].toInt()
                    Log.d("id", id.toString())
                    Log.d("pos", afPos.toString())
                }
            } else {
                Log.e(tag, "Unable to open UART device")
                throw IllegalStateException()
            }
        } catch (e: IOException) {
            Log.e(tag, "IOException", e)
            handler.sendMessage(handler.obtainMessage(MSG_UART_IOEXCEPTION))
        }
    }

    // 中心を0°と見たときの角度からサーボを動作させる
    fun setDegree(id: Int, degree: Int) {
        val parDegree: Double = (degree.toDouble() / 270.0) + 0.5
        val pos = ((parDegree * 8000) + 3500).toInt()
        setPos(id, pos)
    }


    // ・・・

    fun close() {
        if (servoChain != null) {
            servoChain?.close()
            enPin.setDirection(Gpio.DIRECTION_OUT_INITIALLY_LOW)
        }
    }
}

```
initにてUartDeviceにICSの[仕様](https://kondo-robot.com/faq/serial-servo-method-tech-2)にもとづき、プロパティを設定します。
サーボとの通信はメインスレッドとは別で行います。新たにHandlerThreadを生成し、取得したHandlerからLooperに処理をpostします。
また、プライマリーコンストラクタの引数で呼び出し元のHandlerを受け取り、例外が発生し場合はMessageにてその旨を伝えます。


## GUIから制御してみる
そんな凝ったものじゃないです。
サーボと連動したSeekBarと現在の角度を表示するTextViewをCardViewに乗せてそれをRecyclerViewで表示します。

![screen.png](https://qiita-image-store.s3.amazonaws.com/0/135868/772409b5-e9b7-dc89-b952-0fba28b0bc1c.png)

実装は以下の通り、

```kotlin:ServoCardRecyclerAdapter.kt

open class ServoCardRecyclerAdapter(val servoIDs: List<Int>) : RecyclerView.Adapter<ServoCardRecyclerAdapter.ServoCardVH>() {

    private lateinit var view: View

    class ServoCardVH(view: View) : RecyclerView.ViewHolder(view) {
        val seekBar: SeekBar = view.findViewById(R.id.degreeSeekbar)
        val textId: TextView = view.findViewById(R.id.textID)
        val textResult: TextView = view.findViewById(R.id.resultText)
    }

    protected open var onSeekBarProgressChange = { _: SeekBar?, _: Int, _: Boolean, _: Int, _: TextView -> }

    override fun onCreateViewHolder(parent: ViewGroup?, viewType: Int): ServoCardVH {
        view = View.inflate(parent!!.context, R.layout.layout_card, null)
        return ServoCardVH(view)
    }


    override fun onBindViewHolder(holder: ServoCardVH, position: Int) {
        holder.textId.text = servoIDs[position].toString()
        holder.seekBar.setOnSeekBarChangeListener(object : SeekBar.OnSeekBarChangeListener {
            override fun onStartTrackingTouch(seekBar: SeekBar?) {}
            override fun onStopTrackingTouch(seekBar: SeekBar?) {}
            override fun onProgressChanged(seekBar: SeekBar?, progress: Int, fromUser: Boolean) {
                onSeekBarProgressChange(seekBar, progress, fromUser, servoIDs[holder.adapterPosition], holder.textResult)
            }
        })
    }

    override fun getItemCount(): Int = servoIDs.size

}
```
```kotlin:MainActivity.kt

class MainActivity : Activity() {

    private lateinit var serialServo: ICSSerialServo
    private val tag: String = MainActivity::class.java.simpleName

    override fun onCreate(savedInstanceState: Bundle?) {
        super.onCreate(savedInstanceState)
        setContentView(R.layout.activity_main)

        val uiHandler = object : Handler(Looper.getMainLooper()) {
            override fun handleMessage(msg: Message?) {
                when (msg!!.what) {

                // ・・・

                }
            }
        }

        val manager = PeripheralManager.getInstance()
        serialServo = ICSSerialServo(manager, uiHandler, this@MainActivity)

        val adapter = object : ServoCardRecyclerAdapter((0..16).toList()) {
            override var onSeekBarProgressChange = { _: SeekBar?, progress: Int, _: Boolean, position: Int, textResult: TextView ->
                val degree = progress - 135 
                serialServo.setDegree(position, degree)
                textResult.text = degree.toString()
            }
        }
        servoRecycler.also { recyclerView ->
            recyclerView.layoutManager = GridLayoutManager(this@MainActivity, 2)
            recyclerView.adapter = adapter
        }

        // ・・・
    }


    override fun onDestroy() {
        serialServo.close()
        super.onDestroy()
    }

}

```
DP0.8以降、**PeripheralManager**の取得方法がシングルトンに変更されました。

**onSeekBarProgressChange**をオーバーライドすることでリスナーを実装します。このラムダ式は**onBindViewHolder**でSeekBarにリスナーをセットするとき、**onProgressChanged**内でコールされます。
実装は**MainActivity**で行い、SeekBarから取得した角度の値をTextViewにセットして、サーボに出力します。
その際、SeekBarは最小値0までしか表すことが出来ないので、0〜270の範囲の値を取得して、-135して調整しています。

![gifeditor_20180515_014724.gif](https://qiita-image-store.s3.amazonaws.com/0/135868/93843cbd-9fa2-68ef-859a-88d5d09bb05b.gif)




## モーションデータを作成してみる
`サーボのID`、`ポジションデータor角度`、`動かしてからのスリープ時間`、を羅列したJsonファイルを作成ておき、それをパースしてサーボに命令を出すような形にしてみました。

```json:src/main/assts/motion.json

{
  "MOTION_HELLO": [
    {
      "id": 13,
      "degree": 90,
      "delay": 0
    },
    {
      "id": 3,
      "degree": -90,
      "delay": 1000
    },
    {
      "id": 9,
      "degree": 45,
      "delay": 0
    },
    {
      "id": 10,
      "degree": 0,
      "delay": 0
    },
    {
      "id": 11,
      "degree": -10,
      "delay": 800
    },
    {
      "id": 11,
      "degree": 90,
      "delay": 500
    },
    {
      "id": 11,
      "degree": -10,
      "delay": 500
    },
    {
      "id": 11,
      "degree": 90,
      "delay": 500
    },
    {
      "id": 11,
      "degree": -10,
      "delay": 500
    }
  ],
  
  //・・・
   
}
```
```kotlin:Const.kt
const val KHR_MOTION_HELLO = "MOTION_HELLO"
const val MOTION_TYPE_POS = 0x55
const val MOTION_TYPE_DEGREE = 0x66
````
```kotlin:ICSSerialServo.kt

    //　motion.jsonを読み込んでJSONObjectを生成して返す
    private fun getMotionJson(fileName:String): JSONObject? {
        return try {
            val builder = StringBuilder()
            BufferedReader(InputStreamReader(context.resources.assets.open(fileName))).use { reader ->
                var string = reader.readLine()
                while (string != null) {
                    builder.append(string + System.getProperty("line.separator")) 
                    string = reader.readLine()
                }
            }
            JSONObject(builder.toString())
        } catch (e: JSONException) {
            Log.e(tag, "JSONException")
            handler.sendMessage(handler.obtainMessage(MSG_JSON_FILE_OPEN_FAILED))
            null
        }
    }

   
    fun motionCmd(cmd: String, motionType: Int) {
        val posDataArrays = motionJson?.getJSONArray(cmd)
        for (i in 0 until posDataArrays?.length()!!) {
            val posData = posDataArrays.getJSONObject(i)
            if (motionType == MOTION_TYPE_POS) {
                setPos(posData.getInt("id"), posData.getInt("pos"))
            } else if (motionType == MOTION_TYPE_DEGREE) {
                setDegree(posData.getInt("id"), posData.getInt("degree"))
            }
            delay(posData.getLong("delay"))
        }
    }

    fun delay(time: Long) = uartHandler.post { Thread.sleep(time) }

```

```kotlin:MainActivity.kt
serialServo.motionCmd(KHR_MOTION_HELLO, MOTION_TYPE_DEGREE)
```
**getMotionJson**でファイルを読み込んでJSONObjectを取得し、**motionCmd**でパースしてサーボを動作させます。
Constの`MOTION_TYPE_POS`、`MOTION_TYPE_DEGREE`は識別子で、jsonのモーションデータがポジションデータと角度どちらで表しているのかを判断して、**setPos**か**setDegree**をコールします。

![gifeditor_20180515_014328.gif](https://qiita-image-store.s3.amazonaws.com/0/135868/d4a282f8-99fa-af92-9ef2-90bc8def186a.gif)

怖い。


# おわりに
Android Thingsでハードを制御する最大のメリットはAndroidのリソースを活かしてGUIを実装できる点です。（その点、万人に受けるものでは無いので、これから伸びていくかと言えば、ビミョーなとこ）あと、Kotlinで制御出来るのもAndroidデベロッパーにとっては非常に嬉しいポイントなのではないでしょうか。