#define set_power_L        SetPower(OUT_A,1);SetPower(OUT_C,1);
 #define THRESHOLD 45
 #define go_forward OnFwd(OUT_AC);
 #define turn_right0 OnFwd(OUT_A);OnRev(OUT_C);
 #define turn_right1 OnFwd(OUT_A);Off(OUT_C);
 #define turn_left1 Off(OUT_A);OnFwd(OUT_C);
 #define turn_left0 OnRev(OUT_A);OnFwd(OUT_C);
 #define STEP 1 
 #define U_turn    OnFwd(OUT_A);OnRev(OUT_C);Wait(180);Off(OUT_AC);

 /*コップを持ち上げる、下ろす動きをマクロ化*/
 #define cup_up OnFwd(OUT_B);Wait(130);Off(OUT_B);
 #define cup_down OnRev(OUT_B);Wait(130);Off(OUT_B);

/*グローバル変数。コップの色に応じて1〜3の値をとり、点Aからのパターン分岐を決める。*/
int number=0;

/*論理積を使ったライントレースのサブルーチン*/
sub line(){
	if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
		go_forward;
		Wait(STEP);
	}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
		turn_right1;
		Wait(STEP);
	}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
		turn_left1;
		Wait(STEP);
	}else{go_forward;
		Wait(STEP);
	}
}

/*紙コップを掴んだ後に「点A」に辿り着いてから行う操作のサブルーチン*/
sub carry_cup(){
	if(number==1){
		while((SENSOR_2<47)&&(SENSOR_2>35)){
			if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				go_forward;
				Wait(STEP);
			}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
				turn_right1;
				Wait(STEP);
			}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				turn_left1;
				Wait(STEP);
			}else{
				go_forward;
				Wait(STEP);
			}
		}
		go_forward;
		Wait(10);//紙コップに完全に接触するまで前進。そこからの位置調整も要調査
		Off(OUT_AC);
		cup_down;
		
		U_turn;
		while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップBかCを認識するまで前進させる
			if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				go_forward;
				Wait(STEP);
			}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
				turn_right1;
				Wait(STEP);
			}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				turn_left1;
				Wait(STEP);
			}else{
				go_forward;
				Wait(STEP);
			}
		}
	}else if(number==2){
		while((SENSOR_1>THRESHOLD)||(SENSOR_3>THRESHOLD)){//交差点を認識するまで前進させる
			if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				go_forward;
				Wait(STEP);
			}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
				turn_right1;
				Wait(STEP);
			}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				turn_left1;
				Wait(STEP);
			}else{
				go_forward;
				Wait(STEP);
			}
		}
		turn_right0;
		Wait(75);//コップ2の逆方向を向かせる
		go_forward;
		Wait(10);
		U_turn;//交差点でそのまま左折するとコップ2を押してしまう為
		while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップ2を認識するまで前進させる
			if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				go_forward;
				Wait(STEP);
			}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
				turn_right1;
				Wait(STEP);
			}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				turn_left1;
				Wait(STEP);
			}else{
				go_forward;
				Wait(STEP);
			}
		}
		go_forward;
		Wait(20);//紙コップに完全に接触するまで前進。そこからの位置調整も要調査
		Off(OUT_AC);
		cup_down;
		OnRev(OUT_AC);
		Wait(130);
		while((SENSOR_1>THRESHOLD)||(SENSOR_3>THRESHOLD)){//交差点を認識するまで前進させる
			if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				go_forward;
				Wait(STEP);
			}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
				turn_right1;
				Wait(STEP);
			}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				turn_left1;
				Wait(STEP);
			}else{
				go_forward;
				Wait(STEP);
			}
		}
		turn_left0;
		Wait(75);//軸にそって紙コップBかCに向かせる
		while((SENSOR_2>35)&&(SENSOR_2<47)){//紙コップを認識するまで
			if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				go_forward;
				Wait(STEP);
			}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
				turn_right1;
				Wait(STEP);
			}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				turn_left1;
				Wait(STEP);
			}else{
				go_forward;
				Wait(STEP);
			}
		}
	}else{
		while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進させる
			if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				go_forward;
				Wait(STEP);
			}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
				turn_right1;
				Wait(STEP);
			}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				turn_left1;
				Wait(STEP);
			}else{
				go_forward;
				Wait(STEP);
			}
		}
		go_forward;
		Wait(10);
		while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進させる
			go_forward;
		}
		turn_right0;
		Wait(75);
		while((SENSOR_2>35)&&(SENSOR_2<47)){//紙コップを認識するまで
			go_forward;
		}
		go_forward;
		Wait(25);
		Off(OUT_AC);
		cup_down;
		OnRev(OUT_AC);
		Wait(110);
		while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進させる
			OnRev(OUT_AC);
			}
		turn_right0;
		Wait(75);
		}
		while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップを認識するまで
			if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				go_forward;
				Wait(STEP);
			}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
				turn_right1;
				Wait(STEP);
			}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
				turn_left1;
				Wait(STEP);
			}else{
				go_forward;
				Wait(STEP);
		}
	}
}
		

/*開始直後の紙コップAを掴み振り向くまでの動きをインライン関数化*/
void start_dash(){
	go_forward;
	Wait(120);//スタート地点からでる為の動き
	while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進
		go_forward;
	}
	turn_right0;
	Wait(75);
	while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップを認識するまで
		if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
			go_forward;
			Wait(STEP);
		}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
			turn_right1;
			Wait(STEP);
		}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
			turn_left1;
			Wait(STEP);
		}else{
			go_forward;
			Wait(STEP);
		}
	}
	go_forward;
	Wait(100);//コップに接触するまで
	Off(OUT_AC);
	if(SENSOR_2>47){//白
		number=1;
	}else if((SENSOR_2<47)&&(SENSOR_2>37)){
		number=2;
	}else{
		number=3;
	}//センサーの値に応じてnumberに値を入れる
	cup_up;
	U_turn;
}


task main(){
	SetSensor(SENSOR_1,SENSOR_LIGHT);
	SetSensor(SENSOR_2,SENSOR_LIGHT);
	SetSensor(SENSOR_3,SENSOR_LIGHT);
	set_power_L;
	int cross=0;//交差点通過回数のカウンタ

	start_dash();//コップAを掴む
	carry_cup();
	go_forward;
	Wait(50);//コップBに接触するまで
	Off(OUT_AC);
	if(SENSOR_2>47){
		number=1;
	}else if((SENSOR_2>35)&&(SENSOR_2<47)){
		number=2;
	}else{
		number=3;
	}//センサーの値に応じてnumberに値を入れる
	cup_up;
	U_turn;
	ClearTimer(0);//タイマー設置
	while(Timer(0)<40){
		if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
			go_forward;
			Wait(STEP);
		}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
			turn_right1;
			Wait(STEP);
		}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
			turn_left1;
			Wait(STEP);
		}else{
			go_forward;
			Wait(STEP);
		}
	}
	carry_cup();
	go_forward;
	Wait(50);//コップCに接触するまで
	Off(OUT_AC);
	if(SENSOR_2>47){
		number=1;
	}else if((SENSOR_2>35)&&(SENSOR_2<47)){
		number=2;
	}else{
		number=3;
	}//センサーの値に応じてnumberに値を入れる
	cup_up;
	U_turn;
	ClearTimer(0);
	while(Timer(0)<20){
		if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
			go_forward;
			Wait(STEP);
		}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
			turn_right1;
			Wait(STEP);
		}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
			turn_left1;
			Wait(STEP);
		}else{
			go_forward;
			Wait(STEP);
		}
	}
	carry_cup();
	Off(OUT_AC);
}


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