File content as of revision 13:126c3f7f9b89:

#include <mbed.h>
#include "debug.h"//DEBUG("",変数);でデバッグ。
#include "state.h"
#include "go.h"
#include "servo.h"
#include "interrupt.h"
#include "position.h"
#include "controller.h"
//debug用に追加
#include "interface.h"
#include "coordinate.h"
///setup関連を集めた
void Setups();
void Tests();
Interface interface(USBTX, USBRX);
int main()
{
    DEBUG("setup start\r\n");
    Setups();
        GoToZeroSlowly();
    

    double posi[3] = {200,200,50};
    SetNextRadRelative(0, -90*kDegreeToRad);
    SetNextRadRelative(1, 150*kDegreeToRad);
    SetNextRadRelative(3, 0*kDegreeToRad);
    SetServoYawRad(0);
    ServoMoveOnArm();
    wait(1);
    ServoMoveOnArm();
    //スタート指示受付
    while(CanStart() == 0) {};
    DEBUG("main start");
    InterruptSetup();
    int worknum = 0;
    while( (worknum = CalPickPlace()) != 23 ) {
        //ワークに向かう
        if(Go(work[worknum]) != 0) continue;
        DEBUG("finish 1\r\n");
        //掴む
        Close();
        DEBUG("finish 2\r\n");
        //上に持ち上げる
        Above();
        DEBUG("finish 3\r\n");
        //取り上げたことを保存
        work[worknum].is_exist = 0;
        //シュート位置計算
        int boxspace = CalPutPlace(work[worknum].color);
        DEBUG("finish 4\r\n");
        //ボックスに行く(失敗(-1)したらcontinue)
        if(Go(shootingbox[boxspace]) != 0) continue;
        DEBUG("finish 5\r\n");
        //放す
        Open();
        DEBUG("finish 6\r\n");
        //上に持ち上げる
        Above();
        DEBUG("finish 7\r\n");
        //置いたことを保存
        shootingbox[boxspace].is_exist = 1;
        shootingbox[boxspace].color = work[worknum].color;
    }
}
void Setups()
{
    GoSetup();
    SetupPosition();

}
void Tests()
{
    static int count = 0;
    count ++;
    if (count > 100000) {
        count =0;
    }
}

#if 0

#include <mbed.h>
#include "debug.h"//DEBUG("",変数);でデバッグ。
#include "state.h"
#include "go.h"
#include "servo.h"
#include "interrupt.h"
#include "position.h"
#include "controller.h"
#include "interface.h"
#include "coordinate.h"
#include "check.h"
int MoveToTarget(const double (&target_mm)[3], WorkState &state);
///setup関連を集めた
void Setups();
void Tests();
Interface in(USBTX, USBRX);
int main()
{
    DEBUG("setup start\r\n");
    Setups();
    //スタート指示受付
    //while(CanStart() == 0) {};
    DEBUG("main start");
    InterruptSetup();
    WorkState state;
    state.color = YELLOW;
    state.areaname = WORKAREA;
    GoToZeroSlowly();
    

    double posi[3] = {200,200,50};
    SetNextRadRelative(0, -90*kDegreeToRad);
    SetNextRadRelative(1, 150*kDegreeToRad);
    SetNextRadRelative(3, 0*kDegreeToRad);
    SetServoYawRad(0);
    ServoMoveOnArm();
    wait(1);
    ServoMoveOnArm();
    while(1) {
        /*
        while(in.AskPermission("change state",'y','n')) {
            state.color = (Color)in.AskNum("color YELLOW : 0, RED : 1");
            state.areaname = (AreaName) in.AskNum("areaname Work:0 common : 1 box :2 ");
        }d
        */
        while(in.AskPermission("change target",'y','n')) {
            for (int i = 0; i < 3; i++) posi[i]= in.AskNum("posi");
        }
        MoveToTarget(posi, state);
        DEBUG("finish\r\n");
        wait(0.5);
        DEBUG("now x %f\r\n", GetNowTipLocateX());
        DEBUG("now y %f\r\n", GetNowTipLocateY());
        DEBUG("now z %f\r\n", GetNowTipLocateZ());
    }
}
void Setups()
{
    GoSetup();
    SetupPosition();

}
void Tests()
{
    static int count = 0;
    count ++;
    if (count > 100000) {
        count =0;
    }
}

#endif