File content as of revision 19:48c3af917932:

#include <mbed.h>
#include "debug.h"//DEBUG("",変数);でデバッグ。
#include "state.h"
#include "go.h"
#include "servo.h"
#include "interrupt.h"
#include "workposition.h"
#include "controller.h"
//debug用に追加
#include "interface.h"
#include "coordinate.h"

const int kWait_ms = 500;

//#define FORDEBUG
///setup関連を集めた
void Setups();
void Tests();
Interface interface(USBTX, USBRX);
int main()
{
    DEBUG("setup start\r\n");
    Setups();

    //スタート指示受付
    DEBUG("stand by");
    while(CanStart() == 0) {};
    DEBUG("main start");
    InterruptSetup();
    int worknum = 0;

#ifdef FORDEBUG
    for(int i = kCommonStart; i < kCommonStart + kCommonAreaNum - 2; i++) work[i].is_exist = 0;
#endif
    while( (worknum = CalPickPlace()) != 23 ) {
        Close();
        //ワークに向かう
        DEBUG("worknum %d, color %d, area %d, position %f, %f, %f\r\n", worknum, work[worknum].color, work[worknum].areaname, work[worknum].position[0],work[worknum].position[1],work[worknum].position[2]);
        DEBUG("go to work\r\n");
        if(Go(work[worknum]) != 0) continue;
        DEBUG("close\r\n");
        //掴む
        Close();
        DEBUG("above\r\n");
        wait_ms(kWait_ms);
        //上に持ち上げる
        Above(work[worknum].areaname);
        wait_ms(kWait_ms);
        //取り上げたことを保存
        work[worknum].is_exist = 0;
        //シュート位置計算
        DEBUG("cal put place\r\n");
        int boxspace = CalPutPlace(work[worknum].color);
        //ボックスに行く(失敗(-1)したらcontinue)
        DEBUG("go to box %d\r\n",boxspace);
        if(Go(shootingbox[boxspace]) != 0) continue;
        DEBUG("open\r\n");
        //放す
        Open();
        wait_ms(kWait_ms);
        DEBUG("above\r\n");
        //上に持ち上げる
        Above(BOX);
        wait_ms(kWait_ms);
        DEBUG("finish\r\n");
        //置いたことを保存
        shootingbox[boxspace].is_exist = 1;
        shootingbox[boxspace].color = work[worknum].color;
    }
}
void Setups()
{
    GoSetup();
    SetupPosition();
    StartCheck();
}
void Tests()
{
    static int count = 0;
    count ++;
    if (count > 100000) {
        count =0;
    }
}