はじめに

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

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

環境 & 必要なハードウェア

Surface Pro 4
Android Things 1.0
Kotlin
KHR-3HV ver2
Raspberry Pi 3
ICS 変換基板

サーボの制御方法

KHR-3HV ver2は全身の17箇所にシリアルサーボ「KRS-2552RHV ICS」が使用されています。
このサーボは「ICS 3.5」というUARTを利用した近藤科学の独自規格のによって制御します。
複数のサーボをデイジーチェーンを接続するので、PWMでの制御と違い、ピンの本数が少なくて済みます。ハードウェアPWMが２つしかないRaspberryPiでは尚更うれしいですね。

角度の計算

KRS2552RHVの動作は仕様より、最大動作角は270°で、3500〜11500の値(以下、ポジションデータ)で指定します。
ホーンの中心を0°と見た時、角度からポジションデータを求める式は、

(目的の角度/270 + 0.5) * (11500 - 3500) + 3500

となります。

ICSコマンドの作成

ICS規格に対応したコマンドを作成し、UARTで送信します。
ICS 3.5の仕様より、サーボモータの回転角度を指定するには、

第1バイト: 0x80 + サーボのID番号
第2バイト: ポジションデータの上位７ビット
第3バイト: ポジションデータの下位７ビット

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

ハードウェア

ICS変換基板

Raspberry PiとKRS2552RHVとの接続は、ICS変換基板を用いります。
これを使用することで、KRS2552RHVなどのICS機器とマイコンのUART接続を別途の回路不要で行うことが出来ます。

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

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

Raspberry PiとICS変換基板のピン配置を参考にしてください。

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

HIGH : TX (送信)
LOW  : RX (送信)

と、対応しているので、必要に応じてプログラムから切り替えます。

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

サーボモータにIDを割り振る

近藤科学のICS3.5 マネージャーを使用してサーボモータに一意のIDを割り振ります。
こういうのに有りがちなWindowsしか対応してない系のソフトウェアです。

Dual USBアダプターHSでサーボとPCを接続し、ICSマネージャーを立ち上げます。
「接続ボタン」を押してID取得後、任意のIDに変更してください。

プログラミング

UARTのセットアップ

以下のようなクラスを作成しました。

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の仕様にもとづき、プロパティを設定します。
サーボとの通信はメインスレッドとは別で行います。新たにHandlerThreadを生成し、取得したHandlerからLooperに処理をpostします。
また、プライマリーコンストラクタの引数で呼び出し元のHandlerを受け取り、例外が発生し場合はMessageにてその旨を伝えます。

GUIから制御してみる

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

実装は以下の通り、

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

}

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して調整しています。

モーションデータを作成してみる

サーボのID、ポジションデータor角度、動かしてからのスリープ時間、を羅列した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
    }
  ],
  
  //・・・
   
}

Const.kt

const val KHR_MOTION_HELLO = "MOTION_HELLO"
const val MOTION_TYPE_POS = 0x55
const val MOTION_TYPE_DEGREE = 0x66

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) }

MainActivity.kt

serialServo.motionCmd(KHR_MOTION_HELLO, MOTION_TYPE_DEGREE)

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

怖い。

おわりに

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