2011a/A1/課題２右

2011a

目次†

目次
課題の内容
メンバー紹介
　ロボットの紹介
　図
　プログラム
　反省＆まとめ
- 前に行く
- 曲がる
- ロタリ
- 近道
- 到着点
- 光
- 音
　コメントをお願いします

↑

課題の内容†

次のような周回コース(黒い線)を動くロボットを作成せよ。

通常の交差点が2箇所以上ある
ロータリー型の交差点が1箇所以上ある (ロータリーでは反時計まわりに進む)
直角に曲がるコーナーおよびそのコーナーをショートカットするコース(点線)が存在する
急なヘアピンカーブが1箇所以上ある
スタート地点に目印がある
黒い線の幅は15～20mm程度センサー、モーターはいくつ使ってもよい。プログラムは次の2つを作成すること。

　1.直角のコーナーを通る

　2.直角のコーナーを通らない(近道する)

ただし互いに逆方向に周回すること。

↑

メンバー紹介†

ホン
オ

↑

　ロボットの紹介†

ロボットはうまく曲がれるように本体を出来れば出来るほど小さく作りました。

センサーが一つだけなら線で外れた時また線を探しにくいので二つのセンサーを付きました。

それに、センサーが前で早く道を認識するように本体の前のほうに付着しました。

↑

　図†

↑

　プログラム†

ホン

#define THERSHOLD 44
#define go_straight(t) OnFwd(OUT_A+OUT_C);Wait(t);
#define turn_left(t) OnRev(OUT_A);OnFwd(OUT_C);Wait(t);
#define turn_right(t) OnFwd(OUT_A);OnRev(OUT_C);Wait(t);
#define go_back(t) OnRev(OUT_A+OUT_C);Wait(t);
#define off Off(OUT_A+OUT_C);
int kousa=0; // ifを使った交差点通行。センサー両方が暗い線に当たるとこの変数は　1になって道を探し始まる。
int sen=0;　//　センサー２つを使って線を探す変数、
int saigo=0;　//この変数は0が普通の状態で1になると最後の線に当たることを意味してとまる。
task main()
{
	SetSensor(SENSOR_1,SENSOR_LIGHT);
	SetSensor(SENSOR_3,SENSOR_LIGHT);
     ClearTimer(1);
	while(true)
	{       
               start go_line;
		until((kousa == 1) || (Timer(1) == 167) || (saigo == 1));
		stop go_line;
		if (saigo == 1)
		{
			break;
		}			
		start search;
		until((SENSOR_1 <= THERSHOLD) || (SENSOR_3 <= THERSHOLD) ||     (Timer(0)>=15));
		stop search;
		if((SENSOR_1 <= THERSHOLD) || (SENSOR_3 <= THERSHOLD))
		{
			kousa--;
		}
		else if((Timer(0)>=15))
		{
			if(sen<1){
				kousa--;
				go_back(40);
				turn_left(0);
				until(SENSOR_1 <= THERSHOLD);
				off;
				sen++;
			}
			else if(sen>=1)
			{
				kousa--;
				go_back(40);
				turn_right(0);
				until(SENSOR_3 <= THERSHOLD);
				off;
				sen--;
			}
		}
	}
	off;
 }			
 task go_line()
 {
	while(true){
		if((SENSOR_1 >= THERSHOLD) && (SENSOR_3 >= THERSHOLD))
		{
			go_straight(0);
		}
		else if ((SENSOR_1 <= THERSHOLD) && (SENSOR_3 >= THERSHOLD))
		{
			turn_left(0);
		}
		else if ((SENSOR_1 >= THERSHOLD) && (SENSOR_3 <= THERSHOLD))
		{
			turn_right(0);	
		}
		else if((SENSOR_1 <= THERSHOLD) && (SENSOR_3 <= THERSHOLD))
    		{	
			PlaySound(SOUND_DOUBLE_BEEP);
			ClearTimer(2);
			go_straight(10);
			off;
			go_straight(0);
			until(((SENSOR_1 <= THERSHOLD) && (SENSOR_3 <=   THERSHOLD)) || (Timer(2) >= 6));
			if((SENSOR_1 <= THERSHOLD) && (SENSOR_3 <= THERSHOLD))
			{	
				off;
				saigo++;
				break;
			}
			else if(Timer(2) >= 6)
			{
				kousa++;
				off;
			} 
		}
	}
}
task search()
{
	ClearTimer(0);
	turn_left(30);
	turn_right(65);
	turn_left(40);
	off;
}

紫：直線
青：普通に曲がる
ピンク：関数を使って曲がる。

#define THERSHOLD 44　　　　　　　　　　　　　　　　　　　//閾値
#define go(t) OnFwd(OUT_A+OUT_C);Wait(t);            　//tの時間の間前に行く
#define back(t) OnRev(OUT_A+OUT_C);Wait(t);　　　　　　//tの時間の間後ろに行く
#define migi(t) OnFwd(OUT_A);OnRev(OUT_C);Wait(t);　　//tの時間の間右に曲がる
#define hidari(t) OnRev(OUT_A);OnFwd(OUT_C);Wait(t);　//tの時間の間左に曲がる
#define off Off(OUT_A+OUT_C);　　　　　　　　　　　　　//止まる
int kousa=0;　　　　　　//交差点を曲がる時使う関数　　　　
int sen=0;　　　　　　　//線を探す時使う関数
int saigo=0;　　　　　　//最後に止まる時使う関数
sub chika()　　　　　　//近道を行く		
{
  OnFwd(OUT_A);
  OnRev(OUT_C);
  Wait(40);
  off;
}

task main()
{
  SetSensor(SENSOR_1,SENSOR_LIGHT);　　　//1番光センサを付ける
  SetSensor(SENSOR_3,SENSOR_LIGHT);　　　//3番光センサを付ける
   ClearTimer(1);
     while(true)
      {
	 start go_line;
	 until((kousa == 1) || (Timer(1) == 167) || (saigo == 1));
	 stop go_line;
	 if (Timer(1)==167)　　　　//16.7秒になれば右に曲がって近道を探す。
	 {
	 	chika();
	 	continue;
	 }
	 else if (saigo == 1)
	 {
	 	break;
	 }			
	 start search;　　　　　//1番が黒とか3番が黒とかタイマーが15秒以上になれば探すのを止まる。
	　until((SENSOR_1 <= THERSHOLD) || (SENSOR_3 <= THERSHOLD) ||     (Timer(0)>=15));
	stop search;
	 if((SENSOR_1 <= THERSHOLD) || (SENSOR_3 <= THERSHOLD))　//二つのセンサー全部が線を認識した時
	 {
	 	kousa--;　　　　//二つのセンサーが全部線を認識したら関数は1をひく
	 }
	 else if((Timer(0)>=15))
	 {　　
          if(sen<1)　　　　　　　　　　　//線を探す関数が1より小さい時
           {
	             kousa--;　　　　　　//交差点を探す時使う関数は1をひく
	 	     back(40);
	             hidari(0);
		     until(SENSOR_1 <= THERSHOLD);　　//センサー1が黒になった時
	             off;
		     sen++;　　　　　　　//線を探す時の関数に1をたす
	   }
	   else if(sen>=1)　　　　　　　//線を探す関数が1以上の時
	   { 
	             kousa--;　　　　　//交差点を探す時使う関数は1をひく
		     back(40);
		     migi(0);
	             until(SENSOR_3 <= THERSHOLD);　　//センサー3が黒になった時までする
		     off;
		    sen--;　　　　　　//線を探す時の関数に1をひく
		}
	    }
        }
    off;
}			
task go_line()
{
    while(true){

 	if((SENSOR_1 >= THERSHOLD) && (SENSOR_3 >= THERSHOLD))
	{
		go(0);
	}　　　　　　　　　　　//1番が白、3番が白なら前に行く
	else if ((SENSOR_1 <= THERSHOLD) && (SENSOR_3 >= THERSHOLD))
	{
		hidari(0);
	}　　　　　　　　　　　//1番が黒、3番が白なら左に曲がる
	else if ((SENSOR_1 >= THERSHOLD) && (SENSOR_3 <= THERSHOLD))
	{
		migi(0);	
	}　　　　　　　　　　　//1番が白、3番が黒なら右に曲がる
	else if((SENSOR_1 <= THERSHOLD) && (SENSOR_3 <= THERSHOLD))　　//1番が黒、3番が黒なら音が出る。
    	{	
	　　　　PlaySound(SOUND_DOUBLE_BEEP);
		ClearTimer(2);
		go(10);
		off;
		go(0);
		until(((SENSOR_1 <= THERSHOLD) && (SENSOR_3 <= THERSHOLD)) || (Timer(2) >= 6));
		if((SENSOR_1 <= THERSHOLD) && (SENSOR_3 <= THERSHOLD))
		{	
		   off;
		   saigo++;　　　　　
		   break;　　　　　　　//関数に1をたしてロボットを止める
		}
		else if(Timer(2) >= 6)
		{
		   kousa++;　　　　　　//交差点に1をたす
		   off;
			} 
		}　　　　　　　　　
	}　　　　　　　　　　　　　　
}
task search()
{
	ClearTimer(0);
	hidari(30);
	migi(65);
	hidari(40);
	off;
}

↑