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