File content as of revision 19:e3383efa483b:

#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=102-20;
const int space=50;//壁やばそうなときのよゆう
const float btime=1.9;
const float allowdegree=0.2;
const float allowlength=0.2;


int main()
{
    
    int teams=1,phase=0;
    teams=teamLED();
    int team = 1;
    setup(teams);
    initmotor(teams);
    open_arm();
    wait(2);
    close_arm();
    
    while(start()){}
   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=150,goaly=1200+(slidedist)*0;//yへのslidediatを無視
    
    phase=phaseSW();
    
    //

//オブジェクト0,1,2回収
    if(phase==0){
       pmove(targetx[0],0); //傾きをはじめから向くように
       open_hand();
       pmove2(targetx[0],targety[0]);
       close_hand();
       wait(0.3);
       beltup();
       wait(btime);
       beltstop();
       open_hand();
       //回収
       pmove2(targetx[1],targety[1]);
       close_hand();
       wait(0.3);
       beltup();
       wait(btime);
       beltstop();
       open_hand();
       //回収
       pmove(targetx[2],targety[2]);
       //回収
       close_hand();
       pmove2(goalx,goaly);
       //リリース
       turnrad_ccw(PI,team);
       open_arm();
       open_hand();
       wait(0.5);
       back300();//(300,1200)なう
        turnrad_ccw(2*PI,team);
//--------------------------------------------------------------------
       open_arm();
       pmove(targetx[3],targety[3]);
       close_arm();
       close_hand();
       turnrad_cw(PI,team);
       /*
       open_hanf();
       pmove(1800,team*(1200+slidedist));
       closehand();
       */
       pmove(150,team*(1200+slidedist-space));//ゴール

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

       
        back300();
        turnrad_ccw(2*PI,team);
        phase=2;
        
       //また縦に回収
       close_arm();
       pmove2(900,team*(1200+slidedist+space));

       open_hand();
       pmove2(targetx[4],targety[4]);
       close_hand();
       wait(0.3);
       beltup();
       wait(btime);
       beltstop();
       open_hand();
       //回収
       pmove(targetx[6],targety[6]);
       close_hand();
       wait(0.3);
       beltup();
       wait(btime);
       beltstop();
       open_hand();
       //回収
       pmove(targetx[8],targety[8]);
       //回収
       close_hand();
       pmove2(150,team*(900+slidedist));//斜め移動
       
       turnrad_ccw(PI,team);
       open_arm();
       open_hand();
       back300();
    
       
       pmove2(700,(600+slidedist));//ハブ

       pmove(targetx[5],targety[5]);
       close_arm();
       close_hand();
       nxback300();
       
       pmove2(150,team*(900+slidedist));
       open_arm();
       open_hand();
       
        back300();
        while(1){
            buzzer(1);
            wait(0.5);
            buzzer(0);
            wait(1);   
        }
    }
    
    
    if(phase==1){
        open_arm();
        open_hand();
     pmove(targetx[7],targety[7]);
     close_arm();
     close_hand();
     nxback300();
     pmove2(150,900+slidedist);
     turnrad_ccw(PI,team);
     open_arm();
     open_hand();
     back300();
     
    pmove2(2100,1200);
        
    }
    
}