[[2009b/A2/ロボコン]]
------------------
CENTER:&size(30){紙パックを移動させるロボットのプログラム};
------------------

 #define eye_L SENSOR_1                      //eye_LをSENSOR_1と定義
 #define eye_R SENSOR_3                      //eye_RをSENSOR_2と定義
 #define  cont 45                            //contを45と定義
 #define time 2                              //timeを2と定義
 #define new_count ClearSensor(SENSOR_2)     //new_countをClearSensor(SENSOR_2)と定義
 sub follow_line()                           //ライントレースのサブタスク
 #define eye_L SENSOR_1				//eye_LをSENSOR_1と定義
 #define eye_R SENSOR_3				//eye_RをSENSOR_2と定義
 #define  cont 45				//contを45と定義
 #define time 2					//timeを2と定義
 #define new_count ClearSensor(SENSOR_2)		//new_countをClearSensor(SENSOR_2)と定義
 sub follow_line()								//ライントレースのサブタスク
 {
	if ((eye_L >= cont ) && (eye_R >= cont )){
		OnRev(OUT_A+OUT_C); Wait(time); Off(OUT_A+OUT_C);    
	}                                                             //白・白で直進
		OnRev(OUT_A+OUT_C); Wait(time); Off(OUT_A+OUT_C);
	}									//白・白で直進
	else if ((eye_L >= cont ) && (eye_R  <= cont )){
                  OnRev(OUT_A); Wait(time); Off(OUT_A);
	}                                                             //白・黒で左に曲がる
		OnRev(OUT_A); Wait(time); Off(OUT_A);
	}									//白・黒で左に曲がる
	else if ((eye_L <= cont ) && (eye_R >= cont )){
		OnRev(OUT_C); Wait(time); Off(OUT_C); 
	}                                                             //黒・白で右に曲がる
		OnRev(OUT_C); Wait(time); Off(OUT_C);
	}									//黒・白で右に曲がる
	else {
		OnRev(OUT_A+OUT_C); Wait(time); Off(OUT_A+OUT_C); 
	}                                                             //黒・黒で直進
		OnRev(OUT_A+OUT_C); Wait(time); Off(OUT_A+OUT_C);
	}									//黒・黒で直進
 }
 task main ()                                      //メインプログラム開始
 
 task main ()								//メインプログラム開始
 {
	SetSensor(eye_L, SENSOR_LIGHT);           //センサー1を光センサーにする
	SetSensor(eye_R, SENSOR_LIGHT);           //センサー2を光センサーにする
	SetSensor(SENSOR_2,SENSOR_ROTATION);      //センサー3を回転センサーにする
	Wait(20);                                 //0.2秒待ってからスタート
 new_count;                                        //回転センサーをリセット
	while ( abs(SENSOR_2) <= 35 )             //回転センサーの絶対値が35になるまで
		{
		follow_line();                   //サブタスクのライントレースをする
		}
 Off(OUT_A+OUT_C); Wait(50);                       //0.5秒停止
 new_count;                                        //回転センサーをリセット
	while ( abs(SENSOR_2) <= 3 )              //紙パックのほうを向く
		{OnRev(OUT_A); OnFwd(OUT_C);}
 OnRev(OUT_A); OnFwd(OUT_C); Wait(5);               //向きを微調節する
 OnRev(OUT_A+OUT_C); Wait(45); Off(OUT_A+OUT_C); Wait(100);    //紙パックをつかみに行くプログラム
 OnRev(OUT_B); Wait(200); Off(OUT_B); Wait(100);
 OnFwd(OUT_A); OnRev(OUT_C); Wait(15);                         //本体を左右にゆすりパックをきっちりつかむ
 OnRev(OUT_A); OnFwd(OUT_C); Wait(15);   Off(OUT_A+OUT_C);
 OnFwd(OUT_A+OUT_C); Wait(60); Off(OUT_A+OUT_C); Wait(100);
 new_count;                                                    //回転センサーをリセット
	while ( abs(SENSOR_2) <= 3 )                          //本体をラインと平行にする
		{
	SetSensor(eye_L, SENSOR_LIGHT);					//センサー1を光センサーにする
	SetSensor(eye_R, SENSOR_LIGHT);					//センサー2を光センサーにする
	SetSensor(SENSOR_2,SENSOR_ROTATION);				//センサー3を回転センサーにする
	Wait(20);							//0.2秒待ってからスタート
	new_count;							//回転センサーをリセット
	while ( abs(SENSOR_2) <= 35 )					//回転センサーの絶対値が35になるまで
	{
		follow_line();						//サブタスクのライントレースをする
	}
	Off(OUT_A+OUT_C); Wait(50);					//0.5秒停止
	new_count;							//回転センサーをリセット
	while ( abs(SENSOR_2) <= 3 )					//紙パックのほうを向く
	{
		OnRev(OUT_A); OnFwd(OUT_C);
	}
	OnRev(OUT_A); OnFwd(OUT_C); Wait(5);				//向きを微調節する
	OnRev(OUT_A+OUT_C); Wait(45); Off(OUT_A+OUT_C); Wait(100);	//紙パックをつかみに行くプログラム
	OnRev(OUT_B); Wait(200); Off(OUT_B); Wait(100);
	OnFwd(OUT_A); OnRev(OUT_C); Wait(15);				//本体を左右にゆすりパックをきっちりつかむ
	OnRev(OUT_A); OnFwd(OUT_C); Wait(15);   Off(OUT_A+OUT_C);
	OnFwd(OUT_A+OUT_C); Wait(60); Off(OUT_A+OUT_C); Wait(100);
	new_count;							//回転センサーをリセット
	while ( abs(SENSOR_2) <= 3 )					//本体をラインと平行にする
	{
		OnFwd(OUT_A); OnRev(OUT_C);
		}
 Off(OUT_A+OUT_C); Wait(100);
 OnFwd(OUT_A); OnRev(OUT_C); Wait(10);                         //向きを微調節する
 OnRev(OUT_A); OnFwd(OUT_C); Wait(10);   Off(OUT_A+OUT_C);
 OnRev(OUT_A+OUT_C); Wait(5); Off(OUT_A+OUT_C); Wait(100);
 OnFwd(OUT_A); OnRev(OUT_C); Wait(5); Off(OUT_A+OUT_C);
 new_count;                                                    //回転センサーをリセット
	while ( abs(SENSOR_2) <= 27)                          //回転センサーの絶対値が27になるまで
		{
		follow_line();                               //ライントレースする
		}
 PlaySound(1); Off(OUT_A+OUT_C); Wait(100);
 new_count;                                                    //回転センサーをリセット
	while ( abs(SENSOR_2) <= 10)                          //回転センサーの絶対値が10になるまで
		{
		OnFwd(OUT_A+OUT_C);                          //バックして右カーブに備える
		}
 Off(OUT_A+OUT_C); Wait(100); 
 OnRev(OUT_A); OnFwd(OUT_C); Wait(30);   Off(OUT_A+OUT_C);     //右へ曲がる
 Off(OUT_A+OUT_C);  Wait(20); 
 OnRev(OUT_A+OUT_C); Wait(30); Off(OUT_A+OUT_C); Wait(50);
 new_count;                                                    //回転センサーをリセット
	while ( abs(SENSOR_2) <= 45  )                        //回転センサーの絶対値が45になるまで
		{
		follow_line();                               //ライントレースする               
		}
 Wait(50); OnFwd(OUT_A+OUT_C); Wait(20); Off(OUT_A+OUT_C);     //バックして右カーブに備える
	repeat(3)                                             //かっこ内を三回くりかえす
		{
	}
	Off(OUT_A+OUT_C); Wait(100);
	OnFwd(OUT_A); OnRev(OUT_C); Wait(10);				//向きを微調節する
	OnRev(OUT_A); OnFwd(OUT_C); Wait(10);   Off(OUT_A+OUT_C);
	OnRev(OUT_A+OUT_C); Wait(5); Off(OUT_A+OUT_C); Wait(100);
	OnFwd(OUT_A); OnRev(OUT_C); Wait(5); Off(OUT_A+OUT_C);
	new_count;							//回転センサーをリセット
	while ( abs(SENSOR_2) <= 27)					//回転センサーの絶対値が27になるまで
	{
		follow_line();						//ライントレースする
	}
	PlaySound(1); Off(OUT_A+OUT_C); Wait(100);
	new_count;							//回転センサーをリセット
	while ( abs(SENSOR_2) <= 10)					//回転センサーの絶対値が10になるまで
	{
		OnFwd(OUT_A+OUT_C);					//バックして右カーブに備える
	}
	Off(OUT_A+OUT_C); Wait(100);
	OnRev(OUT_A); OnFwd(OUT_C); Wait(30);   Off(OUT_A+OUT_C);	//右へ曲がる
	Off(OUT_A+OUT_C);  Wait(20);
	OnRev(OUT_A+OUT_C); Wait(30); Off(OUT_A+OUT_C); Wait(50);
	new_count;							//回転センサーをリセット
	while ( abs(SENSOR_2) <= 45  )					//回転センサーの絶対値が45になるまで
	{
		follow_line();						//ライントレースする
	}
	Wait(50); OnFwd(OUT_A+OUT_C); Wait(20); Off(OUT_A+OUT_C);	//バックして右カーブに備える
	repeat(3)							//かっこ内を三回くりかえす
	{
		OnRev(OUT_A); OnFwd(OUT_C); Wait(5); Off(OUT_A+OUT_C); Wait(50);
		OnRev(OUT_A+OUT_C); Wait(10); Off(OUT_A+OUT_C); Wait(50);
		OnRev(OUT_A); OnFwd(OUT_C); Wait(10); Off(OUT_A+OUT_C); Wait(50);
		}                                            //角度を少しずつ曲げながらライン上に乗る
 PlaySound(1); Off(OUT_A+OUT_C);
 new_count;                                                    //回転センサーをリセット
	while ( abs(SENSOR_2) <= 51 )                         //回転センサーの絶対値が51になるまで
		{
		follow_line();                               //ライントレースする
		}
 OnFwd(OUT_B); Wait(190); Off(OUT_B); Wait(100);               //紙パックを離す
 OnFwd(OUT_A+OUT_C); Wait(40); Off(OUT_A+OUT_C);               //バックして最後のライントレースの線上に乗る
 PlaySound(1); Off(OUT_A+OUT_C);
 new_count;
	while ( abs(SENSOR_2) <= 3 )                          //ラインの方を向く
	}								//角度を少しずつ曲げながらライン上に乗る
	PlaySound(1); Off(OUT_A+OUT_C);
	new_count;							//回転センサーをリセット
	while ( abs(SENSOR_2) <= 51 )					//回転センサーの絶対値が51になるまで
	{
		follow_line();						//ライントレースする
	}
	OnFwd(OUT_B); Wait(190); Off(OUT_B); Wait(100);			//紙パックを離す
	OnFwd(OUT_A+OUT_C); Wait(40); Off(OUT_A+OUT_C);			//バックして最後のライントレースの線上に乗る
	PlaySound(1); Off(OUT_A+OUT_C);
	new_count;
	while ( abs(SENSOR_2) <= 3 )					//ラインの方を向く
	{
		OnRev(OUT_A); OnFwd(OUT_C);
		}
 Off(OUT_A+OUT_C);
 OnRev(OUT_A); OnFwd(OUT_C); Wait(7);                          //向きを微調節する
 Off(OUT_A+OUT_C); Wait(20);
 new_count;                                                    //回転センサーをリセット
	while ( abs(SENSOR_2) <= 45 )                         //回転センサーの絶対値が45になるまで
		{
		follow_line();                               //ライントレースする
		}
 OnRev(OUT_C); Wait(80);                                       //スタート地点のほうを向く
 OnRev(OUT_A+OUT_C); Wait(100); Off(OUT_A+OUT_C);              //スタート地点に入り停止
	}
	Off(OUT_A+OUT_C);
	OnRev(OUT_A); OnFwd(OUT_C); Wait(7);				//向きを微調節する
	Off(OUT_A+OUT_C); Wait(20);
	new_count;							//回転センサーをリセット
	while ( abs(SENSOR_2) <= 45 )					//回転センサーの絶対値が45になるまで
	{
		follow_line();						//ライントレースする
	}
	OnRev(OUT_C); Wait(80);						//スタート地点のほうを向く
	OnRev(OUT_A+OUT_C); Wait(100); Off(OUT_A+OUT_C);		//スタート地点に入り停止
 }



トップ   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS