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