#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(); //アームを下げて次の準備 } }