File content as of revision 21:7df0f8cd2737:

#include "mbed.h"
#include "math.h"
#include "locate.h"
#include "move.h"
#include "servo.h"
#include "button.h"

DigitalOut ledg(PC_2); //green
DigitalOut ledy(PC_3); //yellow
DigitalOut ledr(PC_0); //red


const int dist=150;//タイヤ、アーム間の距離
const int slidedist=0; //102-20;
const int space=50;//壁やばそうなときのよゆう
//btimeはservoに移動
const float allowdegree=0.2;
const float allowlength=0.2;

const int unit=583;


int main()
{
    Serial pc(SERIAL_TX, SERIAL_RX);    //pcと通信
    int teams=1,phase=0;
    teams=teamLED();
    int team = 1;
    setup(teams);
    initmotor(teams);

    open_arm();
    wait(2);
    close_arm();
    buzzer(1);
    wait(1);
    buzzer(0);

    int targetx[9] = { 600,600,600,1200,900,1200,900,1200,900 };
    int targety[9] = { team*(300+slidedist),team*(600+slidedist),team*(900+slidedist),team*(1200+slidedist),team*(900+slidedist),team*(600+slidedist),team*(600+slidedist),team*(0+slidedist),team*(300+slidedist) };
    int goalx=220,goaly=1200-30+(slidedist)*0;//yへのslidediatを無視

    phase=phaseSW();

    if(phase==0) {
        buzzer(1);
        wait(0.4);
        buzzer(0);
        wait(0.2);
        buzzer(1);
        wait(0.4);
        buzzer(0);
        wait(0.2);
        buzzer(1);
        wait(0.4);
        buzzer(0);
    }

    while(start()) {}
    if(phase==15) {
        movelength(900);
        buzzer(1);

        while(1) {
            update();
        }
    }

    if(phase==14) {
        int i = 1;
        while(1)
        {
            
            turnrad(-2*PI*3*(i++));
            wait(5);
        }
        
        while(1) {
            update();
        }
    }
    if(phase==13) {
        move(-70, -70);
    }

    if(phase==12) {
        while(1) {
            open_arm();
            wait(3);
            close_arm();
            wait(3);
        }
    }

    if(phase==11) {
        while(1) {
            close_hand();
            wait(0.3);
            lift();
            wait(0.2);
            open_hand();
            wait(2);
        }
    }

    if(phase==10) {
        while(1) {
            move(70,70);
            update();
        }
    }
    if(phase==9) {
        while(1) {
            move(32,30);
            while(1) {
                update_np();
                if(coordinateX()>900) {
                    break;
                }
            }
            pc.printf("start");
            update();
        }
    }

//オブジェクト0,1,2回収
    if(phase==0) {

        pmove2(targetx[0],-60);
        open_hand();
        pmove2(targetx[0],targety[0]);
        close_hand();
        wait(0.3);
        lift();
        open_hand();
        //回収
        pmove2(targetx[1],targety[1]);
        close_hand();
        wait(0.3);
        lift();
        open_hand();
        //回収
        pmove2(targetx[2],targety[2]);
        //回収
        close_hand();
        pmove2(goalx,goaly-40);
        //リリース
        turnrad_ccw(PI,team);
        open_arm();
        open_hand();
        wait(0.5);

        back300();//(300,1200)なう
        turnrad_ccw(2*PI,team);
//--------------------------------------------------------------------
        open_hand();
        \
        pmove2(targetx[3]-50,targety[3]);

        close_arm();
        close_hand();
        turnrad_cw(PI,team);

        pmove2(300,team*(goaly));//ゴール

        open_arm();
        open_hand();
        wait(0.3);


        back300();
        turnrad_ccw(2*PI,team);
        phase=2;

        //また縦に回収
        close_arm();
        pmove2(900,team*(1200+30));

        open_hand();
        pmove2(targetx[4],targety[4]);
        close_hand();
        wait(0.3);
        lift();
        open_hand();
        //回収
        pmove2(targetx[6],targety[6]);
        close_hand();
        wait(0.3);
        lift();
        open_hand();
        //回収
        pmove2(targetx[8],targety[8]);
        //回収
        close_hand();
        pmove2(150,team*(600+slidedist));//斜め移動

        turnrad_ccw(PI,team);
        open_arm();
        open_hand();
        back300();


        pmove2(600,600);
        pmove2(targetx[5]-50,targety[5]);
        close_arm();
        close_hand();
        nxback300();
        pmove2(150,600+slidedist);
        open_arm();
        open_hand();
        back300();
        pmove2(900-50,50);
        pmove2(1200-90,0);
        close_arm();
        close_hand();
        nxback300();
        pmove2(150,900+slidedist);
        turnrad_ccw(PI,team);
        open_arm();
        open_hand();
        back300();

        while(1) {
            buzzer(1);
            wait(0.5);
            buzzer(0);
            wait(1);
        }
    }


    if(phase==2) {
        open_arm();

        open_hand();
        pmove2(900,0);
        pmove2(1200-80,0);//オブジェクト
        close_arm();
        close_hand();
        nxback300();
        pmove2(150,900+slidedist);
        turnrad_ccw(PI,team);
        open_arm();
        open_hand();
        back300();

        pmove2(600,600);
        pmove2(targetx[5]-80,targety[5]);
        close_arm();
        close_hand();
        nxback300();
        pmove2(150,600+slidedist);
        back300();

    }

    if(phase==1) {

        pmove(900,0);
        open_hand();
        pmove2(targetx[8],targety[8]);
        close_hand();
        wait(0.3);
        lift();
        open_hand();
        //回収
        pmove2(targetx[6],targety[6]);
        close_hand();
        wait(0.3);
        lift();
        open_hand();
        //回収
        pmove(targetx[4],targety[4]);
        //回収
        close_hand();
        pmove2(goalx,goaly-40);
        //リリース
        turnrad_ccw(PI,team);
        open_arm();
        open_hand();
        wait(0.5);
        back300();//(300,1200)なう

    }


    if(phase==3) {


        pmove2(targetx[0],-60);
        open_hand();
        pmove2(targetx[0],targety[0]);
        close_hand();
        wait(0.3);
        lift();
        open_hand();
        //回収
        pmove2(targetx[1],targety[1]);
        close_hand();
        wait(0.3);
        lift();
        open_hand();
        //回収
        pmove2(targetx[2],targety[2]);
        //回収
        close_hand();
        pmove2(goalx,goaly-40);
        //リリース
        turnrad_ccw(PI,team);
        open_arm();
        open_hand();
        wait(0.5);
        back300();//(300,1200)なう
        turnrad_ccw(2*PI,team);


        close_arm();
        pmove2(900,team*(1200+30));

        open_hand();
        pmove2(targetx[4],targety[4]);
        close_hand();
        wait(0.3);
        lift();
        open_hand();
        //回収
        pmove2(targetx[6],targety[6]);
        close_hand();
        wait(0.3);
        lift();
        open_hand();
        //回収
        pmove2(targetx[8],targety[8]);
        //回収
        close_hand();
        pmove2(150,team*(600+slidedist));//斜め移動

        turnrad_ccw((-1)*PI,team);
        open_arm();
        open_hand();
        back300();
    }

    if(phase == 10) {
        buzzer(1);
        turnrad(6*PI);
        while(1) update();
    }



    if(phase == 8) {
        open_arm();
        open_hand();

        pmove2(0,1000);
        pmove2(600,1200);
        pmove2(1200-50,1200);
        close_hand();
        close_arm();
        turnrad_cw((-1)*PI,1);
        pmove2(150,1200);
        open_hand();
        open_arm();
        back300();

        turnrad_ccw(0,1);
        pmove2(1800,1200);
        close_hand();
        close_arm();
        turnrad_cw((-1)*PI,1);
        pmove2(150,1200);
        open_hand();
        open_arm();
        back300();
    }

    if(phase == 7) {
        open_arm();
        open_hand();

        pmove2(0,1000);
        pmove2(600,1200);
        pmove2(1800,1200);
        close_hand();
        close_arm();
        turnrad_cw((-1)*PI,1);
        pmove2(150,1200);
        open_hand();
        open_arm();
        back300();


    }

    if(phase == 6) {
        buzzer(1);
        initbutton();
        int command[3];
        commandIn(command);

        commandMove(command[0], command[1], command[2]);
    }

    if(phase == 5) {
        buzzer(1);
        initbutton();
        int command[3];
        commandIn(command);

        commandMoveEnemy(command[0], command[1], command[2]);
    }

}