GyroBoy（ジャイロボーイ）　～レゴマインドストームEV3～

ジャイロボーイ

教育用レゴマインドストームEV3でジャイロボーイを作った時の備忘録。

ジャイロボーイはジャイロセンサーでバランスを取りながら２輪で直立しながら前進、後退、その場で回転等ができるロボット。

組立説明書とサンプルプログラムが付属しているので本体さえ説明書通りに組み立てることが出来れば作ることができる。

ジャイロセンサーを使って２輪で直立する仕組みが知りたくてプログラムを自分なりに解析してみた。

何故、直立できるのかは正直良くわからなかったが自分なりに分かった部分に関して記事に残しておく事にした。

基本的な動作

基本的な動きは、

1. 二輪でバランスを取りながら自走
2. 右腕のカラーセンサーで色を読み取る
3. 赤なら停止
4. 緑で前進
5. 青でその場で時計回り
6. 黄でその場で反時計回り
7. 白で後退
8. 左腕の超音波センサーで２５ｃｍ以内に物体を検知すると若干後退をしながら両腕を振る

となっている

動画

まずは動いている様子は以下の動画を見てほしい。

直立した後、黄色を読み込ませてその場で反時計回りにゆっくりと回転している。

組立説明書

教育用レゴマインドストームEV3はOSに合ったソフトウェアをこちらのサイトからダウンロードしてインストールする。

ソフトウェアを起動して組立説明書ー＞コアセットモデルー＞ジャイロボーイを選択する。

１／３ページはジャイロボーイの紹介ビデオページとなっている。

右三角で次のページが表示される。

２／３ページは組立説明書。

ページ数は１１９ページだが指定した設計図の通りに組み立てれば組み立てることができる。

３／３ページはプログラムとなっている。

今回、このプログラムを解析してみた。

プログラム

ジャイロボーイのプログラムは以下の通り。

main

処理群１と処理群２に分かれており、２つの処理群が同時並行で実行される。

処理群１はジャイロボーイの直立に関するロジック、処理群２は前進・後退、その場での回転などに関するロジックになっている。

処理群１

ジャイロボーイの直立に関するロジック。

その１

1. 初期処理と顔の表情に関する処理
2. バランス用ループ（BAL）の前半部分（GT～GG）処理詳細は後述する。GTでループの時間に関する処理、GGでジャイロセンサーに関する計算処理を行っている。

その２

マイブロックGM～Chkの処理詳細は後述する。

ざっくりいうとGMでモーターに関する計算計算、EQでジャイロセンサーとモーターの値からベースとなるパワー（出力）の計算処理、Ctrlで左右のパワーの分配計算を行っている。

計算ブロック（0.005-(a-b))のa-bはループのGT直後から直前までの時間を表している。

よって0.005-(a-b)秒待機する事で１ループ（BAL)が0.005秒前後でほぼ同じ時間になるように調整している。

またOKがTrueになったらループを終了するのだが、Trueになる条件はpwr（パワー）１００が１秒以上継続することなので転倒などでどちらか一方にパワーが偏ってしまった時にループが終了する様になっている。

その３

モーターを止めてタッチセンサーが押されるまで待機する。

タッチセンサーが押されたらループを継続する。

処理群２

処理群１と同時並行で実行される。

カラーセンサーで読み取った色別の処理と超音波センサーで２５ｃｍ以内の障害物を検知した時の処理が記述されている。

• Cdrvはドライブ値を表しておりプラスの数値で前進、マイナスの数値をセットすると後退する。
• Cstrはステアリング値を表しておりプラスの数値をセットすると時計回り、マイナスの値をセットすると反時計回りにジャイロボーイがその場で回転する。
• Cdrv、Cstrの値はEQ及びCtrlで左右のパワーの補正に利用している

RST

初期処理。

モーターやジャイロセンサー、タイマー、各種変数をリセットしている。

gOS

静止状態でのジャイロセンサーの平均角速度（gOS）を算出している。

尚、gOSはGyroSensor OffSet（ジャイロセンサーの補正値）の略と思われる。

その１

その２

その３

変数gMnはジャイロセンサーの角速度の最小値、変数gMxは最大値を算出する為の変数。

初期値に変数gMnは十分に大きい数値、g変数Mxには十分に小さい数値をセットしてその値を上回る値の時は変数を書き換えるやり方で最大値、最小値を算出している。

最大値ー最小値が２以下（ほぼ停止している状態）になるまでOSLループを繰り返す。

OSLループの中で更にgChkループを２００回繰り返している。

変数gOSはgChkループでのジャイロセンサーの角速度の平均値（合算して２００で割っている）。

この処理（gOS）では変数gOSはほぼゼロになるが、この後のGGにて変数gOSは再計算される。

GT

１ループ辺りの平均処理時間（tInt）の算出処理。

cLoは初回はゼロがセットされているの上のフローが実行されて変数tIntには0.014（秒）がセットされる。

２回目以降はTimer1をcLoで割ってMain処理のBALループの平均処理時間（１ループ辺りの時間）を算出して変数tIntにセットしている。

GG

ジャイロセンサーの角速度、スピード、角度に関する値を計算している

ジャイロセンサー関連の３つの変数を計算している。

この３つの変数とモーターの回転角度を使って後続のEQにてモーターのパワーを算出しているのだが正直「やっている事は分かった」のだが「これで何故直立するのか」は分かっていない。

ジャイロボーイが前に倒れそう（ジャイロセンサーの値がプラス）なら前進、後ろに倒れそう（ジャイロセンサー値がマイナス）なら後退させることによって直立を保とうとしている理屈は分かるのだが、現時点ではそれぞれの変数の意味までは理解出来なかった。

GM

モーターの回転角度に関する値を計算している。

その１

その２

モーターの回転角度に関する２つの変数を計算している。

EQ

ジャイロセンサーの値とモーターの値からパワー（pwr）の算出を行っている。

その１

その２

以下の計算式で変数ｍPos（GMで計算した通算の回転角度）を補正計算すると共に変数pwrを計算している。

Ctrl

上記EQで計算した変数pwrをベースにドライブ値（Cdrv）とステアリング値（Cstr）で左右のパワーを調整して計算する。

以下の計算式で左右（A、Dドライブ）のモーターのパワーを補正する。

尚、計算後のパワーが１００を上回る、またはー１００を下回る場合はそれぞれ１００、ー１００を上限、下限にする。

Chk

Mainの処理群１のBALループを終了する為の条件を設定する。

変数pwrの絶対値が２秒以上連続して１００の場合は転倒など何らかの異常とみなして変数okをTrueにする。

それによりBALループが終了する。

動画

この記事の内容を動画で説明している。