Android Thingsで4足ロボットを作る ~ Android ThingsとPCA9685でサーボ制御)

Tweet

Android Thingsばかりやっているように見えますが、
カブクでは主にWebサービス周りをやっている大橋です。

他のシリーズ物を立てておきながら、全然別の記事を書いてしまう現象に名前をつけたいですね。

前の記事では3Dプリントで作成した戦車を作りました。
戦車の次に来るのはやはり足があるロボットですよね。
アーマード・○アとかでも2足→タンク(重装備)→4足(中装備、ややスピード重視)→逆関節(軽量)と
装備を変えていくものです。

今回もThingiverseで公開されているjBot Q1 mini Quadruped Robot (Designed by Jason Workshop) ※CC BY-NC 3.0をRaspberry Pi、Android Thingsで動くようにして見たいと思います。

なお今回の記事はAndroid Things Advent Calendar 2017 8日目の記事です。

最終品

以下です。

jBot Q1 mini

jBotQ1では全部で8個のサーボを制御する必要がある為、
Raspberry PiではPWMのピンの数が足りません。
今回はこの8個のサーボを制御するために、最大16個のPWMをI2C経由で制御できる、PCA9685を利用します。

なお、公式のAndroid Things用PCA9685ライブラリは今のところ存在しないようです。

またjBotQ1ではESP-WROOM-02(ESP8266)を利用して、
HTTPサーバーを作り、ブラウザでアクセスし、ロボットを動かしています。

細かいサーボの調整などもこのブラウザ上から行います。
オリジナルでは調整したデータはボード内に保存していました。

今回は、オリジナルと同様にHTTPサーバーをAndroid Things上につくり、
上記の制御・設定用のHTMLを返却するようにします。
また、調整データはSharedPreferencesとして保存します。

まとめると

• サーボの制御はPCA9685で使う
  • 公式制御ライブラリが存在しない為、別のライブラリで制御する
• 制御・設定用にHTTPサーバー立てる
  • Android Things上にHTTPサーバを作る
  • サーボの調整データはSharedPreferencesとして保存

あたりが今回の肝となるポイントです。

必要なハードウェア

• Raspberry Pi 3 Model B & Android Things
• PCA9685
• SG90サーボ × 8
• Raspberry Piを動かすためのモバイルバッテリー
• 3Dプリントした、ロボットの各パーツ
• M2のネジ × 28

実際に作っていく

3Dデータの修正

オリジナルのデータでは、もっと小さい制御ボードを想定しているため、
Raspberry Piが乗っかる様な3Dデータにはなっていないません。
その為、自分でRaspberry Piが乗るように大きさ調整する必要があります。
色々やったのですが、データはまだ公開できていません。すみません。

Android Thingsのコード

全てのコードは以下にあります。
全てKotlinで書いています。

https://github.com/kabuku/jbotq1

ライブラリ

まず、探した所PCA9685を扱うにはいかが良さそうです。
https://github.com/wintersandroid/Android-Things-PCA9685

また、HTTPサーバを立てる必要があるので、AndroidでHTTPサーバを立てるのによく使われる、NanoHTTPDを利用します。

上記を利用するためにプロジェクトのbuild.gradleにmavenリポジトリを追加します。

allprojects {
    repositories {
        google()
        jcenter()
        maven { url 'https://jitpack.io' }　// ←追加
    }
}

またアプリケーションのbuild.gradleに以下を追加します。

dependencies {
    // 色々...

    compile 'com.github.wintersandroid:Android-Things-PCA9685:0.11'  //Android Things用PCA9685制御ライブラリ

    compile 'org.nanohttpd:nanohttpd:2.2.0'

    // 色々...
}

jBotQ1 miniの制御周りのコード(PCBA9685の初期化と利用)

jBotQ1の制御はJbot.kt内で行っています。
(あんまりよくない気もしますが)constructor内で、PCA9685の初期化を行っています。
利用しているライブラリではPCA9685を使ってサーボ制御をするために、PCA9685Servoというクラスが用意されているので利用します。

init {
        val peripheralManagerService = PeripheralManagerService()
        val tweakAngleStr = pref.getString("tweakAngle", "0,0,0,0,0,0,0,0")
        this.tweakAngle = tweakAngleStr.split(",").map { it.toInt() }.toIntArray()
        this.mPca9685Servo = PCA9685Servo(PCA9685.PCA9685_ADDRESS, peripheralManagerService) // PCA9685の初期化

        this.mPca9685Servo.setServoMinMaxPwm(SERVO_MIN, SERVO_MAX, PWMRES_MIN, PWMRES_MAX)  // サーボを制御するためのfreq、dutyの設定
        zeroPosition()
    }

サーボの制御はrunProgramLineで行っています。
各サーボの角度の配列(currentProgram: IntArray)を引数に、現状と比較及びスピード調整しながら行っていきます。

最終的にサーボの角度を設定するのは、setServoAngleメソッドです。ライブラリでmPca9685Servo.setServoAngle(channel, angle)とすれば指定したチャネルのサーボを動かせます。

private fun runProgramLine(currentProgram: IntArray) {
    val interTotalTime = currentProgram[ALL_MATRIX - 1]
    val interDelayCounter = interTotalTime / BASE_DELAY_TIME
    for (interStepLoop in 0 until interDelayCounter) {
        for (servoIndex in 0 until ALL_SERVOS) {
            val currentPosition = mCurrentServosPosition[servoIndex]
            val toPosition = currentProgram[servoIndex]

            if (currentPosition == toPosition) {
                // NOP
            } else if (currentPosition > toPosition) {
                val currentMovePosition = map((BASE_DELAY_TIME * interStepLoop).toLong(), 0L, interTotalTime.toLong(), 0L, (currentPosition - toPosition).toLong())
                if (currentPosition - currentMovePosition.toInt() >= toPosition) {
                    setServoAngle(servoIndex, currentPosition - currentMovePosition.toInt())
                }
            } else if (currentPosition < toPosition) {
                val currentMovePosition = map((BASE_DELAY_TIME * interStepLoop).toLong(), 0L, interTotalTime.toLong(), 0L, (toPosition - currentPosition).toLong())
                if (currentPosition + currentMovePosition <= toPosition) {
                    setServoAngle(servoIndex, currentPosition + currentMovePosition.toInt())
                }
            }
        }
        Thread.sleep(BASE_DELAY_TIME.toLong())
    }
    mCurrentServosPosition = currentProgram.copyOf()
}

fun setServoAngle(servoIndex: Int, angle: Int) {
    val channel = CHANNEL_SERVO_ORDER[servoIndex]
    val tweakedAngle = angle + tweakAngle[servoIndex]
    Log.d(TAG, "Set $servoIndex angle to $tweakedAngle ($angle)")
    mPca9685Servo.setServoAngle(channel, tweakedAngle)
}

実際にどのような順序でどのようにサーボを動かすかは、JbotServoProgram.ktにまとまっています。
このあたりは全てオリジナルのコードから持ってきたものです。

enum class JbotServoProgram(val servoProgramLine: Array<IntArray>) {

    WAIT(arrayOf(
            intArrayOf(90, 90, 90, 90, 90, 90, 90, 90, 500),
            intArrayOf(70, 90, 90, 110, 110, 90, 90, 70, 500)
    )),
    //このあと前進、後退などのプログラムが続く

HTTPサーバ

HTTPサーバの処理は、ControllerServerにまとまっています。
NanoHTTPDを使うとかなり簡単にAndroid上にHTTPサーバを作れます。

class ControllerServer(private val context: Context, private val jbot: Jbot, port: Int = 8080) : NanoHTTPD(port) {
    companion object {
        private val TAG = ControllerServer::class.java.simpleName
    }

    init {
        start(NanoHTTPD.SOCKET_READ_TIMEOUT, false)
    }

    override fun serve(session: IHTTPSession?): Response {

        if (session != null) {
            try {
                when (session.uri) {
                    "/" -> return handleIndex(session)
                    "/save" -> return handleSave(session)
                    "/controller" -> return handleController(session)
                    "/editor" -> return handleEditor(session)
                    "/zero" -> return handleZero(session)
                    "/setting" -> return handleSetting(session)
                    "/online" -> return handleOnline(session)
                }

            } catch (t: Throwable) {
                Log.e(TAG, "get error", t)
                throw t
            }
        }

        return super.serve(session)
    }

色々やっていますが、サーボ調整用とコントローラーが主な処理です。

サーボの初期位置はプログラム上の0ポジションとずれる為、調整が必要です。
そのあたりの処理を行っているのがhandleSettingとhandleSaveです。

handleSettingで調整用の画面を表示し、handleSaveで保存しています。

private fun handleSave(session: IHTTPSession): Response {

        val key = session.parms["key"]
        val value = session.parms["value"]

        val keyInt = Integer.parseInt(key)
        val valueInt = Integer.parseInt(value)

        if (keyInt == 100) {
            for (i in 0 until 8) {
                jbot.setTweakAngle(i, 0)
            }
        } else {
            if (valueInt >= -124 && valueInt <= 124) {
                jbot.setTweakAngle(keyInt, valueInt)
            }
        }
        return newFixedLengthResponse("(key, value)=($key,$value)")
    }

    private fun handleSetting(session: IHTTPSession): Response {
        val inputStream = context.resources.openRawResource(R.raw.setting)
        var contents = inputStream.bufferedReader().readLines().joinToString("\n")
        for (key in arrayOf("4", "0", "5", "1", "6", "2", "7", "3")) {

            contents = contents.replace(Regex("__" + key + "__"), session.parms.getOrDefault(key, jbot.tweakAngle[key.toInt()].toString()))
        }
        return newFixedLengthResponse(contents)
    }

またコントローラーの処理はhandleIndex handleControllerで行っています。

private fun handleIndex(session: IHTTPSession): Response {
        val inputStream = context.resources.openRawResource(R.raw.index)
        val contents = inputStream.bufferedReader().readLines().joinToString("\n")
        return newFixedLengthResponse(contents)
    }

private fun handleController(session: IHTTPSession): Response {
    val pm = session.parms["pm"]
    val servo = session.parms["servo"]

    if (!pm.isNullOrEmpty()) {
        if (pm!!.toInt() == 100) {
            jbot.zeroPosition()
        } else {
            jbot.runProgram(JbotServoProgram.values()[pm.toInt() - 1])
        }
    }
    if (!servo.isNullOrEmpty()) {
        val servoIndex = servo!!.toInt()
        val angle = session.parms["value"]
        jbot.setServoAngle(servoIndex, angle!!.toInt())
    }

    return newFixedLengthResponse("(pm)=($pm) (servo)=($servo)")
}

MainActivity

最後にMainActivityでHTTPサーバの起動と、Jbotクラスの初期化を行います。

class MainActivity : Activity() {

    companion object {
        private val TAG = MainActivity::class.java.simpleName
    }

    private var mJbot: Jbot? = null
    private var mControllerServer: ControllerServer? = null

    override fun onCreate(savedInstanceState: Bundle?) {
        super.onCreate(savedInstanceState)
        val handler = Handler()
        this.mJbot = Jbot(handler, null, getSharedPreferences("jBot", Context.MODE_PRIVATE))
        this.mControllerServer = ControllerServer(this.applicationContext, this.mJbot!!)
    }

    override fun onDestroy() {
        super.onDestroy()
        mJbot?.close()
        mJbot = null

        mControllerServer?.closeAllConnections()
        mControllerServer?.stop()
        mControllerServer = null
    }
}

まとめ

色々やっていますが、ぶっちゃけ全くAndroid Thingsでやる必要はない感じの処理でした。
ESP-WROOM-32などがあれば十分に作れると思います。

Android Thingsの場合は、これにAndroidアプリでコントローラーを作ったり、
Google AssistantやDialogflowを使って、音声操作を化膿したり
Firebaseや、GCPを使って遠隔操作したりすると、非常に面白い物ができるかもしれません。