File content as of revision 16:a9885a66048f:

#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=30;
const int slidedist=150;
const int space=100;

int main()
{
    int team=1;
    setup();
    initmotor();
    
   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;
    
    

//オブジェクト0,1,2回収

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


}