LoginSignup
4
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

@ShuzoCreate(しゅーぞー)

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

Posted at

はじめに

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

IMG_20180514_123937.jpg

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

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

  • 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しか対応してない系のソフトウェアです。
スクリーンショット (16).png

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で表示します。

screen.png

実装は以下の通り、

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

gifeditor_20180515_014724.gif

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

サーボの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をコールします。

gifeditor_20180515_014328.gif

怖い。

おわりに

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

4
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
4
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?