#define CONN 0
 #define INBOX MAILBOX3
 #define ang_up 340				//持ち上げ用角度
 #define ang_put 80				//カン置く角度
 #define ANGLE 50				//掴む用角度
 #define POWER 30				//正転
 #define POWER_LEGS 40			//足回り用
 #define POWER_RE -30			//逆転
 sub go_f(int l){						//指定距離前進
 	float Ang=l*2.0463;					//回転角度計算
 	RotateMotor(OUT_A,POWER_LEGS,Ang);	//前進
 	Off(OUT_AB);
 }
 sub hold(){						//カンを掴む
 	go_f(100);							//前進してカンの位置を合わせる
 	RotateMotor(OUT_C,POWER_RE,ANGLE);	//カン掴む
 }
 sub stack(){					//カンを積む
 	go_f(170);							//前進してカンの位置を合わせる
 	go_f(-10);							//カンから離れて位置調整
 	RotateMotor(OUT_B,-40,ang_put);		//カンを置く(積む)
 }
 sub down_arm(){						//アームを下げる
 	OnFwd(OUT_B,POWER_RE);
 	Wait(1100);						
 	Off(OUT_B);
 }
 task main(){
 	int msg=0;
 	until(BluetoothStatus(CONN)==NO_ERR);
 	go_f(-50);
 	down_arm(); 							//アームを下げて次の準備
 	while(1){
 		hold();								//カン掴む
 		OnFwd(OUT_C,POWER_RE);				//保持し続ける
 		RotateMotor(OUT_B,50,ang_up);		//持ち上げる
 		go_f(-150);							//位置調整
 
 		ReceiveRemoteNumber(MAILBOX3,true,msg);			//集める側からの信号が来るまで待機
 		while(msg!=2){
 			ReceiveRemoteNumber(MAILBOX3,true,msg);
 			Wait(10);
 		}
 		stack();							//信号が来たら積む
 		Off(OUT_C);							//カンを保持する力を抜く
 		RotateMotor(OUT_C,POWER,ANGLE);		//カン離す
 		go_f(-100);							//位置調整
 		down_arm(); 						//アームを下げて次の準備
 	}
 }


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