yuto kawamura / Mbed 2 deprecated cat18_operate_last

yei

Dependencies:   interface mbed enc_1multi calPID motorout KondoServoLibrary

Fork of cat18_operate by Catch the GIANT Caplico!

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?

UserRevisionLine numberNew 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 }