[[2013b/MemberOnly/進行状況]]

#contents

三缶積んだあとにゴールまでライントレースするプログラム

 	//↓缶の発見及び積み上げのための定義↓//
 #define CONN 1
 #define SIGNAL_A 11
 #define SIGNAL_B 12
 #define SIGNAL_C 13
 #define SIGNAL_D 14
 #define SIGNAL_E 15
 #define SIGNAL_F 16
 #define SIGNAL_G 17
 #define go_forward OnFwd(OUT_AB,50);//出力50%で前進
 #include "koushin.nxc"
 #define tr OnFwdSync(OUT_AB,60,100);
 #define tl OnFwdSync(OUT_AB,60,-100);
 #define go OnFwdSync(OUT_AB,75,0)
 #define unko 60
 #define direction 13
 #define measure_time 10


	//↓ライントレース用の定義↓//
 #define BB 37 //閾値
 #define BW 47
 #define WB 53
 #define WW 60

 #define OnRL(speedR,speedL) OnFwd(OUT_A,speedR);OnFwd(OUT_B,speedL);

 #define FWD OnRL(40,40); //前進

 #define T_LEFT OnRL(40,0); //左折
 #define T_RIGHT OnRL(0,40); //右折
 #define Q_LEFT OnRL(30,-30); //左旋回
 #define Q_RIGHT OnRL(-30,30); //右旋回

 #define STEP 5 //一回の判断で行う動作の長さ
 #define nMAX 20 //カウンタの最大値
 #define SB Off(OUT_AB);Wait(500); //小休止
 #define cross_line RotateMotorEx(OUT_AB,40,500,0,true,true); //交差点通過

	int Online1=0; //交差点通過ためのカウントの変数
	int Next=0; //動作変更のための変数

 sub CROSS_1() //黒線の右の縁に沿って動き、ゴールを目指す。
 {

	while(Online1<nMAX){

	if(SENSOR_1<BB){		

	Q_RIGHT;
	
	Online1++;

	} else {

	if(SENSOR_1<BW){
	
	T_RIGHT;
	
	} else if(SENSOR_1<WB){	
	
	FWD;
	
	} else if(SENSOR_1<WW){
	
	T_LEFT;

	} else {

	Q_LEFT;

	}

	Online1=0;
		
	}

	Wait(STEP);

	}

	Next++; //T字路と交差点を通過した回数を追加(それぞれ一回ずつ)
	SB;
	cross_line;
	Online1=0; //カウンタをリセット

 }	

 sub can_tsumu()
 {//缶を積み、持ち上げるサブルーチン 

 SendRemoteNumber(CONN,MAILBOX1,SIGNAL_B);
 /*Sのメールボックス1にSIGNAL_Bを送信(缶を載せ、離し、アームを最上部へ)*/
 
 Off(OUT_AB);//モータAB停止
 
 receive(MAILBOX2,"SIGNAL_F");
 /*Sからメールボックス2にSIGNAL_Fを受信*/

  OnRev(OUT_AB,50);Wait(1500);//1.5秒後退

  Off(OUT_AB);//モータAB停止

  SendRemoteNumber(CONN,MAILBOX1,SIGNAL_A);
  /*Sのメールボックス1にSIGNAL_Aを送信(再び缶を掴み、上部へ)*/
  
  Off(OUT_AB);//モータAB停止
  
  receive(MAILBOX2,"SIGNAL_C");
  /*Sからメールボックス2にSIGNAL_Cを受信*/
  
   OnFwd(OUT_AB,40);Wait(1000);//1秒前進
  
   Off(OUT_AB);Wait(3000);//3秒停止
  
   sendmsg(CONN,MAILBOX1,MAILBOX2,"SIGNAL_D","SIGNAL_E");
   /*Sのメールボックス1にSIGNALDを送信*/
  
 }
  
 task main()
 {
  
 	SetSensorLight(S1); //光センサーを設定
 	SetSensorLowspeed(S4); //超音波センサーを設定
 
 	while(Next<1){ //T字路を渡ったら缶の積み上げに入る
 
 		CROSS_1();
 
 	}
 
 	while(Next==1){
  
 		OnFwdSync(OUT_AB,40,0); //缶を見つけるまで前進
 
 		if(SensorUS(S4)<=12){
 
  		TextOut(0,LCD_LINE2,"while(hakken==1)");  
 
 		can_tsumu();
 
 		Next++;
 
 		}
 
 	}
 
 	while(Next==2){ //交差点についたら缶を置く
 
 		CROSS_1();
 
 	}
 
 
 	while(Next==3){
 
 	RotateMotorEx(OUT_AB,40,580,100,true,true); //向きをゴールの方へ向ける
 
 	SB; //小休止(連続して動作させると変な動きをするので)
 
 	RotateMotorEx(OUT_AB,-40,800,0,true,true); //ゴール内に缶を置けるように後退
 
 	SendRemoteNumber(CONN,MAILBOX1,SIGNAL_G); //缶を下ろす
 
 	Next++;
 
 	}
 
 }


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