File content as of revision 41:7e3b95c2fcac:

#include "mbed.h"
#include "state.h"
#include "workinfo.h"
#include "debug.h"
#include "coordinate.h"
//全部右で考えてる
//穴の数
const int kWorkAreaNum = 6;
const int kCommonAreaNum = 17;
const int kBoxNum = 12;
//ワークエリアの配列のいれかた
int kWorkAreaIndex[kWorkAreaNum] = {
    3, 4, 5,
    0, 1, 2,
};
//共通ワークエリア
const int kCommonAreaIndex[kCommonAreaNum] = {
    17, 18, 19, 20, 21, 22,
    12, 13, 14, 15, 16,
    6,  7,  8,  9, 10, 11,
};
//shootingboxの配列のいれかた
const int kBoxIndex[kBoxNum] = {
    11, 10,  9,
    8,  7,  6,
    5,  4,  3,
    2,  1,  0,
};
//////////////////////////////変更出来るパラメータ
//priority 取る順番を決める
const int kWorkAreaPriority[kWorkAreaNum] = {
    102, 101, 100,
    105, 104, 103,
};
const int kCommonAreaPriority[kCommonAreaNum] = {
    17, 16, 15, 14, 13, 12,
    11, 10, 9, 8, 7,
    6, 5, 4, 3, 2, 1,
};
//workareaの補正値
const int kWorkAreaCorrect[kWorkAreaNum][3] = {
#ifdef RIGHT
    {0,-20,10},{0,0,10},{30,0,10},
    {0+20,0+30,100},{0,-20,0},{0,0,0},
#elif defined LEFT
    {0,-20,10},{0,0,10},{30,0,50},
    {0+20,0+30,100},{0,-20,0},{0,0,0},
#endif

};
const int kCommonAreaCorrect[kCommonAreaNum][3] = {
#ifdef RIGHT
    {0,20,0},{0,0,0},{0,40,0},{-10,50,0},{-10,30,0},{-30,50,0},
    {0,50,0},{0,50,0},{0,50,0},{0,50,0},{0,50,0},
    {10,50,0},{0,50,0},{30,50,0},{20,50,0},{0,50,0},{-10,50,0},
#elif defined LEFT
    {0,20,0},{0,0,0},{0,40,0},{-10,50,0},{-10,30,0},{-30,50,0},
    {0,50,0},{0,50,0},{0,50,0},{0,50,0},{0,50,0},
    {10,50,0},{0,50,0},{30,50,0},{20,50,0},{0,50,0},{-10,50,0},
#endif
};
const int kBoxCorrect[kBoxNum][3] = {
#ifdef RIGHT
    {0,0,0},{0,0,0},{0,0,0},
    {0,0,0},{0,0,0},{-20,30,0},
    {0,0,0},{0,0,0},{-20,30,0},
    {0,0,0},{0,0,0},{0,0,0},
#elif defined LEFT
    {0,0,0},{0,0,0},{0,0,0},
    {0,0,0},{0,0,0},{-20,30,0},
    {0,0,0},{0,0,0},{-20,30,0},
    {0,0,0},{0,0,0},{0,0,0},
#endif

};
const int kHandCenter = 22;//想定先端と実際の先端位置のずれ

const int kYawWorkAreaRedDegree = 270;
const int kYawWorkAreaYellowDegree = 90;
const int kYawCommonAreaDegree = 90;
const int kYawBoxDegree = 90;

const int kPitchWorkAreaDegree = -90;
const int kPitchCommonAreaDegree = -5;
const int kPitchBoxDegree = 10;
//////////////////////////////
//配列のスタート番号
const int kWorkStart = 0;
const int kCommonStart = kWorkAreaNum;

WorkPosi work[kWorkAreaNum + kCommonAreaNum + 1];//最後の1つは番兵
BoxPosi shootingbox[kBoxNum];
Pattern now_pattern = NOPATTERN;


//関数名
void AreaNameSetup();//areanameを入れる。
void ShootingBoxPosition();//シューティングボックスにおける穴の座標を代入している
void WorkPosition();//ワークエリア、共通ワークエリアにおける穴の座標を代入している
void Priority();//ワークエリア、共通ワークエリアにおける穴の優先度を決めてる
void ArrangementPattern(Pattern pattern);
void ShootingBoxStuff();
void ExistReset();//is_existとcolorをリセット

void WorkPosition()
{
    DEBUG("WorkPosition() start\r\n");
//ワークエリア
    //x座標
    for(int i=0; i<3; i++) {
#ifdef RIGHT
        work[i].position[0] = -630 + 180 * i;
        work[i + 3].position[0] = -540 + 180 * i;
#elif defined LEFT
        work[i].position[0] = -540 + 180 * i;
        work[i + 3].position[0] = -630 + 180 * i;
#else
        DEBUG("error:There is no defined RIGHT/LEFT in WorkPosition\r\n");
#endif
    }
    //y座標
    for(int i=0; i<3; i++) {
        work[i].position[1]= 300 - 150;
        work[i+3].position[1]= 210 + 150;
    }
    //z座標
    for(int i=0; i<6; i++) work[i].position[2]=-420;
//共通ワークエリア
    //x座標
    for(int i = kCommonStart; i<12; i++) {
        work[i].position[0]=-595+(i-kCommonStart)*80;
        work[i+11].position[0]=-595+(i-kCommonStart)*80;
    }
    for(int i=12; i<17; i++) work[i].position[0]=-555+(i-12)*80;
    //y座標
    for(int i=kCommonStart; i<sizeof(work)/sizeof(work[0]); i++) {
        if(i < 12) {
            work[i].position[1]=595 + kHandCenter;
        } else if(i < 17) {
            work[i].position[1]=675 + kHandCenter;
        } else if(i < kCommonStart + kCommonAreaNum) {
            work[i].position[1]=755 + kHandCenter;
        }
    }
    //z座標
    for(int i=kCommonStart; i<kCommonStart + kCommonAreaNum; i++) work[i].position[2]=-350;
    for(int i = 0; i < kWorkAreaNum; i++) {
        for(int j = 0; j < 3; j++) work[kWorkAreaIndex[i]].position[j] += kWorkAreaCorrect[i][j];
    }
    for(int i = 0; i < kCommonAreaNum; i++) {
        for(int j = 0; j < 3; j++) work[kCommonAreaIndex[i]].position[j] += kCommonAreaCorrect[i][j];
        DEBUG("work %d %f,%f,%f\r\n",kCommonAreaIndex[i], work[kCommonAreaIndex[i]].position[0], work[kCommonAreaIndex[i]].position[1], work[kCommonAreaIndex[i]].position[2] );
    }
    DEBUG("WorkPosition() finish\r\n");
}
void ShootingBoxPosition()
{
    DEBUG("ShootingBoxPosition() start\r\n");
    //軸は一番右下の穴を原点に取っている.
    //x座標
    for(int i=0; i<3; i++) {
        shootingbox[i].position[0]=240-80*i;
        shootingbox[i+3].position[0]=240-80*i;
        shootingbox[i+6].position[0]=240-80*i;
        shootingbox[i+9].position[0]=240-80*i;
    }
    //y座標
    for(int i=0; i<4; i++) {
        shootingbox[i*3].position[1]=160+80*i;
        shootingbox[i*3+1].position[1]=160+80*i;
        shootingbox[i*3+2].position[1]=160+80*i;
    }
    for(int i = 0; i < kBoxNum; i++) shootingbox[i].position[1] += kArmLength[3] *(1 - cos(kPitchBoxDegree * kDegreeToRad));
    //z座標
    for(int i=0; i<sizeof(shootingbox)/sizeof(shootingbox[0]); i++) shootingbox[i].position[2]=125 + kArmLength[3] * sin(kPitchBoxDegree * kDegreeToRad) + 40;
    for(int i = 0; i < kBoxNum; i++) {
        for(int j = 0; j < 3; j++) shootingbox[kBoxIndex[i]].position[j] += kBoxCorrect[i][j];
    }
    DEBUG("ShootingBoxPosition() finish\r\n");
}

void SetupPosition()
{
    DEBUG("SetupPosition() start\r\n");
    ExistReset();
    AreaNameSetup();
    WorkPosition();
    ShootingBoxPosition();
    Priority();
    DEBUG("SetupPosition() finish\r\n");
}
double GetIdealYawRad(WorkState state)
{
    double degree = 0;
    switch(state.areaname) {
        case BOX:
            degree = kYawBoxDegree;
            break;
        case COMMONAREA:
            degree = kYawCommonAreaDegree;
            break;
        case WORKAREA:
            switch(state.color) {
                case RED:
                    degree = kYawWorkAreaRedDegree;
                    break;
                case YELLOW:
                    degree = kYawWorkAreaYellowDegree;
                    break;
            }
            break;
    }
    return degree * kDegreeToRad;
}
double GetIdealPitchRad(WorkState state)
{
    double degree = 0;
    switch(state.areaname) {
        case BOX:
            degree = kPitchBoxDegree;
            break;
        case COMMONAREA:
            degree = kPitchCommonAreaDegree;
            break;
        case WORKAREA:
            degree = kPitchWorkAreaDegree;
            break;
    }
    return degree * kDegreeToRad;
}
void YawPitchSetup()
{
    DEBUG("YawPitchSetup() start\r\n");
    for(int i = 0; i < kWorkAreaNum+ kCommonAreaNum; i++) {
        work[i].yaw_rad = GetIdealYawRad(work[i]);
        work[i].pitch_rad = GetIdealPitchRad(work[i]);
    }
    for(int i = 0; i < kBoxNum; i++) {
        shootingbox[i].yaw_rad = GetIdealYawRad(shootingbox[i]);
        shootingbox[i].pitch_rad = GetIdealPitchRad(shootingbox[i]);
    }
    DEBUG("YawPitchSetup() finish\r\n");
}
//areanameをセット。BOXは初期段階で入っている。
void AreaNameSetup()
{
    DEBUG("AreaNameSetup() start\r\n");
    for(int i = kWorkStart; i < kWorkAreaNum; i++) work[i].areaname = WORKAREA;
    for(int i = kCommonStart; i < kCommonStart + kCommonAreaNum; i++) work[i].areaname = COMMONAREA;
    DEBUG("AreaNameSetup() finish\r\n");
}
void Priority()
{
    DEBUG("Priority() start\r\n");
    work[23].priority=1000;//SearchCommonArea で使用。最初の時の比較に使用.最後だということを示す。
    for(int i = kWorkStart; i < kWorkAreaNum; i++) work[kWorkAreaIndex[i]].priority = kWorkAreaPriority[i];
    for(int i = 0; i < kCommonAreaNum; i++) work[kCommonAreaIndex[i]].priority = kCommonAreaPriority[i];
    DEBUG("Priority() finish\r\n");
}
void ArrangementPattern(Pattern pattern)//引数にはAとかで入力//初めに一回だけ呼び出すことを想定
{
    DEBUG("ArrangementPattern() start\r\n");
    //workareaの設定
    work[0].color=RED;
    work[1].color=RED;
    work[2].color=RED;
    work[3].color=YELLOW;
    work[4].color=YELLOW;
    work[5].color=YELLOW;


    switch(pattern) {
        case A:
            DEBUG("ArrangementPattern() A\r\n");
#ifdef RIGHT
            DEBUG("right\r\n");
            work[6].color=RED;
            work[13].color=RED;
            work[20].color=RED;
            work[8].color=YELLOW;
            work[15].color=YELLOW;
            work[22].color=YELLOW;

            work[6].is_exist=1;
            work[13].is_exist=1;
            work[20].is_exist=1;
            work[8].is_exist=1;
            work[15].is_exist=1;
            work[22].is_exist=1;

#elif defined LEFT
            DEBUG("left\r\n");
            work[9].color=RED;
            work[13].color=RED;
            work[17].color=RED;
            work[11].color=YELLOW;
            work[15].color=YELLOW;
            work[19].color=YELLOW;

            work[9].is_exist=1;
            work[13].is_exist=1;
            work[17].is_exist=1;
            work[11].is_exist=1;
            work[15].is_exist=1;
            work[19].is_exist=1;
#else
            DEBUG("error:There is no defined RIGHT/LEFT in ArrangementPattern\r\n");
#endif
            break;

        case B:
            DEBUG("ArrangementPattern() B\r\n");
#ifdef RIGHT
            DEBUG("right\r\n");
            work[7].color=RED;
            work[9].color=RED;
            work[11].color=RED;
            work[17].color=YELLOW;
            work[19].color=YELLOW;
            work[21].color=YELLOW;

            work[7].is_exist=1;
            work[9].is_exist=1;
            work[11].is_exist=1;
            work[17].is_exist=1;
            work[19].is_exist=1;
            work[21].is_exist=1;

#elif defined LEFT
            DEBUG("left\r\n");
            work[18].color=RED;
            work[20].color=RED;
            work[22].color=RED;
            work[6].color=YELLOW;
            work[8].color=YELLOW;
            work[10].color=YELLOW;

            work[18].is_exist=1;
            work[20].is_exist=1;
            work[22].is_exist=1;
            work[6].is_exist=1;
            work[8].is_exist=1;
            work[10].is_exist=1;
#else
            DEBUG("error:There is no defined RIGHT/LEFT in ArrangementPattern\r\n");
#endif
            break;

        case C:
            DEBUG("ArrangementPattern() C\r\n");
            work[15].color=RED;
            work[22].color=RED;
            work[11].color=RED;
            work[19].color=YELLOW;
            work[12].color=YELLOW;
            work[8].color=YELLOW;

            work[15].is_exist=1;
            work[22].is_exist=1;
            work[11].is_exist=1;
            work[19].is_exist=1;
            work[12].is_exist=1;
            work[8].is_exist=1;
            break;

        case D:
            DEBUG("ArrangementPattern() D\r\n");
            work[22].color=RED;
            work[15].color=RED;
            work[11].color=RED;
            work[17].color=YELLOW;
            work[13].color=YELLOW;
            work[6].color=YELLOW;

            work[22].is_exist=1;
            work[15].is_exist=1;
            work[11].is_exist=1;
            work[17].is_exist=1;
            work[13].is_exist=1;
            work[6].is_exist=1;
            break;

        default:
            break;
    }
    DEBUG("ArrangementPattern() finish\r\n");
}
void ExistReset()
{
    DEBUG("ExistReset() start\r\n");
    ///初期化してる。いったん全部存在してないことにする//要はリセット
    for(int i=kCommonStart; i < kCommonStart + kCommonAreaNum; i++) {
        work[i].is_exist=0;//共通ワークエリアの初期化
        work[i].color = NOCOLOR;
    }
    for(int i=kWorkStart; i < kWorkAreaNum; i++) { //初期化してる。ワークエリアの初期化
        work[i].is_exist=1;
    }
    for(int i=0; i<12; i++) {//shootingの初期値
        shootingbox[i].is_exist=0;
        shootingbox[i].color = NOCOLOR;
    }
    DEBUG("ExistReset() finish\r\n");
}