File content as of revision 45:51ab3ca47228:

#include <mbed.h>
#include "debug.h"//DEBUG("",変数);でデバッグ。
#include "state.h"
#include "go.h"
#include "servo.h"
#include "interrupt.h"
#include "workinfo.h"
#include "controller.h"
#include "calplace.h"
#include "check.h"
#include "discoveryboard.h"
//debug用に追加
#include "coordinate.h"
//#define DEBUG_MODE
const int kWait_ms = 500;
const double kServoOffsetDegreeInBox =
#ifdef RIGHT
    7;
#elif defined LEFT
    7;
#endif

int worknum = 0;
int main()
{
    GoSetup();//encoder等のsetup
    SetupPosition();//ワークの位置設定
    if(PositionSetupIsInBan() == 1) return 1;//ワーク位置が禁止領域に入っていたら強制終了
    GoToFirstPosition();//機体をスタート位置に持っていく
    DEBUG("stand by. Please put pattern button.\r\n");
    while(PermitStart() == 0) {};//スタート指示受付
    DEBUG("main start\r\n");
    InterruptSetup();
    while((worknum = CalPickPlace()) != 23 ) {//取るワーク計算.23は番兵
        //ワークに向かう.-1でスキップ
        if(Go(work[worknum]) == -1) {
            work[worknum].is_exist = 0;
            continue;
        }
        wait_ms(kWait_ms);
        //掴む
        Close();
        wait_ms(kWait_ms);
        //上に持ち上げる
        Above(work[worknum].areaname);
        wait_ms(kWait_ms);
        //取り上げたことを保存
        work[worknum].is_exist = 0;
        //シュート位置計算
        int boxspace = CalPutPlace(work[worknum].color);
        OriginDegree[0] = kOriginDegree[0] - kServoOffsetDegreeInBox;
        //ボックスに行く(失敗(-1)したらcontinue)
        //3のときだけ特別に速度下げる
        if(worknum == 0) servo_dist_mm /= 2;
        if(Go(shootingbox[boxspace]) != 0) continue;
        servo_dist_mm = kServoVelocity_mm* kCalTargetTicker_s ;//
        ManualPutPlace(shootingbox[boxspace], 15);
        //放す
        Open();
        wait_ms(kWait_ms);
        OriginDegree[0] = kOriginDegree[0];
        //上に持ち上げる
        Above(BOX);
        wait_ms(kWait_ms);
        //置いたことを保存
        shootingbox[boxspace].is_exist = 1;
        shootingbox[boxspace].color = work[worknum].color;
    }
    // SpiSetup();
    //ManualMode();
}