File content as of revision 20:4b8ce61a00b2:

#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()
{   

    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);
        }
        
    if(phase==14){
       while(1){ 
        turnrad_cw(-2*PI,1);
        turnrad_ccw(0,1);
       }
    } 
    if(phase==13){
        nxback300();
        }
    
    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);
        }
    }
//オブジェクト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();
       
       pmove(targetx[3]-50,targety[3]);
       
       close_arm();
       close_hand();
       turnrad_cw(PI,team);

       pmove2(150,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],targety[5]);
     close_arm();
     close_hand();
     nxback300();
     pmove2(150,600+slidedist);
    open_arm();
    open_hand();
    back300();
     pmove2(900,50);
     pmove2(1200-50,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]);
    }
}