1．セグウェイ風ロボットの作成 TOPへ 3．リモコンロボットの作成
2．ホッケーロボットの作成


ここでは応用として、LEGO MINDSTORMS NXT付属の組立て図に記載されているホッケーロボットのプログラミングを行いたいと思います。


1．ロボットの仕様

これから作るロボットの仕様は次の通りです。


2．ロボットの作成

付属の組立て図p.50〜67に記載のスティックアームとターゲットを作成してください。 また今回用いる超音波センサは、この教材の基礎編で扱ったものではなく、組立て図p.28〜30に記載されているものを用います。
hockey1.png(262067 byte)
hockey2.png(65811 byte) hockey3.png(54909 byte)

アームのモータをポートＡ、右のモータをポートＢ、左のモータをポートＣに接続してください。 また、超音波センサをポート４に入力してください。



3．プログラムの作成・解説

それでは、このホッケーロボットのプログラムを作成します。ここでは、hockey.cとします。 （ソースファイルのダウンロード）

まず、今回のような大規模なプログラムを作成する場合は、プログラムの大まかな流れを考えます。 今回は探索〜ボールを打つ動作を行うまでを次のような流れで行うものとします。
  1. センサや変数の初期化する
  2. その場でターンして、ボールの探索する
  3. ボールに対しホッケーロボットを正対させる
  4. ボールに近づく
  5. スティックを動かし、ボールを打つ
この手順に則ってプログラムを作っていきます。また、初期化を除いた動作を関数で記述するとします。


１．メインプログラムとマクロ

ここでは諸定数の定義と「１．初期化」、各関数の呼び出しを含めた一連の動作を行います。
#include "nxtlib.h"

#define S_SONAR S4

#define TURN_POWER 20	// 探索時のモータのパワー
#define CURR_POWER 18	// 正対時のモータのパワー

#define TIRE_RAD 26	// タイヤの半径[mm]
#define BOT_DIAM 116	// 左右のタイヤ間の距離[mm]
#define MAX_DIST 50	// 測定可能距離[cm]
#define TARGET_DIST 23	// 打つときの間隔[cm]
#define CORRE_ANGLE 40	// 正対時の探索角度[deg]
#define CHECK_TIME 10	// 超音波での調査時間間隔 [msec]

// 各関数のプロトタイプ宣言
void ball_search();		// ボール探索
int ball_corre();		// ポール正対
void move_straight(int distance, int power);	// 直進動作
void move_stick();		// アームスティックの作動

task main(){
	int length;
	SetSensorSonar(S_SONAR);

	ball_search();
	length = ball_corre() - TARGET_DIST;
	length *= 10;	// 単位をmmに変換する
	wait10Msec(10);
	move_straight(length,50);
	wait10Msec(10);
	move_stick();
}

諸定義はコメント文の通りの定義です。
TARGET_DISTは、今回のロボットでは超音波センサがロボット後方に取り付けられているため、ボールを打つ際に超音波センサとターゲットとの間に生じる間隔をあらわしています。
また、ここでは同時に関数をプロトタイプ宣言しておきました。これらの詳細については後に述べます。

mainタスクでは、「変数やセンサの初期化」等の一連の流れを実行します。 これは先ほど考えたプログラムの流れと（単位変換等の諸計算はありますが）ほぼ同じように作られているのが分かると思います。


２．ボールの探索：ball_search
void ball_search(){
	motor[motorB] = TURN_POWER;
	motor[motorC] = -TURN_POWER;
	while(SensorValue(S_SONAR) > (MAX_DIST + TARGET_DIST)){
		wait1Msec(CHECK_TIME);
	}
	motor[motorB] = 0;
	motor[motorC] = 0;
}

この関数は、基礎編の超音波センサの項目で使用したサンプルを改造しただけです。 あちらは直進でしたが、こちらはその場回転となっただけです。回転しながらある距離以下になった場合、そこで停止をする。
これが「ボールの探索」を行う関数の内容です。

ただし、超音波センサによる探索時間の間隔を考えなければなりません。 超音波センサは音波を飛ばして測定するためある程度の時間を要し、またその処理も若干時間が掛かります。
そのため、測定したい的までの距離から大体の測定時間を計算します。 加えてロボットの１回転に掛かる時間からどれくらいの角度の精度が欲しいのか、という２つから探索の時間間隔を決めます。

３．ボールとの正対：ball_corre 
task corre_circle(){
	int target_rotate;

	while(true){
		nMotorEncoder[motorB] = 0;
		target_rotate = CORRE_ANGLE * (BOT_DIAM/2) / TIRE_RAD;
		motor[motorB] = CURR_POWER;
		motor[motorC] = -CURR_POWER;
		while(nMotorEncoder[motorB] < target_rotate){
			wait1Msec(1);
		}

		nMotorEncoder[motorB] = 0;
		target_rotate *= -1;
		motor[motorB] = -CURR_POWER;
		motor[motorC] = CURR_POWER;
		while(nMotorEncoder[motorB] > target_rotate){
			wait1Msec(1);
		}
	}
}

int ball_corre(){
	int pre_dist,cur_dist,lat_dist;
	pre_dist = SensorValue(S_SONAR);
	StartTask(corre_circle);
	wait1Msec(CHECK_TIME);
	cur_dist = SensorValue(S_SONAR);
	wait1Msec(CHECK_TIME);

	while(true){
		lat_dist = SensorValue(S_SONAR);
		if(pre_dist<=cur_dist && cur_dist<lat_dist && lat_dist<(MAX_DIST+TARGET_DIST)){
			motor[motorB] = 0;
			motor[motorC] = 0;
			StopTask(corre_circle);
			break;
		}
		pre_dist = cur_dist;
		cur_dist = lat_dist;
		wait1Msec(CHECK_TIME);
	}

	cur_dist = SensorValue(S_SONAR);
	return cur_dist;
}

正対動作とは「的（ボール）とロボットがちょうど真っ直ぐになるように調整するロボットの向きを調整すること」です。 今回はマクロで定義された角度の範囲内で的までの距離の最小値を探し、その最小値のときの向きで停止する動作を行います。
このとき、指定した角度だけ回転する動作と超音波で探索する動作の２つを同時に行わなければならないため、今回はマルチタスクのプログラムとしました。 回転するタスクを「corre_circle」とし、超音波による探索をメインのタスクで行うようになっていますが、シングルタスクで実現する方法もあります。


まず、回転するタスク：corre_circle を見ていきましょう。
このタスクはマクロのCORRE_ANGLEの角度だけ回転します。この際、回転角を知るためにロータリーエンコーダを用いています。


次に、メインタスク：ball_corre を見ていきます。
この関数では、一定時間(CHECK_TIME)毎に超音波センサを用いて的までの距離が最小になる向きを調べます。 最小値の条件としては、次の条件を全て満たす場合を最小値とします。 この条件を満たしたとき、左右のモータと回転するタスク：corre_circle を停止させ、その向きで測定した距離を戻り値とします。



４．直進動作：move_straight 
void move_straight(int distance, int power){
	int rotary;

	nMotorEncoder[motorB] = 0;
	nMotorEncoder[motorC] = 0;
	nSyncedMotors = synchBC;
	nSyncedTurnRatio = 100;

	rotary = (distance / TIRE_RAD) * (180/PI);
	nMotorEncoderTarget[motorB] = rotary;

	motor[motorB] = power;
	while(nMotorEncoder[motorB] < rotary){
		wait1Msec(1);
	}
}

距離とパワーを引数としてその距離分だけ直進します。この直進はモータの同調を用いた直進なので、正確に直進することが出来ます。


４．ボールを打つ：move_stick 
void move_stick(){
	motor[motorA] = 100;
	wait1Msec(300);
}

アームスティックが接続されたモータを300msec回します。これにより、アームが回転しボールを打つことが出来ます。




4．プログラムの実行

このプログラムをコンパイル、ダウンロードしてください。

それではいよいよ動かしてみましょう。プログラムを動かす前にロボットのアームと的の位置を調整します。 次の写真のように、ロボットのアームを上げておきます。
hockey4.jpg(44627 byte)

また、次の図のようにロボットの中心を向くように的を垂直に配置します。
hockey5.png(27830 byte)
これで準備は完了です。プログラムを動かしてみましょう。

実際に動いているムービーを掲載しておきます。参考にして下さい。

保存する場合はコチラ（avi形式：3.72MB）


5．更なる応用へ

このロボットでは、ターゲットの向きに指定があったり、測定可能距離に限界があるなどの様々な制約があります。 これらを解決して、いかなる場所にどんな向きであってもボールを打つことが出来るように改造するのもいいでしょう。
また、探索や正対の精度もそこまで高くは無いので、このアルゴリズムを変更することも十分に有効と言えます。
さらに単なる探索だけでなく、付属の組立て図の表紙のようにライトセンサを用いて、ボールの色を識別するなど十分に改良の余地があります。


1．セグウェイ風ロボットの作成 TOPへ 3．リモコンロボットの作成