File content as of revision 17:5b9fbb6133ad:

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


int main(){
        setup();
    initmotor();
    close_arm();
    while(1){
    pmove(600,0); 
    pmove2(300,600); 
    pmove2(0,0);     
    }
    
}

/*

int main()
{
    
    int team=1,phase=0;
    team=teamLED();
    setup();
    initmotor();
    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=0,goaly1=1200+150,goaly2=900+slidedist,goaly3=600+slidedist,goaly4=300+slidedist;
    
    phase=phaseSW();

    //phaseごとの初期位置修正
    
    
    //

//オブジェクト0,1,2回収
    if(phase==0){
       pmove(targetx[0],0);
       open_hand();
       pmove(targetx[0],targety[0]);
       close_hand();
       beltup();
       wait(btime);
       beltstop();
       open_hand();
       //回収
       pmove(targetx[1],targety[1]);
       close_hand();
       beltup();
       wait(btime);
       beltstop();
       open_hand();
       //回収
       pmove(targetx[2],targety[2]);
       //回収
       close_hand();
       pmove(600,1200+slidedist-space*3);//次につなげるハブ
       pmove(goalx,goaly1);
       //リリース
       open_arm();
       open_hand();
       back300();//(300,1200)なう
    turnrad_ccw(2*PI);
       phase=1;    
       
    }
    
    if(phase==1){
       //オブジェクト3,4回収
       open_arm();
       pmove(targetx[3],targety[3]);
       close_arm();
       close_hand();
       beltup();
       wait(btime);
       beltstop();
       pmove(1200,900+slidedist);//ハブ
       open_hand();
       pmove(targetx[4],targety[4]);
       
       pmove(goalx,goaly2);
       open_arm();
       open_hand();
       //リリース
       back300();
    
        phase=2;
    }
    
    if(phase==2){
       //オブジェクト5,6回収
       open_arm();
       pmove(1200,900+slidedist);//ハブ
       pmove(targetx[5],targety[5]);
       //回収
       pmove(targetx[6],targety[6]);
       //回収?

       pmove(goalx,goaly3);
       back300();
       phase=3;
    }
    
    if(phase==3){
       //オブジェクト7,8回収
       pmove(1200,600+slidedist);//ハブ

       pmove(targetx[7],targety[7]);
       //回収
        pmove(900,0);
        //回収
        pmove(targetx[8],targety[8]);
       //回収?
       pmove(goalx,300+slidedist+30);
       //リリース
       back300();
       
       phase=4;
       }


}*/