- 追加された行はこの色です。
- 削除された行はこの色です。
#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(); //アームを下げて次の準備
}
}