#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_A);
}

sub hold(){					//カンを掴む
	go_f(100);					//前進してカンの位置を合わせる
	RotateMotor(OUT_C,POWER_RE,ANGLE);	    //カン掴む
}

sub stack(){					//カンを積む
	go_f(170);					//前進してカンの位置を合わせる
	go_f(-10);					//カンから離れて位置調整
	RotateMotor(OUT_B,POWER_RE,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);              //bluetoothが確立するまで待機
	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();						//2の信号が来たら積む
		Off(OUT_C);						//カンを保持する力を抜く
		RotateMotor(OUT_C,POWER,ANGLE);		//カン離す
		go_f(-100);						//位置調整
		down_arm(); 						//アームを下げて次の準備
	}
}

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2017-08-13 (日) 23:42:40 (736d)