2015b/Member 目次

ライントレースするロボット

黒い線に沿って動くロボットを制作した。

走行する経路

716*500, 経路

上の図を、「B地点 → P右折 → Q右折 → P左折 → Q左折 → Q直進 → P直進 → A地点」の順に走行する

ロボット本体

初期ロボットに光センサを付けた構造

 480*360

モータBは右のタイヤに、モータCは左のタイヤに取り付けた。

定義、サブルーチン

マクロ

#define THRESHOLD 50		//閾値
#define STEP 1		//Waitの引数として使う

引数を変数に代入する「scan_move」「scan_angle」

scan_moveで使うための変数

変数target...交差点かどうかを判別するための数値

変数speed3...ロボットが直進するときの、モータのパワー

int target;	//交差点かどうかを判断するとき、基準とする数値
int speed3;	//直進するときの、モータのパワーに使用

引数でとった値を、target, speed3に代入する「scan_move」

sub scan_move(int aim, int power) {	//引数にとった値を、target, speed3に代入する
	target = aim;					//引数aimを、targetに代入
	speed3 = power;				//引数powerを、speed3に代入
}

scan_angleで使うための変数

次の3つの変数は、交差点にぶつかった後、どんな動作をするのかを決める。

int R_ang;	//交差点で右旋回するときの、モータの回転角
int L_ang;	//交差点で左旋回するときの、モータの回転角
int GO_ang;	//交差点で直進するときの、モータの回転角

引数でとった値を、L_ang, GO_ang, R_angに代入する「scan_angle」

sub scan_angle(int l, int g, int r) {	//引数にとった値を、L_ang, GO_ang, R_angに代入
	L_ang = l;					//引数lを、L_angに代入
	GO_ang = g;					//引数gを、GO_angに代入
	R_ang = r;					//引数rを、R_angに代入
}

左旋回、右旋回、直進をする「turn_left」「turn_right」「go_forward」

モータのパワーを引数にとって、左旋回する「turn_left」

sub turn_left(int speedA, int speedB) {	//左に曲がる
	OnFwd(OUT_B,speedA); 			//引数speedAのパワーで、モータBを前転
	OnRev(OUT_C,speedB);				//引数speedBのパワーで、モータCを後転
}

モータのパワーを引数にとって、右旋回する「turn_right」

sub turn_right(int speedA, int speedB) {	//右に曲がる
	OnFwd(OUT_C,speedB); 			//引数speedBのパワーで、モータCを前転
	OnRev(OUT_B,speedA);				//引数speedAのパワーで、モータBを後転
}

モータのパワーを引数にとって、直進する「go_forward」

sub go_forward(int speed3) {		//まっすぐ進む
	OnFwd(OUT_BC,speed3);			//speed3のパワーで、モータBCを前転
}

交差点にぶつかったときの動作「cross_action」

交差点にぶつかった際、1秒間停止したあと、その交差点を直進する or 右折する or 左折する

「cross_action」は、「scan_ang」で値が代入された「L_ang」「R_ang」「GO_ang」、それに加えてモータのパワーとして「speed1」を引数にとる

sub cross_action(int speed1, int L_ang, int R_ang, int GO_ang) { //交差点に入った時の動作(countが2と7のとき以外)
	Off(OUT_BC);								 //モーター停止
	Wait(1000);								 //1秒間−
	RotateMotorEx(OUT_BC, speed1, L_ang, -100, true, true);		 //speed1のパワーで、L_angの角度分、その場左旋回−
	Wait(100);								 //間にWaitを挟まないと誤作動するため
	RotateMotorEx(OUT_BC, speed1, R_ang, 100, true, true);		 //speed1のパワーで、R_angの角度分、その場右旋回−
	Wait(100);								 //誤作動を防ぐため
	RotateMotorEx(OUT_BC, speed1, GO_ang, 0, true, true);		 //speed1のパワーで、GO_angの角度分、直進−
}

上のプログラムの動作を説明すると

1秒間ロボットを停止させる

▲癲璽燭L_ang度回転させて、その場左旋回

モータをR_ang度回転させて、その場右旋回

ぅ癲璽燭GO_ang度回転させて、直進

の順に実行される

「モータが0度動く = 動かない」と考えると

・R_ang = 0, GO_ang = 0のとき、交差点を左折する(△実行される)

・L_ang = 0, R_ang = 0のとき、交差点を直進する(い実行される)

・L_ang = 0のとき、交差点を右折する(→い僚腓納孫圓気譴)

メイン関数

task main()
{

メイン関数で新しく使う変数

変数count...交差点にぶつかった回数を代入

変数tacho...モータBの回転数を蓄積する

	SetSensorLight(S2);
	int speed1 = 20;  		//モータのパワー用
	int speed2 = 15;  		//モータのパワー用(speed1より少し弱い)
	int count = 0;    		//何回交差点にぶつかったかを代入する
	int tacho = 0;    		//「その場で左旋回する」動作をし続けたときの、回転角の合計
	long t;	    		//タイマーを使うときに使用

count < 8 の間だけ繰り返す

	while (count < 8){		//count < 8 の間、ループを続ける

交差点ごとに動作を変える

ここでの操作は、「交差点の判別」や「交差点にぶつかった際の動作」に影響を及ぼす

		switch(count) {  	 //countの値によって動作が変わる
            //例:case countの値 : scan_move(targetの値,speed3の値); scan_angle(左旋回する角度,直進する角度,右旋回する角度);
		 case 0  : scan_move(20, 50); scan_angle(0, 70, 50); break;
		 case 1  : scan_move(28, 60); scan_angle(0, 80, 20); break;
		 case 2  : scan_move(10000, 40); break;
		 case 3  : scan_move(20, 45); scan_angle(40, 0, 0); break;
		 case 4  : scan_move(30, 50); scan_angle(160, 0, 0); break; 
		 case 5  : scan_move(20, 50); scan_angle(0, 50, 0); break;
		 case 6  : scan_move(20, 60); break;
		 case 7  : scan_move(25, 40); scan_angle(0, 0, 0); break;
		 default : break;
		} 		

countの値から、今何個目の交差点にいるのかを判断

例えば、countが0のときは、「1個目の交差点までの、ロボットのスピード」と、「1個目の交差点の判別基準」、「1個目の交差点にぶつかったときの動作」を設定する 480*335 480*335

上図より、橙(count=0)→黄(count=1)→黄緑(count=2)→緑(count=3)→水色(count=4)→青(count=5)→紫(count=6)→赤(count=7)の順に進む

count=2の時だけは、交差点に到達する前にcountが切り替わる。

黒い線に沿って動くためのプログラム

               t = CurrentTick();	//ここで現在時刻をtに代入(CurrentTick() - t と使うことで、経過時間がわかる)		
		while ((tacho < target) && ((count != 2) || (CurrentTick() - t < 22500))) {	//「ライントレースするループ」
		/*「target(指定した角度)をタイヤの回転数が超えない」 または 「count=2のときに、22.5秒経過していない」
		   ↑の条件を満たしている間だけループを続ける*/

		//つまり、count != 2の時は、tacho >= targetを満たすときにループを抜け出す
                         count = 2の時は、22.5秒経過したときにループを抜け出す
		     	if (SENSOR_2 < THRESHOLD -15){  		//光センサの数値が、THRESHOLD-15より低いとき                       		
				turn_left(speed1, speed1); 			//左右のモータをspeed1で左旋回(その場で左回転)
							
			} else if (SENSOR_2 < THRESHOLD -7) {	//光センサの数値が、THRESHOLD-7より低いとき						
				tacho = 0;					//tacho(turn_left1し続けた時の、タイヤの回転角の合計)を0にする−
				turn_left(speed1, speed2);			//右モータをspeed1、左モータをspeed2(speed1より少し弱い)にして、左旋回
	
			} else if (SENSOR_2 < THRESHOLD +7){	//光センサの数値が、THRESHOLD+7より低いとき
				tacho = 0;					//tachoを0にする−
				go_forward(speed3);				//左右のモータをspeed3にして、直進

			} else if (SENSOR_2 < THRESHOLD +15){	//光センサの数値が、THRESHOLD+15より低いとき
				tacho = 0;					//tachoを0にする−
				turn_right(speed1, speed2);			//右モータをspeed2、左モータをspeed1にして、右旋回
		
			} else {					//光センサの数値が、THRESHOLD+15より高いとき
				tacho = 0;					//tachoを0にする−
				turn_right(speed1, speed1);			//左右のモータをspeed1で右旋回(その場で右回転)
				
			}		
			Wait(STEP);					//STEP(0.001)秒だけ動く
			if (SENSOR_2 >= THRESHOLD-15) {		//光センサの数値が、THRESHOLD-15より高いとき (=その場で左回転していないとき)
				ResetTachoCount(OUT_B);			//モータの回転数をリセット(その場で左回転したとき以外はリセット)−
			} 
			tacho += MotorTachoCount(OUT_B);		//tachoに、モータの回転角を代入(「その場で左回転した時」の回転角しか代入されない)−
		}							

tachoは、「モータBが回転した角度」を蓄積するための変数。tachoについて

 SENSOR_2 < THRESHOLD(閾値) - 15」以外のとき、tachoを0にする。

◆SENSOR_2 < THRESHOLD(閾値) - 15」以外のとき、TachoCountB(モータBが回転した角度)を0にリセットする

tacho += MotorTachoCount(OUT_B) は、「モータBが回転した角度」をtachoに追加していることを表している

の,,が成り立つ

◆↓より、tachoに0以外の値が代入されるのは、「SENSOR_2 < THRESHOLD - 15」を満たすとき(ロボットがその場で左旋回するとき)だけである

,茲蝓一度でも「SENSOR_2 < THRESHOLD - 15」を満たさなかったら、tachoは0にリセットされる。

つまり、tacho > targetとなるのは、連続的に「SENSOR_2 < THRESHOLD - 15」を満たし続けている(その場左旋回が、連続して起こっている)ときだけ

交差点にぶつかった際の動作

		//↓↓以下は「ライントレースするループ」を抜けだした時の動作↓↓
		if ((count != 2) && (count != 7)) {	//countの値が、2or7以外のとき
			cross_action(speed1, L_ang, R_ang, GO_ang);  //サブルーチンcross_action(交差点の際の動作)を実行
		}
 		count++;	//countに1を加える	
		tacho = 0;	//tachoを0にする
		ResetTachoCount(OUT_B);	//モータの回転角をリセット(交差点の動作の時の、モータの回転角をリセット)	
	}

すべてのループが終わった後の動作

	//↓↓以下は、すべてのループが終わった時の動作↓↓	
	Off(OUT_BC);  //ロボットを停止させる
	
}

メイン関数のまとめ

プログラムを一つ一つ説明したため、プログラムの全体像がつかめなくなってしまった。なので、whileループや、変数の代入関係を中心にまとめたものを載せる

/*task main()
{ 	
	count = 0;	//交差点にぶつかった回数を代入する
	tacho = 0;	//モータBの回転角の合計
	long t;
	while (count < 8){		//count < 8 の間、ループを続ける
		switch(count) {  	 //countの値によって動作を変える
		 /*countごとで、target(交差点の判断基準)
			         speed3(直進時のパワー)
			         L_ang, R_ang, GO_ang(交差点に当たった際の動作)
		                 を決める*/                 
		}
		while ((tacho < target) && ((count != 2) || (CurrentTick() - t < 22500))) {	//「ライントレースするループ」
			//光センサを使って、実際にライントレース
               	    //(SENSOR_2 >= THRESHOLD-15)のときは、tacho = 0
			//モータBが動いたとき、その回転数が「MotorTachoCount(OUT_B)」に代入される
		 	if (SENSOR_2 >= THRESHOLD-15) {		//光センサの数値が、THRESHOLD-15より高いとき (=その場で左回転していないとき)
				ResetTachoCount(OUT_B);			//モータの回転数をリセット(その場で左回転したとき以外はリセット)
			} 
			tacho += MotorTachoCount(OUT_B);		//tachoに、モータの回転数を代入(「その場で左回転した時」の回転角しか代入されない)

		}							
		//↓↓以下は「ライントレースするループ」を抜けだした時の動作↓↓
		if ((count != 2) && (count != 7)) {	//countの値が、2or7以外のとき
			cross_action(speed1, L_ang, R_ang, GO_ang);  //サブルーチンcross_action(交差点の際の動作)を実行
		}
		
		count++;	//countに1を加える	
		tacho = 0;	//tachoを0にする
		ResetTachoCount(OUT_B);	//モータの回転角をリセット(交差点の動作の時の、モータの回転角をリセット)	
	}
	//↓↓以下は、すべてのループが終わった時の動作↓↓	
	Off(OUT_BC);
	
} */

工夫点

・授業で習ったプログラムと基本的に同じだが、交差点を判別するとき、タイマーではなくモータの回転数を使った。そのおかげで、交差点ごとにスピードを変えられるようになった。そして、NXTの電池残量によって動きが変わることも少なかった。

・右旋回、左旋回、直進の組み合わせで、交差点での進み方(右折、左折、真っ直ぐ、など)を決めたこと

反省点

・条件分岐や、サブルーチンを無駄に多く使っている(case文で書かれている箇所は特に)

・TachoCountを信用しすぎていた。誤差がかなりあったので、タイマーを使った方が正確だったかもしれない。


添付ファイル: fileIMG_1320.JPG 102件 [詳細] file2015b-mission2c.png 103件 [詳細] file2015b-mission2a.png 125件 [詳細] file2015b-mission2.png 120件 [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2016-02-11 (木) 13:44:34