1 .２足歩行ロボットの開発１
ＴＯＰ
3.ＲＣカーの開発

2.２足歩行ロボットの開発２

先ほどと同様に、また違った形の２足歩行ロボットを作成してみましょう。

1.ロボットの仕様

今回作成する2足歩行ロボットは以下のようなものとします。

2.ハードウェアの作成

ロボット本体は『LEGO MINDSTORMSをプラットフォームとするプログラム開発環境の教材化』に記載のものを参照してください。


3.プログラムの解説

ロボットを動かすプログラムを解説していきましょう。
ここでは作成ファイル名をbiped_walker2.cとします。
作成したプログラムbiped_walker2.c

3-1.メイン関数


#define LEG 300 // LEG MOVE TIME
#define ANKLE 1200 // ANKEL MOVE TIME
int main(int argc,char **argv){
	t_stop=execi(&stop,0,NULL,100,DEFAULT_STACK_SIZE); // make stop task
	t_walk=execi(&walk,0,NULL,90,DEFAULT_STACK_SIZE); // make walk task
	t_eye=execi(&eye,0,NULL,10,DEFAULT_STACK_SIZE); // make eye task
	return 0;
}
この関数では各タスクを呼び出しています。各タスクの優先度は以下の通り。
関数名 優先度
stop(ストップ関数) 100
walk(ウォーク関数) 90
eye(アイ関数) 10

3-2.ストップ関数

int stop(){ // rcx stop function
	int stopchar;
	stopchar=getchar();
	if(stopchar==KEY_RUN){
		cputs("stop");
		killall(100);
	}
	return 0;
}
　この関数はプログラム自体をストップさせるという動作をします。なぜこの関数が必要なのかというと、他のウォーク関数(walk)やアイ関数(eye)はwhile(1)の無限ループで、なおかつマルチタスクであるので、普通にRunボタンを押しただけではmain関数は止まっても他のタスクは生きていて、結局プログラムは止まってくれません。そのためプログラム内にRunボタン検知を設けて、Runボタンが押された時にすべてのタスクを強制終了させることが必要になるのです。なお強制終了の命令文は『killall(100)』(100とは優先度を指し、これ以下のタスクはすべて消滅する)です。

3-3.ウォーク関数

int walk(){ // Walk function
	motor_a_speed(MAX_SPEED);
	motor_b_speed(MAX_SPEED);
	
	motor_b_dir(rev);
	msleep(1500);
	motor_b_dir(brake);
	while(1){
		leg_move_right(); // right leg move function
		ankle_move_right(); // right ankle move function
		leg_move_left(); // left leg move function
		ankle_move_left(); // left ankle move function
	}
	return 0;
}
この関数はロボットを歩行させる関数で、最初の一歩を踏み出す準備をし、その後、４つの関数を連続して呼び出すことにより歩行させています。モーターポートＢに接続されているモーターが足首関節を動かします。

3-4.右・左足駆動関数

void leg_move_right(){ // right leg move function
	motor_a_dir(fwd);
	msleep(LEG);
	motor_b_dir(rev);
	msleep(ANKLE);
	motor_b_dir(brake);
	wait_event(&ts1_wakeup,0);
	motor_a_dir(brake);
}

void leg_move_left(){ // left leg move function
	motor_a_dir(rev);
	msleep(LEG);
	motor_b_dir(fwd);
	msleep(ANKLE);
	motor_b_dir(brake);
	wait_event(&ts1_wakeup,0);
	motor_a_dir(brake);
}

wakeup_t ts1_wakeup(wakeup_t data){
	return SENSOR_1<0xf000;
}
leg_move_right関数のフローチャート
　これらの関数は両方とも足全体を動かす腰関節を制御する動作をします。モーターポートＡに接続されているモーターが腰関節を動かします。また、センサーポート１に接続されているタッチセンサーは腰関節の角度検出用に使われています。

3-5.右・左足首駆動関数

void ankle_move_right(){ // right ankle move function
	motor_b_dir(fwd);
	msleep(1500);
	wait_event(&ts3_wakeup,0);
	dsound_system(DSOUND_BEEP);
	msleep(1500);
	motor_b_dir(brake);
}

void ankle_move_left(){ // left ankle move function
	motor_b_dir(rev);
	msleep(1500);
	wait_event(&ts2_wakeup,0);
	dsound_system(DSOUND_BEEP);
	msleep(1500);
	motor_b_dir(brake);
}

wakeup_t ts2_wakeup(wakeup_t data){
	return SENSOR_2>0xf000;
}

wakeup_t ts3_wakeup(wakeup_t data){
	return SENSOR_3>0xf000;
}
leg_move_right関数のフローチャート
　これらの関数は両方とも足首を制御する動作をします。センサーポート１、２に接続されているタッチセンサーは足の接地検出用に使われています。

3-6.アイ関数

#define EYE_MOVE 200 // EYE MOVE TIME

int eye(){ // Move eye function
	motor_c_speed(MAX_SPEED);

	while(1){
		motor_c_dir(fwd);
		msleep(EYE_MOVE);
		motor_c_dir(brake);
		msleep(EYE_MOVE);

		motor_c_dir(rev);
		msleep(EYE_MOVE);
		motor_c_dir(brake);
		msleep(EYE_MOVE);

		motor_c_dir(rev);
		msleep(EYE_MOVE);
		motor_c_dir(brake);
		msleep(EYE_MOVE);

		motor_c_dir(fwd);
		msleep(EYE_MOVE);
		motor_c_dir(brake);
		msleep(EYE_MOVE);
	}
	return 0;
}
パフォーマンス用に用意した関数で、マイクロモーターにより、ロボットの目を動かします。

4.プログラムの実行

　ではこれらのプログラムをコンパイル、ダウンロードしてください。それではいよいよ動かしてみましょう。うまく動くでしょうか？
動いているムービーを乗せておきます。参考にして下さい。

撮影したムービーbiped_walker2.avi(AVI形式)

4.まとめ

　この章ではより高度な２足歩行ロボットを作成しました。大掛かりなロボットになりましたが、プログラムは今まで学んできたことばかりです。ＬＥＧＯブロックで作られた２足歩行ロボットはインターネット上に多々あります。一度探してみてください。
1 .２足歩行ロボットの開発１
ＴＯＰ
3.ＲＣカーの開発