ソフトウェアを作ろう！
それではハードウェアがとりあえずできたのでロボットを制御するソフトウェア作りに入ります。ロボットに搭載予定のRaspberry Pi Pico用のソフトウェアをPlatformIOで作っていきます。
ロボットは様々な姿勢やポーズをいろいろなパターンで切り替えたり、得られたセンサ値によってその振る舞いを変えたりすることが必要になります。これらをすべて単純な条件分岐などで実現しようとすると非常に大変です。また新たなポーズを追加したり、ということも難しくなります。そこで今回は「ステートマシン」としてロボットのソフトウェアを作っていくことを考えます。
ステートマシンとは
「ステートマシン」という単語はあまり聞き慣れないかもしれません。有限ステートマシン(Finite State Machine)とも呼ばれ、ある状態(State)から別の状態へ遷移していくことで振る舞いを変えていく機械のことを指します。ロボットのように多様な状況に置かれ、それぞれで異なる振る舞いをしなければならないような場合に有効な方法です。例えば「歩きながら、ときどき周りを見回す、しばらくすると寝る」というような場合、図１のように表します。この表し方を状態遷移図といいます。
しばらくすると寝る」を状態遷移図で表現
図１において、各○は一つの状態を表します。そして矢印はある状態から状態への遷移（移り変わり）を表しています。同じ○から○への矢印はその状態を何度も繰り返すことを表現しています。この図で言えば、「歩く」状態を繰り返し、ときどき「見回す」状態に遷移し、更に「見回す」から「寝る」に遷移する場合があることを表しています。この遷移は特定の条件であったり、確率であったりします。
このようにロボットが取りうる状況をすべて”状態”として用意し、状態間を遷移で繋いであげることによって複雑な振る舞いも簡潔に表現可能となります。
今回は後の事も考え、このステートマシンの形でソフトウェア実装できるように作っていきます。
ステートマシンを実装！
今回はPlatformIOを使ってC++で実装していきます。まずはRobotクラスを作成します。このクラスはロボットの全体の管理を行います。次にRobotStateクラスを作成します。これがステートマシンにおける「状態」になります。またRobotStateでは別の状態を登録しておき、条件に応じてその状態に遷移できるようにします（図２）。実際には他にも細々と実装していますが大まかにはこのような実装です。
これらはすべて基底クラスとして使われ、サブクラスに継承されて実装するものとして用意しておきます。要するに雛形として形だけ用意しておいて、個別の用途ごとに異なる実装を行おう、というものです。
今回はとりあえず１つのモーションを実行する状態を一つだけ用意し、実行させてみましょう（図３）。首振り動作と歩行モーションの状態を作って、それぞれ1つずつで動作させてみます。
動作テストをしてみる
実際に動作させてみましょう！まずは首振りモーションです（動画１）
次は歩行モーションです（動画２）
歩行モーションは6つのポーズを繰り返すことでできており、その６つのポーズを順番に繰り返すことで歩行します。
図４のように、（２）で片足を上げて（３）で前に出し、（４）で足を下ろす。反対側で同じことを（５）、（６）、（７）と行います。どうやらちゃんと歩けているようです！
ちなみに搭載しているサーボモータSG90は本来4.8Vでの動作なのですが、電池１本の3.7Vでも十分動作しました。
ハンチング問題をどうするか
さてここで一つ問題が発生です。動画３を見てみましょう。重心移動テストをしていたときのことです。
頭が時折小刻みに震えているのがわかるでしょうか。このようにサーボモータが主に静止時に小刻みに震える現象を「ハンチング」と言います。
すぐさま深刻な問題を引き起こすというわけでもないのですが、身体全体が振動してしまい今後何らかの悪影響があるかもしれませんし、何より見た目がよくありません。
次回はこのハンチングを抑えて更に複雑なモーションを作成していきます。
