File content as of revision 20:13934809e117:

#include "mbed.h"
#include "state.h"
#include "workinfo.h"
#include "debug.h"
#include "define_right_or_left.h"
#include "coordinate.h"
//全部右で考えてる
//穴の数
const int kWorkAreaNum = 6;
const int kCommonAreaNum = 17;
const int kBoxNum = 12;

//////////////////////////////変更出来るパラメータ
//priority 取る順番を決める
const int kWorkAreaPriority[kWorkAreaNum] = {
    103, 102, 0,
    102, 101, 100,
};
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] = {
    {0,0,0},{0,0,0},{0,0,0},
    {0,0,0},{0,0,0},{0,0,0},
};
const int kCommonAreaCorrect[kCommonAreaNum][3] = {
    {0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},
    {0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},
    {0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},
};
const int kBoxCorrect[kBoxNum][3] = {
    {0,0,0},{0,0,0},{0,0,0},
    {0,0,0},{0,0,0},{0,0,0},
    {0,0,0},{0,0,0},{0,0,0},
    {0,0,0},{0,0,0},{0,0,0},
};
const int kHandCenter = 22.80;//想定先端と実際の先端位置のずれ

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

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

WorkPosi work[kWorkAreaNum + kCommonAreaNum + 1];//最後の1つは番兵
BoxPosi shootingbox[kBoxNum];
Pattern now_pattern = NOPATTERN;
//ワークエリアの配列のいれかた
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,
};

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


void SetupPosition()
{
    DEBUG("SetupPosition() start\r\n");
    ExistReset();
    AreaNameSetup();
    WorkPosition();
    ShootingBoxPosition();
    Priority();
    DEBUG("SetupPosition() finish\r\n");
}
double GetIdealYawRad(const 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(const 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 WorkPosition()
{
    DEBUG("WorkPosition() start\r\n");
//ワークエリア
    //x座標
    for(int i=0; i<3; i++) {
        work[i].position[0] = -630 + 180 * i;
        work[i + 3].position[0] = -540 + 180 * i;
    }
    //handの先の値となるよう修正
    for(int i = 0; i < kWorkAreaNum; i++) work[i].position[0] += kHandCenter;
    //y座標
    for(int i=0; i<3; i++) {
        work[i].position[1]=140;
        work[i+3].position[1]=370;
    }
    //z座標
    for(int i=0; i<6; i++) work[i].position[2]=-350;
//共通ワークエリア
    //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;
    for(int i = kCommonStart; i < kCommonStart + kCommonAreaNum; i++) work[i].position[0] += kHandCenter;
    //y座標
    for(int i=kCommonStart; i<sizeof(work)/sizeof(work[0]); i++) {
        if(i<12) {
            work[i].position[1]=595+30;
        } else if(i<17) {
            work[i].position[1]=675+30;
        } else if(i< kCommonStart + kCommonAreaNum) {
            work[i].position[1]=755+30;
        }
    }
    //z座標
    for(int i=kCommonStart; i<kCommonStart + kCommonAreaNum; i++) work[i].position[2]=-350;
    for(int i = kWorkStart; i < kWorkAreaNum; i++) {
        for(int j = 0; j < 3; j++) work[kWorkAreaIndex[i]].position[j] += kWorkAreaCorrect[i][j];
    }
    for(int i = kCommonStart; i < kCommonAreaNum; i++) {
        for(int j = 0; j < 3; j++) work[kCommonAreaIndex[i]].position[j] += kCommonAreaCorrect[i][j];
    }
    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;
    }
    for(int i = 0; i < kBoxNum; i++) shootingbox[i].position[0] += kHandCenter;
    //y座標
    for(int i=0; i<4; i++) {
        shootingbox[i*3].position[1]=160+80*i+30;
        shootingbox[i*3+1].position[1]=160+80*i+30;
        shootingbox[i*3+2].position[1]=160+80*i+30;
    }
    //z座標
    for(int i=0; i<sizeof(shootingbox)/sizeof(shootingbox[0]); i++) shootingbox[i].position[2]=95;
    for(int i = 0; i < kBoxNum; i++) {
        for(int j = 0; j < 3; j++) work[kBoxIndex[i]].position[j] += kBoxCorrect[i][j];
    }
    DEBUG("ShootingBoxPosition() 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;
#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;
#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");
}