Diff: workinfo/workinfo.cpp

Revision:

20:13934809e117

Child:

23:f45eb02433a5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/workinfo/workinfo.cpp	Mon Aug 27 03:47:21 2018 +0000
@@ -0,0 +1,376 @@
+#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");
+}
\ No newline at end of file