![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
yei
Dependencies: interface mbed enc_1multi calPID motorout KondoServoLibrary
Fork of cat18_operate by
workinfo/workinfo.cpp@45:51ab3ca47228, 2018-09-09 (annotated)
- Committer:
- yuto17320508
- Date:
- Sun Sep 09 00:18:39 2018 +0000
- Revision:
- 45:51ab3ca47228
- Parent:
- 44:d02b2165c4ed
a;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimizuta | 20:13934809e117 | 1 | #include "mbed.h" |
shimizuta | 20:13934809e117 | 2 | #include "state.h" |
shimizuta | 20:13934809e117 | 3 | #include "workinfo.h" |
shimizuta | 20:13934809e117 | 4 | #include "debug.h" |
shimizuta | 20:13934809e117 | 5 | #include "coordinate.h" |
shimizuta | 20:13934809e117 | 6 | //全部右で考えてる |
shimizuta | 20:13934809e117 | 7 | //穴の数 |
shimizuta | 20:13934809e117 | 8 | const int kWorkAreaNum = 6; |
shimizuta | 20:13934809e117 | 9 | const int kCommonAreaNum = 17; |
shimizuta | 20:13934809e117 | 10 | const int kBoxNum = 12; |
shimizuta | 23:f45eb02433a5 | 11 | //ワークエリアの配列のいれかた |
shimizuta | 23:f45eb02433a5 | 12 | int kWorkAreaIndex[kWorkAreaNum] = { |
shimizuta | 23:f45eb02433a5 | 13 | 3, 4, 5, |
shimizuta | 23:f45eb02433a5 | 14 | 0, 1, 2, |
shimizuta | 23:f45eb02433a5 | 15 | }; |
shimizuta | 23:f45eb02433a5 | 16 | //共通ワークエリア |
shimizuta | 23:f45eb02433a5 | 17 | const int kCommonAreaIndex[kCommonAreaNum] = { |
shimizuta | 23:f45eb02433a5 | 18 | 17, 18, 19, 20, 21, 22, |
shimizuta | 23:f45eb02433a5 | 19 | 12, 13, 14, 15, 16, |
shimizuta | 23:f45eb02433a5 | 20 | 6, 7, 8, 9, 10, 11, |
shimizuta | 23:f45eb02433a5 | 21 | }; |
shimizuta | 23:f45eb02433a5 | 22 | //shootingboxの配列のいれかた |
shimizuta | 23:f45eb02433a5 | 23 | const int kBoxIndex[kBoxNum] = { |
shimizuta | 23:f45eb02433a5 | 24 | 11, 10, 9, |
shimizuta | 23:f45eb02433a5 | 25 | 8, 7, 6, |
shimizuta | 23:f45eb02433a5 | 26 | 5, 4, 3, |
shimizuta | 23:f45eb02433a5 | 27 | 2, 1, 0, |
shimizuta | 23:f45eb02433a5 | 28 | }; |
shimizuta | 20:13934809e117 | 29 | //////////////////////////////変更出来るパラメータ |
shimizuta | 20:13934809e117 | 30 | //priority 取る順番を決める |
shimizuta | 20:13934809e117 | 31 | const int kWorkAreaPriority[kWorkAreaNum] = { |
yuto17320508 | 32:21229c96144a | 32 | 102, 101, 100, |
shimizuta | 38:9bed85d32c97 | 33 | 105, 104, 103, |
shimizuta | 20:13934809e117 | 34 | }; |
shimizuta | 20:13934809e117 | 35 | const int kCommonAreaPriority[kCommonAreaNum] = { |
shimizuta | 20:13934809e117 | 36 | 17, 16, 15, 14, 13, 12, |
shimizuta | 20:13934809e117 | 37 | 11, 10, 9, 8, 7, |
shimizuta | 20:13934809e117 | 38 | 6, 5, 4, 3, 2, 1, |
shimizuta | 20:13934809e117 | 39 | }; |
shimizuta | 20:13934809e117 | 40 | //workareaの補正値 |
shimizuta | 20:13934809e117 | 41 | const int kWorkAreaCorrect[kWorkAreaNum][3] = { |
yuto17320508 | 41:7e3b95c2fcac | 42 | #ifdef RIGHT |
yuto17320508 | 44:d02b2165c4ed | 43 | {0,0,-5},{0,10,-5},{-20,10,-3}, |
yuto17320508 | 44:d02b2165c4ed | 44 | {0+20,0+30,0},{0,-20,0},{0,0,0}, |
yuto17320508 | 41:7e3b95c2fcac | 45 | #elif defined LEFT |
shimizuta | 42:8d6ac9b45df3 | 46 | {0,-20,10},{0,0,10},{0,0,0}, |
yuto17320508 | 44:d02b2165c4ed | 47 | {0+20,0+30,0},{0,-20,0},{0,0,0}, |
yuto17320508 | 41:7e3b95c2fcac | 48 | #endif |
yuto17320508 | 41:7e3b95c2fcac | 49 | |
shimizuta | 20:13934809e117 | 50 | }; |
shimizuta | 20:13934809e117 | 51 | const int kCommonAreaCorrect[kCommonAreaNum][3] = { |
yuto17320508 | 41:7e3b95c2fcac | 52 | #ifdef RIGHT |
yuto17320508 | 44:d02b2165c4ed | 53 | {0,0,0},{0,0,0},{0,0,0},{-10,0,0},{-10,30,0},{-30,10,0}, |
yuto17320508 | 41:7e3b95c2fcac | 54 | {0,50,0},{0,50,0},{0,50,0},{0,50,0},{0,50,0}, |
yuto17320508 | 45:51ab3ca47228 | 55 | {60,50,20},{0,50,0},{30,50,20},{20,50,0},{0,50,0},{-10,50,0}, |
yuto17320508 | 41:7e3b95c2fcac | 56 | #elif defined LEFT |
yuto17320508 | 41:7e3b95c2fcac | 57 | {0,20,0},{0,0,0},{0,40,0},{-10,50,0},{-10,30,0},{-30,50,0}, |
yuto17320508 | 41:7e3b95c2fcac | 58 | {0,50,0},{0,50,0},{0,50,0},{0,50,0},{0,50,0}, |
yuto17320508 | 41:7e3b95c2fcac | 59 | {10,50,0},{0,50,0},{30,50,0},{20,50,0},{0,50,0},{-10,50,0}, |
yuto17320508 | 41:7e3b95c2fcac | 60 | #endif |
shimizuta | 20:13934809e117 | 61 | }; |
shimizuta | 20:13934809e117 | 62 | const int kBoxCorrect[kBoxNum][3] = { |
yuto17320508 | 41:7e3b95c2fcac | 63 | #ifdef RIGHT |
yuto17320508 | 41:7e3b95c2fcac | 64 | {0,0,0},{0,0,0},{0,0,0}, |
yuto17320508 | 41:7e3b95c2fcac | 65 | {0,0,0},{0,0,0},{-20,30,0}, |
yuto17320508 | 41:7e3b95c2fcac | 66 | {0,0,0},{0,0,0},{-20,30,0}, |
yuto17320508 | 41:7e3b95c2fcac | 67 | {0,0,0},{0,0,0},{0,0,0}, |
yuto17320508 | 41:7e3b95c2fcac | 68 | #elif defined LEFT |
yuto17320508 | 41:7e3b95c2fcac | 69 | {0,0,0},{0,0,0},{0,0,0}, |
yuto17320508 | 41:7e3b95c2fcac | 70 | {0,0,0},{0,0,0},{-20,30,0}, |
yuto17320508 | 41:7e3b95c2fcac | 71 | {0,0,0},{0,0,0},{-20,30,0}, |
yuto17320508 | 41:7e3b95c2fcac | 72 | {0,0,0},{0,0,0},{0,0,0}, |
yuto17320508 | 41:7e3b95c2fcac | 73 | #endif |
yuto17320508 | 41:7e3b95c2fcac | 74 | |
shimizuta | 20:13934809e117 | 75 | }; |
shimizuta | 29:ea48140fc85e | 76 | const int kHandCenter = 22;//想定先端と実際の先端位置のずれ |
shimizuta | 20:13934809e117 | 77 | |
shimizuta | 20:13934809e117 | 78 | const int kYawWorkAreaRedDegree = 270; |
shimizuta | 20:13934809e117 | 79 | const int kYawWorkAreaYellowDegree = 90; |
shimizuta | 20:13934809e117 | 80 | const int kYawCommonAreaDegree = 90; |
shimizuta | 20:13934809e117 | 81 | const int kYawBoxDegree = 90; |
shimizuta | 20:13934809e117 | 82 | |
shimizuta | 20:13934809e117 | 83 | const int kPitchWorkAreaDegree = -90; |
shimizuta | 28:bcfc84b481bc | 84 | const int kPitchCommonAreaDegree = -5; |
shimizuta | 38:9bed85d32c97 | 85 | const int kPitchBoxDegree = 10; |
shimizuta | 20:13934809e117 | 86 | ////////////////////////////// |
shimizuta | 20:13934809e117 | 87 | //配列のスタート番号 |
shimizuta | 20:13934809e117 | 88 | const int kWorkStart = 0; |
shimizuta | 20:13934809e117 | 89 | const int kCommonStart = kWorkAreaNum; |
shimizuta | 20:13934809e117 | 90 | |
shimizuta | 20:13934809e117 | 91 | WorkPosi work[kWorkAreaNum + kCommonAreaNum + 1];//最後の1つは番兵 |
shimizuta | 20:13934809e117 | 92 | BoxPosi shootingbox[kBoxNum]; |
shimizuta | 20:13934809e117 | 93 | Pattern now_pattern = NOPATTERN; |
shimizuta | 23:f45eb02433a5 | 94 | |
shimizuta | 20:13934809e117 | 95 | |
shimizuta | 20:13934809e117 | 96 | //関数名 |
shimizuta | 20:13934809e117 | 97 | void AreaNameSetup();//areanameを入れる。 |
shimizuta | 20:13934809e117 | 98 | void ShootingBoxPosition();//シューティングボックスにおける穴の座標を代入している |
shimizuta | 20:13934809e117 | 99 | void WorkPosition();//ワークエリア、共通ワークエリアにおける穴の座標を代入している |
shimizuta | 20:13934809e117 | 100 | void Priority();//ワークエリア、共通ワークエリアにおける穴の優先度を決めてる |
shimizuta | 20:13934809e117 | 101 | void ArrangementPattern(Pattern pattern); |
shimizuta | 20:13934809e117 | 102 | void ShootingBoxStuff(); |
shimizuta | 20:13934809e117 | 103 | void ExistReset();//is_existとcolorをリセット |
shimizuta | 20:13934809e117 | 104 | |
yoka06 | 26:5e292422cc08 | 105 | void WorkPosition() |
yoka06 | 26:5e292422cc08 | 106 | { |
yoka06 | 26:5e292422cc08 | 107 | DEBUG("WorkPosition() start\r\n"); |
yoka06 | 26:5e292422cc08 | 108 | //ワークエリア |
yoka06 | 26:5e292422cc08 | 109 | //x座標 |
yoka06 | 26:5e292422cc08 | 110 | for(int i=0; i<3; i++) { |
shimizuta | 29:ea48140fc85e | 111 | #ifdef RIGHT |
yoka06 | 26:5e292422cc08 | 112 | work[i].position[0] = -630 + 180 * i; |
yoka06 | 26:5e292422cc08 | 113 | work[i + 3].position[0] = -540 + 180 * i; |
shimizuta | 29:ea48140fc85e | 114 | #elif defined LEFT |
shimizuta | 29:ea48140fc85e | 115 | work[i].position[0] = -540 + 180 * i; |
shimizuta | 29:ea48140fc85e | 116 | work[i + 3].position[0] = -630 + 180 * i; |
shimizuta | 29:ea48140fc85e | 117 | #else |
shimizuta | 29:ea48140fc85e | 118 | DEBUG("error:There is no defined RIGHT/LEFT in WorkPosition\r\n"); |
shimizuta | 29:ea48140fc85e | 119 | #endif |
yoka06 | 26:5e292422cc08 | 120 | } |
yoka06 | 26:5e292422cc08 | 121 | //y座標 |
yoka06 | 26:5e292422cc08 | 122 | for(int i=0; i<3; i++) { |
shimizuta | 28:bcfc84b481bc | 123 | work[i].position[1]= 300 - 150; |
shimizuta | 30:af136b83b202 | 124 | work[i+3].position[1]= 210 + 150; |
yoka06 | 26:5e292422cc08 | 125 | } |
yoka06 | 26:5e292422cc08 | 126 | //z座標 |
shimizuta | 30:af136b83b202 | 127 | for(int i=0; i<6; i++) work[i].position[2]=-420; |
yoka06 | 26:5e292422cc08 | 128 | //共通ワークエリア |
yoka06 | 26:5e292422cc08 | 129 | //x座標 |
shimizuta | 29:ea48140fc85e | 130 | for(int i = kCommonStart; i<12; i++) { |
yoka06 | 26:5e292422cc08 | 131 | work[i].position[0]=-595+(i-kCommonStart)*80; |
yoka06 | 26:5e292422cc08 | 132 | work[i+11].position[0]=-595+(i-kCommonStart)*80; |
yoka06 | 26:5e292422cc08 | 133 | } |
yoka06 | 26:5e292422cc08 | 134 | for(int i=12; i<17; i++) work[i].position[0]=-555+(i-12)*80; |
yoka06 | 26:5e292422cc08 | 135 | //y座標 |
yoka06 | 26:5e292422cc08 | 136 | for(int i=kCommonStart; i<sizeof(work)/sizeof(work[0]); i++) { |
shimizuta | 29:ea48140fc85e | 137 | if(i < 12) { |
shimizuta | 29:ea48140fc85e | 138 | work[i].position[1]=595 + kHandCenter; |
shimizuta | 29:ea48140fc85e | 139 | } else if(i < 17) { |
shimizuta | 29:ea48140fc85e | 140 | work[i].position[1]=675 + kHandCenter; |
shimizuta | 29:ea48140fc85e | 141 | } else if(i < kCommonStart + kCommonAreaNum) { |
shimizuta | 29:ea48140fc85e | 142 | work[i].position[1]=755 + kHandCenter; |
yoka06 | 26:5e292422cc08 | 143 | } |
yoka06 | 26:5e292422cc08 | 144 | } |
yoka06 | 26:5e292422cc08 | 145 | //z座標 |
shimizuta | 28:bcfc84b481bc | 146 | for(int i=kCommonStart; i<kCommonStart + kCommonAreaNum; i++) work[i].position[2]=-350; |
yoka06 | 26:5e292422cc08 | 147 | for(int i = 0; i < kWorkAreaNum; i++) { |
yoka06 | 26:5e292422cc08 | 148 | for(int j = 0; j < 3; j++) work[kWorkAreaIndex[i]].position[j] += kWorkAreaCorrect[i][j]; |
yoka06 | 26:5e292422cc08 | 149 | } |
yoka06 | 26:5e292422cc08 | 150 | for(int i = 0; i < kCommonAreaNum; i++) { |
yoka06 | 26:5e292422cc08 | 151 | for(int j = 0; j < 3; j++) work[kCommonAreaIndex[i]].position[j] += kCommonAreaCorrect[i][j]; |
yoka06 | 26:5e292422cc08 | 152 | 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] ); |
yoka06 | 26:5e292422cc08 | 153 | } |
yoka06 | 26:5e292422cc08 | 154 | DEBUG("WorkPosition() finish\r\n"); |
yoka06 | 26:5e292422cc08 | 155 | } |
yoka06 | 26:5e292422cc08 | 156 | void ShootingBoxPosition() |
yoka06 | 26:5e292422cc08 | 157 | { |
yoka06 | 26:5e292422cc08 | 158 | DEBUG("ShootingBoxPosition() start\r\n"); |
yoka06 | 26:5e292422cc08 | 159 | //軸は一番右下の穴を原点に取っている. |
yoka06 | 26:5e292422cc08 | 160 | //x座標 |
yoka06 | 26:5e292422cc08 | 161 | for(int i=0; i<3; i++) { |
yoka06 | 26:5e292422cc08 | 162 | shootingbox[i].position[0]=240-80*i; |
yoka06 | 26:5e292422cc08 | 163 | shootingbox[i+3].position[0]=240-80*i; |
yoka06 | 26:5e292422cc08 | 164 | shootingbox[i+6].position[0]=240-80*i; |
yoka06 | 26:5e292422cc08 | 165 | shootingbox[i+9].position[0]=240-80*i; |
yoka06 | 26:5e292422cc08 | 166 | } |
yoka06 | 26:5e292422cc08 | 167 | //y座標 |
yoka06 | 26:5e292422cc08 | 168 | for(int i=0; i<4; i++) { |
shimizuta | 37:d4b711619962 | 169 | shootingbox[i*3].position[1]=160+80*i; |
shimizuta | 37:d4b711619962 | 170 | shootingbox[i*3+1].position[1]=160+80*i; |
shimizuta | 37:d4b711619962 | 171 | shootingbox[i*3+2].position[1]=160+80*i; |
yoka06 | 26:5e292422cc08 | 172 | } |
shimizuta | 37:d4b711619962 | 173 | for(int i = 0; i < kBoxNum; i++) shootingbox[i].position[1] += kArmLength[3] *(1 - cos(kPitchBoxDegree * kDegreeToRad)); |
yoka06 | 26:5e292422cc08 | 174 | //z座標 |
shimizuta | 42:8d6ac9b45df3 | 175 | for(int i=0; i<sizeof(shootingbox)/sizeof(shootingbox[0]); i++) shootingbox[i].position[2]=125 + kArmLength[3] * sin(kPitchBoxDegree * kDegreeToRad); |
yoka06 | 26:5e292422cc08 | 176 | for(int i = 0; i < kBoxNum; i++) { |
yoka06 | 26:5e292422cc08 | 177 | for(int j = 0; j < 3; j++) shootingbox[kBoxIndex[i]].position[j] += kBoxCorrect[i][j]; |
yoka06 | 26:5e292422cc08 | 178 | } |
yoka06 | 26:5e292422cc08 | 179 | DEBUG("ShootingBoxPosition() finish\r\n"); |
yoka06 | 26:5e292422cc08 | 180 | } |
shimizuta | 20:13934809e117 | 181 | |
shimizuta | 20:13934809e117 | 182 | void SetupPosition() |
shimizuta | 20:13934809e117 | 183 | { |
shimizuta | 20:13934809e117 | 184 | DEBUG("SetupPosition() start\r\n"); |
shimizuta | 20:13934809e117 | 185 | ExistReset(); |
shimizuta | 20:13934809e117 | 186 | AreaNameSetup(); |
shimizuta | 20:13934809e117 | 187 | WorkPosition(); |
shimizuta | 20:13934809e117 | 188 | ShootingBoxPosition(); |
shimizuta | 20:13934809e117 | 189 | Priority(); |
shimizuta | 20:13934809e117 | 190 | DEBUG("SetupPosition() finish\r\n"); |
shimizuta | 20:13934809e117 | 191 | } |
shimizuta | 37:d4b711619962 | 192 | double GetIdealYawRad(WorkState state) |
shimizuta | 20:13934809e117 | 193 | { |
shimizuta | 20:13934809e117 | 194 | double degree = 0; |
shimizuta | 20:13934809e117 | 195 | switch(state.areaname) { |
shimizuta | 20:13934809e117 | 196 | case BOX: |
shimizuta | 20:13934809e117 | 197 | degree = kYawBoxDegree; |
shimizuta | 20:13934809e117 | 198 | break; |
shimizuta | 20:13934809e117 | 199 | case COMMONAREA: |
shimizuta | 20:13934809e117 | 200 | degree = kYawCommonAreaDegree; |
shimizuta | 20:13934809e117 | 201 | break; |
shimizuta | 20:13934809e117 | 202 | case WORKAREA: |
shimizuta | 20:13934809e117 | 203 | switch(state.color) { |
shimizuta | 20:13934809e117 | 204 | case RED: |
shimizuta | 20:13934809e117 | 205 | degree = kYawWorkAreaRedDegree; |
shimizuta | 20:13934809e117 | 206 | break; |
shimizuta | 20:13934809e117 | 207 | case YELLOW: |
shimizuta | 20:13934809e117 | 208 | degree = kYawWorkAreaYellowDegree; |
shimizuta | 20:13934809e117 | 209 | break; |
shimizuta | 20:13934809e117 | 210 | } |
shimizuta | 20:13934809e117 | 211 | break; |
shimizuta | 20:13934809e117 | 212 | } |
shimizuta | 20:13934809e117 | 213 | return degree * kDegreeToRad; |
shimizuta | 20:13934809e117 | 214 | } |
shimizuta | 37:d4b711619962 | 215 | double GetIdealPitchRad(WorkState state) |
shimizuta | 20:13934809e117 | 216 | { |
shimizuta | 20:13934809e117 | 217 | double degree = 0; |
shimizuta | 20:13934809e117 | 218 | switch(state.areaname) { |
shimizuta | 20:13934809e117 | 219 | case BOX: |
shimizuta | 20:13934809e117 | 220 | degree = kPitchBoxDegree; |
shimizuta | 20:13934809e117 | 221 | break; |
shimizuta | 20:13934809e117 | 222 | case COMMONAREA: |
shimizuta | 20:13934809e117 | 223 | degree = kPitchCommonAreaDegree; |
shimizuta | 20:13934809e117 | 224 | break; |
shimizuta | 20:13934809e117 | 225 | case WORKAREA: |
shimizuta | 20:13934809e117 | 226 | degree = kPitchWorkAreaDegree; |
shimizuta | 20:13934809e117 | 227 | break; |
shimizuta | 20:13934809e117 | 228 | } |
shimizuta | 20:13934809e117 | 229 | return degree * kDegreeToRad; |
shimizuta | 20:13934809e117 | 230 | } |
shimizuta | 20:13934809e117 | 231 | void YawPitchSetup() |
shimizuta | 20:13934809e117 | 232 | { |
shimizuta | 20:13934809e117 | 233 | DEBUG("YawPitchSetup() start\r\n"); |
shimizuta | 20:13934809e117 | 234 | for(int i = 0; i < kWorkAreaNum+ kCommonAreaNum; i++) { |
shimizuta | 20:13934809e117 | 235 | work[i].yaw_rad = GetIdealYawRad(work[i]); |
shimizuta | 20:13934809e117 | 236 | work[i].pitch_rad = GetIdealPitchRad(work[i]); |
shimizuta | 20:13934809e117 | 237 | } |
shimizuta | 20:13934809e117 | 238 | for(int i = 0; i < kBoxNum; i++) { |
shimizuta | 20:13934809e117 | 239 | shootingbox[i].yaw_rad = GetIdealYawRad(shootingbox[i]); |
shimizuta | 20:13934809e117 | 240 | shootingbox[i].pitch_rad = GetIdealPitchRad(shootingbox[i]); |
shimizuta | 20:13934809e117 | 241 | } |
shimizuta | 20:13934809e117 | 242 | DEBUG("YawPitchSetup() finish\r\n"); |
shimizuta | 20:13934809e117 | 243 | } |
shimizuta | 20:13934809e117 | 244 | //areanameをセット。BOXは初期段階で入っている。 |
shimizuta | 20:13934809e117 | 245 | void AreaNameSetup() |
shimizuta | 20:13934809e117 | 246 | { |
shimizuta | 20:13934809e117 | 247 | DEBUG("AreaNameSetup() start\r\n"); |
shimizuta | 20:13934809e117 | 248 | for(int i = kWorkStart; i < kWorkAreaNum; i++) work[i].areaname = WORKAREA; |
shimizuta | 20:13934809e117 | 249 | for(int i = kCommonStart; i < kCommonStart + kCommonAreaNum; i++) work[i].areaname = COMMONAREA; |
shimizuta | 20:13934809e117 | 250 | DEBUG("AreaNameSetup() finish\r\n"); |
shimizuta | 20:13934809e117 | 251 | } |
shimizuta | 20:13934809e117 | 252 | void Priority() |
shimizuta | 20:13934809e117 | 253 | { |
shimizuta | 20:13934809e117 | 254 | DEBUG("Priority() start\r\n"); |
shimizuta | 20:13934809e117 | 255 | work[23].priority=1000;//SearchCommonArea で使用。最初の時の比較に使用.最後だということを示す。 |
shimizuta | 20:13934809e117 | 256 | for(int i = kWorkStart; i < kWorkAreaNum; i++) work[kWorkAreaIndex[i]].priority = kWorkAreaPriority[i]; |
shimizuta | 20:13934809e117 | 257 | for(int i = 0; i < kCommonAreaNum; i++) work[kCommonAreaIndex[i]].priority = kCommonAreaPriority[i]; |
shimizuta | 20:13934809e117 | 258 | DEBUG("Priority() finish\r\n"); |
shimizuta | 20:13934809e117 | 259 | } |
shimizuta | 20:13934809e117 | 260 | void ArrangementPattern(Pattern pattern)//引数にはAとかで入力//初めに一回だけ呼び出すことを想定 |
shimizuta | 20:13934809e117 | 261 | { |
shimizuta | 20:13934809e117 | 262 | DEBUG("ArrangementPattern() start\r\n"); |
shimizuta | 20:13934809e117 | 263 | //workareaの設定 |
shimizuta | 20:13934809e117 | 264 | work[0].color=RED; |
shimizuta | 20:13934809e117 | 265 | work[1].color=RED; |
shimizuta | 20:13934809e117 | 266 | work[2].color=RED; |
shimizuta | 20:13934809e117 | 267 | work[3].color=YELLOW; |
shimizuta | 20:13934809e117 | 268 | work[4].color=YELLOW; |
shimizuta | 20:13934809e117 | 269 | work[5].color=YELLOW; |
shimizuta | 20:13934809e117 | 270 | |
shimizuta | 20:13934809e117 | 271 | |
shimizuta | 20:13934809e117 | 272 | switch(pattern) { |
shimizuta | 20:13934809e117 | 273 | case A: |
shimizuta | 20:13934809e117 | 274 | DEBUG("ArrangementPattern() A\r\n"); |
shimizuta | 20:13934809e117 | 275 | #ifdef RIGHT |
shimizuta | 20:13934809e117 | 276 | DEBUG("right\r\n"); |
shimizuta | 20:13934809e117 | 277 | work[6].color=RED; |
shimizuta | 20:13934809e117 | 278 | work[13].color=RED; |
shimizuta | 20:13934809e117 | 279 | work[20].color=RED; |
shimizuta | 20:13934809e117 | 280 | work[8].color=YELLOW; |
shimizuta | 20:13934809e117 | 281 | work[15].color=YELLOW; |
shimizuta | 20:13934809e117 | 282 | work[22].color=YELLOW; |
shimizuta | 20:13934809e117 | 283 | |
shimizuta | 20:13934809e117 | 284 | work[6].is_exist=1; |
shimizuta | 20:13934809e117 | 285 | work[13].is_exist=1; |
shimizuta | 20:13934809e117 | 286 | work[20].is_exist=1; |
shimizuta | 20:13934809e117 | 287 | work[8].is_exist=1; |
shimizuta | 20:13934809e117 | 288 | work[15].is_exist=1; |
shimizuta | 20:13934809e117 | 289 | work[22].is_exist=1; |
shimizuta | 20:13934809e117 | 290 | |
shimizuta | 20:13934809e117 | 291 | #elif defined LEFT |
shimizuta | 20:13934809e117 | 292 | DEBUG("left\r\n"); |
shimizuta | 20:13934809e117 | 293 | work[9].color=RED; |
shimizuta | 20:13934809e117 | 294 | work[13].color=RED; |
shimizuta | 20:13934809e117 | 295 | work[17].color=RED; |
shimizuta | 20:13934809e117 | 296 | work[11].color=YELLOW; |
shimizuta | 20:13934809e117 | 297 | work[15].color=YELLOW; |
shimizuta | 20:13934809e117 | 298 | work[19].color=YELLOW; |
shimizuta | 20:13934809e117 | 299 | |
shimizuta | 20:13934809e117 | 300 | work[9].is_exist=1; |
shimizuta | 20:13934809e117 | 301 | work[13].is_exist=1; |
shimizuta | 20:13934809e117 | 302 | work[17].is_exist=1; |
shimizuta | 20:13934809e117 | 303 | work[11].is_exist=1; |
shimizuta | 20:13934809e117 | 304 | work[15].is_exist=1; |
shimizuta | 20:13934809e117 | 305 | work[19].is_exist=1; |
shimizuta | 29:ea48140fc85e | 306 | #else |
shimizuta | 29:ea48140fc85e | 307 | DEBUG("error:There is no defined RIGHT/LEFT in ArrangementPattern\r\n"); |
shimizuta | 20:13934809e117 | 308 | #endif |
shimizuta | 20:13934809e117 | 309 | break; |
shimizuta | 20:13934809e117 | 310 | |
shimizuta | 20:13934809e117 | 311 | case B: |
shimizuta | 20:13934809e117 | 312 | DEBUG("ArrangementPattern() B\r\n"); |
shimizuta | 20:13934809e117 | 313 | #ifdef RIGHT |
shimizuta | 20:13934809e117 | 314 | DEBUG("right\r\n"); |
shimizuta | 20:13934809e117 | 315 | work[7].color=RED; |
shimizuta | 20:13934809e117 | 316 | work[9].color=RED; |
shimizuta | 20:13934809e117 | 317 | work[11].color=RED; |
shimizuta | 20:13934809e117 | 318 | work[17].color=YELLOW; |
shimizuta | 20:13934809e117 | 319 | work[19].color=YELLOW; |
shimizuta | 20:13934809e117 | 320 | work[21].color=YELLOW; |
shimizuta | 20:13934809e117 | 321 | |
shimizuta | 20:13934809e117 | 322 | work[7].is_exist=1; |
shimizuta | 20:13934809e117 | 323 | work[9].is_exist=1; |
shimizuta | 20:13934809e117 | 324 | work[11].is_exist=1; |
shimizuta | 20:13934809e117 | 325 | work[17].is_exist=1; |
shimizuta | 20:13934809e117 | 326 | work[19].is_exist=1; |
shimizuta | 20:13934809e117 | 327 | work[21].is_exist=1; |
shimizuta | 20:13934809e117 | 328 | |
shimizuta | 20:13934809e117 | 329 | #elif defined LEFT |
shimizuta | 20:13934809e117 | 330 | DEBUG("left\r\n"); |
shimizuta | 20:13934809e117 | 331 | work[18].color=RED; |
shimizuta | 20:13934809e117 | 332 | work[20].color=RED; |
shimizuta | 20:13934809e117 | 333 | work[22].color=RED; |
shimizuta | 20:13934809e117 | 334 | work[6].color=YELLOW; |
shimizuta | 20:13934809e117 | 335 | work[8].color=YELLOW; |
shimizuta | 20:13934809e117 | 336 | work[10].color=YELLOW; |
shimizuta | 20:13934809e117 | 337 | |
shimizuta | 20:13934809e117 | 338 | work[18].is_exist=1; |
shimizuta | 20:13934809e117 | 339 | work[20].is_exist=1; |
shimizuta | 20:13934809e117 | 340 | work[22].is_exist=1; |
shimizuta | 20:13934809e117 | 341 | work[6].is_exist=1; |
shimizuta | 20:13934809e117 | 342 | work[8].is_exist=1; |
shimizuta | 20:13934809e117 | 343 | work[10].is_exist=1; |
shimizuta | 29:ea48140fc85e | 344 | #else |
shimizuta | 29:ea48140fc85e | 345 | DEBUG("error:There is no defined RIGHT/LEFT in ArrangementPattern\r\n"); |
shimizuta | 20:13934809e117 | 346 | #endif |
shimizuta | 20:13934809e117 | 347 | break; |
shimizuta | 20:13934809e117 | 348 | |
shimizuta | 20:13934809e117 | 349 | case C: |
shimizuta | 20:13934809e117 | 350 | DEBUG("ArrangementPattern() C\r\n"); |
shimizuta | 20:13934809e117 | 351 | work[15].color=RED; |
shimizuta | 20:13934809e117 | 352 | work[22].color=RED; |
shimizuta | 20:13934809e117 | 353 | work[11].color=RED; |
shimizuta | 20:13934809e117 | 354 | work[19].color=YELLOW; |
shimizuta | 20:13934809e117 | 355 | work[12].color=YELLOW; |
shimizuta | 20:13934809e117 | 356 | work[8].color=YELLOW; |
shimizuta | 20:13934809e117 | 357 | |
shimizuta | 20:13934809e117 | 358 | work[15].is_exist=1; |
shimizuta | 20:13934809e117 | 359 | work[22].is_exist=1; |
shimizuta | 20:13934809e117 | 360 | work[11].is_exist=1; |
shimizuta | 20:13934809e117 | 361 | work[19].is_exist=1; |
shimizuta | 20:13934809e117 | 362 | work[12].is_exist=1; |
shimizuta | 20:13934809e117 | 363 | work[8].is_exist=1; |
shimizuta | 20:13934809e117 | 364 | break; |
shimizuta | 20:13934809e117 | 365 | |
shimizuta | 20:13934809e117 | 366 | case D: |
shimizuta | 20:13934809e117 | 367 | DEBUG("ArrangementPattern() D\r\n"); |
shimizuta | 20:13934809e117 | 368 | work[22].color=RED; |
shimizuta | 20:13934809e117 | 369 | work[15].color=RED; |
shimizuta | 20:13934809e117 | 370 | work[11].color=RED; |
shimizuta | 20:13934809e117 | 371 | work[17].color=YELLOW; |
shimizuta | 20:13934809e117 | 372 | work[13].color=YELLOW; |
shimizuta | 20:13934809e117 | 373 | work[6].color=YELLOW; |
shimizuta | 20:13934809e117 | 374 | |
shimizuta | 20:13934809e117 | 375 | work[22].is_exist=1; |
shimizuta | 20:13934809e117 | 376 | work[15].is_exist=1; |
shimizuta | 20:13934809e117 | 377 | work[11].is_exist=1; |
shimizuta | 20:13934809e117 | 378 | work[17].is_exist=1; |
shimizuta | 20:13934809e117 | 379 | work[13].is_exist=1; |
shimizuta | 20:13934809e117 | 380 | work[6].is_exist=1; |
shimizuta | 20:13934809e117 | 381 | break; |
shimizuta | 20:13934809e117 | 382 | |
shimizuta | 20:13934809e117 | 383 | default: |
shimizuta | 20:13934809e117 | 384 | break; |
shimizuta | 20:13934809e117 | 385 | } |
shimizuta | 20:13934809e117 | 386 | DEBUG("ArrangementPattern() finish\r\n"); |
shimizuta | 20:13934809e117 | 387 | } |
shimizuta | 20:13934809e117 | 388 | void ExistReset() |
shimizuta | 20:13934809e117 | 389 | { |
shimizuta | 20:13934809e117 | 390 | DEBUG("ExistReset() start\r\n"); |
shimizuta | 20:13934809e117 | 391 | ///初期化してる。いったん全部存在してないことにする//要はリセット |
shimizuta | 20:13934809e117 | 392 | for(int i=kCommonStart; i < kCommonStart + kCommonAreaNum; i++) { |
shimizuta | 20:13934809e117 | 393 | work[i].is_exist=0;//共通ワークエリアの初期化 |
shimizuta | 20:13934809e117 | 394 | work[i].color = NOCOLOR; |
shimizuta | 20:13934809e117 | 395 | } |
shimizuta | 20:13934809e117 | 396 | for(int i=kWorkStart; i < kWorkAreaNum; i++) { //初期化してる。ワークエリアの初期化 |
shimizuta | 20:13934809e117 | 397 | work[i].is_exist=1; |
shimizuta | 20:13934809e117 | 398 | } |
shimizuta | 20:13934809e117 | 399 | for(int i=0; i<12; i++) {//shootingの初期値 |
shimizuta | 20:13934809e117 | 400 | shootingbox[i].is_exist=0; |
shimizuta | 20:13934809e117 | 401 | shootingbox[i].color = NOCOLOR; |
shimizuta | 20:13934809e117 | 402 | } |
shimizuta | 20:13934809e117 | 403 | DEBUG("ExistReset() finish\r\n"); |
shimizuta | 20:13934809e117 | 404 | } |