2005/C2

メインプログラム

int place;	//牛乳パックを置く場所を設定
int get=1;	//牛乳パックの数

//ライントレース
sub trace() {
	while ((SENSOR_1 > 48) || (SENSOR_3 > 48)) {
		if (SENSOR_1 < 48) {
			OnFwd(OUT_C);
			Off(OUT_A);
		} else if (SENSOR_3 < 48) {
			OnFwd(OUT_A);
			Off(OUT_C);
		} else {
 			OnFwd(OUT_A+OUT_C);
		}
	}
}

//牛乳パックをつかむ
sub getbox() {
	Wait(50);
	ClearTimer(0);
	while (Timer(0) <= 10) {
		SendMessage(51);
	}
	Wait(50);
}

//牛乳パックを置く
sub putbox() {
	Wait(50);
	ClearTimer(0);
	while (Timer(0) <= 10) {
		SendMessage(52);
	}
	Wait(50);
}

task main() {

//センサの設定
	SetSensor(SENSOR_1,SENSOR_LIGHT);	//ライントレース(左)
	SetSensor(SENSOR_2,SENSOR_LIGHT);	//牛乳パックの色判別
	SetSensor(SENSOR_3,SENSOR_LIGHT);	//ライントレース(右)

	while(get <= 6) {

//初めの牛乳パック
		if (get == 1) {
			getbox();

	//ライントレースへ
			OnRev(OUT_A+OUT_C);
			Wait(15);
			OnFwd(OUT_C);
			Wait(20);
			OnFwd(OUT_A);
			Wait(100);
			until(SENSOR_1 < 45);

//2個目の牛乳パック
		} else if (get == 2) {

	//つかむ位置合わせ
			OnRev(OUT_A+OUT_C);
			Wait(40);
			Off(OUT_A);
			OnFwd(OUT_C);
			Wait(40);
			OnFwd(OUT_A);
			Wait(55);
			Off(OUT_C);
			Wait(50);
			OnFwd(OUT_C);
			Wait(30);
			Off(OUT_A+OUT_C);

			getbox();

	//振り返り、そのままライントレースへ
			OnRev(OUT_A+OUT_C);
			Wait(5);
			OnFwd(OUT_A);
			Wait(70);
			until(SENSOR_1 < 45);

//3個目以降の牛乳パック
		} else if (get >= 3) {

	//奥から2番目の交差点まで進む
			Off(OUT_C);
			OnFwd(OUT_A);
			Wait(35);
			OnFwd(OUT_C);
			until(SENSOR_1 < 40);
			trace();
			OnFwd(OUT_A+OUT_C);
			Wait(20);
			trace();

	//3〜5個目の牛乳パック
			if (get <= 5) {
				OnFwd(OUT_A+OUT_C);
				Wait(20);
				Off(OUT_C);
				OnRev(OUT_A);
				Wait(90);
				OnFwd(OUT_A+OUT_C);

		//牛乳パックごとに位置を合わせ、牛乳パックをつかむ
				if (get == 3) {			//3個目
					Wait(15);
					Off(OUT_A+OUT_C);
					getbox();
				} else if (get == 4) {		//4個目
					Wait(50);
					Off(OUT_A+OUT_C);
					getbox();
					OnRev(OUT_A+OUT_C);
					Wait(30);
					Off(OUT_A+OUT_C);
				} else {			//5個目
					Wait(85);
					getbox();
					OnRev(OUT_A+OUT_C);
					Wait(60);
					Off(OUT_A+OUT_C);
				}

		//黒線上に戻る
				OnRev(OUT_A);
				until(SENSOR_3 < 45);
				OnRev(OUT_C);
				Wait(20);

	//6個目の牛乳パック
			} else {

		//一番奥の交差点まで進む
				OnFwd(OUT_A+OUT_C);
				Wait(20);
				trace();

		//位置を合わせ、牛乳パックをつかむ
				OnFwd(OUT_A+OUT_C);
				Wait(20);
				Off(OUT_C);
				OnRev(OUT_A);
				Wait(90);
				OnFwd(OUT_A+OUT_C);
				Wait(15);
				Off(OUT_A+OUT_C);
				getbox();

		//黒線上へ戻る
				OnRev(OUT_A);
				until(SENSOR_3 < 45);
				OnRev(OUT_C);
				Wait(20);

		//奥から2番目の交差点まで進む(越える)
				trace();
				OnFwd(OUT_A+OUT_C);
				Wait(20);
			}

	//ライントレースへ
			trace();
			OnFwd(OUT_A+OUT_C);
			Wait(20);
			trace();
			Off(OUT_C);
			OnFwd(OUT_A);
			Wait(25);
			OnFwd(OUT_C);
			until(SENSOR_1 < 45);
		}

//カゴ側へ移動
		trace();

//牛乳パックの色判別→入れるカゴを決定
		if (SENSOR_2 < 40) {		//黒
			place = 3;
		} else if (SENSOR_2 > 46) {	//白
			place = 2;
		} else {			//緑
			place = 1;
		}

//カゴ前へ移動
		repeat(place) {
			OnFwd(OUT_A+OUT_C);
			Wait(20);
			trace();
		}

//床上の牛乳パック→持ち上げてカゴに入れる
		if ((get >= 3) && (get <= 5)) {		//判別(3〜5個目の牛乳パック)
			OnFwd(OUT_A+OUT_C);
			Wait(30);
			Off(OUT_A+OUT_C);
			Wait(20);

	//車体を上げる
			OnFwd(OUT_B);
			Wait(100);
			Off(OUT_B);

			Wait(20);
			OnRev(OUT_C);
			Wait(100);
			OnFwd(OUT_A+OUT_C);
			Wait(40);
			Off(OUT_A+OUT_C);
			putbox();

	//牛乳パックを離した後、車体を下げる
			OnRev(OUT_B);
			Wait(80);
			Off(OUT_B);

			Wait(20);

//箱上の牛乳パック→そのままカゴに入れる
		} else {
			OnFwd(OUT_A+OUT_C);
			Wait(20);
			Off(OUT_A);
			OnRev(OUT_C);
			Wait(85);
			OnFwd(OUT_A+OUT_C);
			Wait(30);
			Off(OUT_A+OUT_C);
			putbox();
		}

//黒線上へ戻る
		OnRev(OUT_C);
		until(SENSOR_1 < 45);

		get++;		//次に取る牛乳パックの番号を設定

//牛乳パック側へ移動
		trace();
		repeat(place) {
			OnFwd(OUT_A+OUT_C);
			Wait(20);
			trace();
		}
	}
}

概要

下段のRCXに組み込んだメインのプログラム。ロボット自体はフィールド上の7個の牛乳パックを運べるよう作っているが、プログラムのほうはは調整が間に合わなかったため、とりあえず6個運ぶところまで書いたもの。下段のRCXには駆動用モータ等、アーム以外の全ての機器が集約されているため、それら全てを制御するこのプログラムは長い。

移動

移動は、牛乳パックをつかむ位置の微調整を除いて、全てライントレースを用いている。ロボット自体が比較的速く動くため、センサの反応がわずかでも、しっかり判定できるよう、黒線検出のセンサ値は高め(トレース時は48以下を黒線上と判定)に設定している。
また、交差点の通過時は、一度センサ判定を止め、0.2秒前進して横線を越えてから再びセンサによるライントレースに戻る方式を採っている。その度にいちいちライントレースの命令文を書くのは面倒なので、ライントレースはサブルーチン(trace())にまとめ、交差点まで進む命令になっている。

牛乳パックをつかむ

次につかむ牛乳パックが何個目であるかを変数getとし、判別する。初めは1で、牛乳パックを置いたときにプラス1。1つ目を入れたら2つ目を、2つ目を置いたら3つ目を取りに行くという寸法である。

牛乳パックを置く

アームの根本につけた光センサによって、変数placeの値を変化させる。この変数は交差点を越える回数で、「移動」の項で書いた命令をrepeat(place){}で繰り返させ、目的とするカゴの前まで移動する。
なお、変数getの値が3〜5のときは、つかんだ牛乳パックは床上に置いてあったものなので、カゴに入れる前に車体を上げ、持ち上げてからカゴの上に移動、カゴに入れるようにしている。

余談

実は発表時、「センサ2は光センサである」という宣言を書き忘れていた。でも動いたということは、なくてもかまわない?


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