戻る

#define sikiiti 40  //黒と白のしきい値
#define WMAX  50000 //線を外れたとき、どれだけ戻るか。
#define BMAX  10   //交差点の判定
#define go_right OnFwd(OUT_C,50);OnFwd(OUT_B,-30);
#define go_left OnFwd(OUT_C,-30);OnFwd(OUT_B,50);
#define go_straight OnFwd(OUT_BC,20);
#define back_right OnFwd(OUT_C,-50);OnFwd(OUT_B,30); 
#define back_left OnFwd(OUT_C,30);OnFwd(OUT_B,-50);
#define back_straight OnFwd(OUT_BC,-40);
#define STOP Off(OUT_BC);Wait(1000);
#define senkai OnFwd(OUT_B,-30);OnFwd(OUT_C,30);Wait(2100);
#define tukamu OnFwd(OUT_A,20);Wait(1300);Off(OUT_A);
#define hanasu OnFwd(OUT_A,-20);Wait(900);Off(OUT_A); 
#define SPEEDy 18
#define importantnumber 3900 //弄ってピッタリの数値を探せ。

int count1=1,count2=0/*黒カウンタ*/,count3=0/*白カウンタ*/,count4=0,count5=0; //カウンタ 用意

inline void turn_right(){
		count3++;
		count2=0;
		go_right;
	}

inline void turn_left(){
		count2++;
		count3=0;
		go_left;
	}

inline void come_right(){
		STOP;
		while(count3>0){
			back_right; 
			count3--;
			Wait(1);
		}
		STOP;
	}

inline void kousa_ten(){
	STOP;
	back_left;
	Wait(300);
	STOP; 
	go_straight;
	Wait(1200);
	STOP;
	
	}

inline void yukku_ri(){
	if(SENSOR_1>sikiiti+10){//白いとき
		OnFwd(OUT_C,SPEEDy*2);OnFwd(OUT_B,SPEEDy*-2);
	}else if(SENSOR_1>sikiiti){
		OnFwd(OUT_C,SPEEDy*2);OnFwd(OUT_B,SPEEDy);
	}else if(SENSOR_1>sikiiti-10){//黒いとき
		OnFwd(OUT_C,SPEEDy);OnFwd(OUT_B,SPEEDy*2);
	}else{
		OnFwd(OUT_C,SPEEDy*-1);OnFwd(OUT_B,SPEEDy*2);
	}
	}

inline void WARNING_KAN(){
	int i,count_warning=0;
	STOP;
	while(count4!=1&&SensorUS(S2)>10){
		yukku_ri();
	}
	STOP;
	if(count4!=1){
		tukamu;
	}else{
		while(count_warning<=1000){
			yukku_ri();
			Wait(1);
			count_warning++;
		}
	}
	STOP;
	i=importantnumber;
	while(count5<=i){
 		yukku_ri();
 		Wait(1);
		count5++;
	}
	STOP;
 	if(count4!=1){
		senkai;
		STOP;
		hanasu;
		back_straight;
		Wait(800);
		STOP;
		senkai;
		while(SENSOR_1<=sikiiti){
			go_right;
 		}
	}
}

inline void Last_action(){
	int count_last=0;
	while(count_last<=500){
		yukku_ri();
		Wait(1);
		count_last++;
	}
	senkai;
	}

task main(){
	SetSensorLight(S1);
	SetSensorLowspeed(S2);
	OnFwd(OUT_A,-5);
	while(count1<7){
		if(SENSOR_1>sikiiti){//白いとき
			if(count3<=WMAX){
				turn_right();
			}else{
				come_right(); //行き過ぎたら戻る
			}
		}else if(SENSOR_1<=sikiiti){//黒いとき
			if(count2<=BMAX){
				turn_left();
			}else{
				PlayTone(440,20);
				if(count1<=5&&count1>=2){//二回目から五回目は交差点を直進する。
					kousa_ten();
				}
				while(SENSOR_1<=sikiiti){
					go_left;
				}
				count1++;
 			}
 		}
		if(SensorUS(S2)<=15&&count4!=3){
				WARNING_KAN();//ここに缶に当たった時の動作
				count4++;
		}
		Wait(1);
	}
	Last_action();
	}
}

戻る


トップ   編集 凍結 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2013-12-13 (金) 17:55:03