OX / Mbed 2 deprecated Ox_ver_function

hahaha

Dependencies:   mbed

main.cpp@0:a291977ec0b1, 2016-12-05 (annotated)

Committer:
arthicha
Date:
Mon Dec 05 18:31:43 2016 +0000
Revision:
0:a291977ec0b1
Child:
1:d8ce226c8c2e
Publich

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arthicha 0:a291977ec0b1 1 #include "mbed.h"
arthicha 0:a291977ec0b1 2 #include "bmu.h"
arthicha 0:a291977ec0b1 3
arthicha 0:a291977ec0b1 4
arthicha 0:a291977ec0b1 5 InterruptIn lim(D3);
arthicha 0:a291977ec0b1 6 InterruptIn rotate(D4);
arthicha 0:a291977ec0b1 7 InterruptIn choose(D5);
arthicha 0:a291977ec0b1 8 Ticker flipper;
arthicha 0:a291977ec0b1 9 Ticker Update;
arthicha 0:a291977ec0b1 10
arthicha 0:a291977ec0b1 11 bmuimu imu;
arthicha 0:a291977ec0b1 12
arthicha 0:a291977ec0b1 13 Serial pc(USBTX,USBRX);
arthicha 0:a291977ec0b1 14 Serial mas(D8,D2);
arthicha 0:a291977ec0b1 15 Serial duino(PA_11, PA_12);
arthicha 0:a291977ec0b1 16
arthicha 0:a291977ec0b1 17 Timer t;
arthicha 0:a291977ec0b1 18 Timer timer;
arthicha 0:a291977ec0b1 19
arthicha 0:a291977ec0b1 20 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 21 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 22 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 23 // variable
arthicha 0:a291977ec0b1 24 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 25 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 26 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 27 bool anach = false;
arthicha 0:a291977ec0b1 28 bool buttonState[3] = {0,0,0}; // button limit switchm rotate and choose state respectively.
arthicha 0:a291977ec0b1 29 unsigned int county = 10; // county down.
arthicha 0:a291977ec0b1 30 char data[50];
arthicha 0:a291977ec0b1 31 char elem;
arthicha 0:a291977ec0b1 32 int yyaaww;
arthicha 0:a291977ec0b1 33 bool errorie = false; // store error in ambigous imu data.
arthicha 0:a291977ec0b1 34 int helperRoll = 0, helperPitch=0,helperYaw=0;
arthicha 0:a291977ec0b1 35 unsigned int indx = 0;
arthicha 0:a291977ec0b1 36 unsigned int state = 0;
arthicha 0:a291977ec0b1 37 int dete[50];
arthicha 0:a291977ec0b1 38 int lis[50] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30};
arthicha 0:a291977ec0b1 39
arthicha 0:a291977ec0b1 40 // state 0 = initial / start.
arthicha 0:a291977ec0b1 41 // 1 = waiting for limit switch.
arthicha 0:a291977ec0b1 42
arthicha 0:a291977ec0b1 43 unsigned int turn = 0; // turn.
arthicha 0:a291977ec0b1 44
arthicha 0:a291977ec0b1 45 unsigned int x = 0, y = 0, z = 0,lx=0,ly=0,lz=0;
arthicha 0:a291977ec0b1 46 int winStatus = 0;
arthicha 0:a291977ec0b1 47
arthicha 0:a291977ec0b1 48
arthicha 0:a291977ec0b1 49
arthicha 0:a291977ec0b1 50 int A , B , C , player , button;
arthicha 0:a291977ec0b1 51 int WMbox[6];
arthicha 0:a291977ec0b1 52 int map[5][5][5],rmap[5][5][5]; //store map.
arthicha 0:a291977ec0b1 53 int Check1[5][5][5] , Check2[5][5][5]; //convert to x or o map.
arthicha 0:a291977ec0b1 54 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 55 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 56 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 57 // function
arthicha 0:a291977ec0b1 58 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 59 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 60 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 61
arthicha 0:a291977ec0b1 62 int Cross(int degree)
arthicha 0:a291977ec0b1 63 {
arthicha 0:a291977ec0b1 64 if (degree == 0)
arthicha 0:a291977ec0b1 65 {
arthicha 0:a291977ec0b1 66 return 1;
arthicha 0:a291977ec0b1 67 }else if (degree == 180)
arthicha 0:a291977ec0b1 68 {
arthicha 0:a291977ec0b1 69 return -1;
arthicha 0:a291977ec0b1 70 }else
arthicha 0:a291977ec0b1 71 {
arthicha 0:a291977ec0b1 72 return 0;
arthicha 0:a291977ec0b1 73 }
arthicha 0:a291977ec0b1 74 }
arthicha 0:a291977ec0b1 75
arthicha 0:a291977ec0b1 76 int Zine(int degree)
arthicha 0:a291977ec0b1 77 {
arthicha 0:a291977ec0b1 78 if (degree == 90)
arthicha 0:a291977ec0b1 79 {
arthicha 0:a291977ec0b1 80 return 1;
arthicha 0:a291977ec0b1 81 }else if (degree == -90)
arthicha 0:a291977ec0b1 82 {
arthicha 0:a291977ec0b1 83 return -1;
arthicha 0:a291977ec0b1 84 }else
arthicha 0:a291977ec0b1 85 {
arthicha 0:a291977ec0b1 86 return 0;
arthicha 0:a291977ec0b1 87 }
arthicha 0:a291977ec0b1 88 }
arthicha 0:a291977ec0b1 89
arthicha 0:a291977ec0b1 90 void Rise1()
arthicha 0:a291977ec0b1 91 {
arthicha 0:a291977ec0b1 92 buttonState[0] = 1;
arthicha 0:a291977ec0b1 93 }
arthicha 0:a291977ec0b1 94
arthicha 0:a291977ec0b1 95 void Fall1()
arthicha 0:a291977ec0b1 96 {
arthicha 0:a291977ec0b1 97 buttonState[0] = 0;
arthicha 0:a291977ec0b1 98 }
arthicha 0:a291977ec0b1 99 void Rise2()
arthicha 0:a291977ec0b1 100 {
arthicha 0:a291977ec0b1 101 buttonState[1] = 1;
arthicha 0:a291977ec0b1 102 }
arthicha 0:a291977ec0b1 103
arthicha 0:a291977ec0b1 104 void Fall2()
arthicha 0:a291977ec0b1 105 {
arthicha 0:a291977ec0b1 106 buttonState[1] = 0;
arthicha 0:a291977ec0b1 107 }
arthicha 0:a291977ec0b1 108 void Rise3()
arthicha 0:a291977ec0b1 109 {
arthicha 0:a291977ec0b1 110 buttonState[2] = 1;
arthicha 0:a291977ec0b1 111 }
arthicha 0:a291977ec0b1 112
arthicha 0:a291977ec0b1 113 void Fall3()
arthicha 0:a291977ec0b1 114 {
arthicha 0:a291977ec0b1 115 buttonState[2] = 0;
arthicha 0:a291977ec0b1 116 }
arthicha 0:a291977ec0b1 117
arthicha 0:a291977ec0b1 118 void UUpdate()
arthicha 0:a291977ec0b1 119 {
arthicha 0:a291977ec0b1 120 imu.update();
arthicha 0:a291977ec0b1 121 }
arthicha 0:a291977ec0b1 122
arthicha 0:a291977ec0b1 123 void Sent(char arrayi[],float it,int siz)
arthicha 0:a291977ec0b1 124 {
arthicha 0:a291977ec0b1 125 for (int i = 0;i<siz;i++)
arthicha 0:a291977ec0b1 126 {
arthicha 0:a291977ec0b1 127 mas.putc(char(arrayi[i]));
arthicha 0:a291977ec0b1 128 wait(it);
arthicha 0:a291977ec0b1 129 }
arthicha 0:a291977ec0b1 130 }
arthicha 0:a291977ec0b1 131
arthicha 0:a291977ec0b1 132 void getStr()
arthicha 0:a291977ec0b1 133 {
arthicha 0:a291977ec0b1 134 while (mas.readable())
arthicha 0:a291977ec0b1 135 {
arthicha 0:a291977ec0b1 136
arthicha 0:a291977ec0b1 137 elem = mas.getc();
arthicha 0:a291977ec0b1 138 pc.printf("char is %d\n\r",elem);
arthicha 0:a291977ec0b1 139 if ((elem == '\n'))
arthicha 0:a291977ec0b1 140 {
arthicha 0:a291977ec0b1 141 pc.printf("string is %s\n\r",data);
arthicha 0:a291977ec0b1 142 for(int i=0;i<indx;i++)
arthicha 0:a291977ec0b1 143 {
arthicha 0:a291977ec0b1 144 if (indx > 2)
arthicha 0:a291977ec0b1 145 {
arthicha 0:a291977ec0b1 146 dete[i] = data[i];
arthicha 0:a291977ec0b1 147 }
arthicha 0:a291977ec0b1 148 }
arthicha 0:a291977ec0b1 149 for(int i=0;i<30;i++)
arthicha 0:a291977ec0b1 150 {
arthicha 0:a291977ec0b1 151 data[i] = '\0';
arthicha 0:a291977ec0b1 152 }
arthicha 0:a291977ec0b1 153 indx = 0;
arthicha 0:a291977ec0b1 154 break;
arthicha 0:a291977ec0b1 155 }else
arthicha 0:a291977ec0b1 156 {
arthicha 0:a291977ec0b1 157 data[indx] = elem;
arthicha 0:a291977ec0b1 158 indx += 1;
arthicha 0:a291977ec0b1 159 }
arthicha 0:a291977ec0b1 160
arthicha 0:a291977ec0b1 161 }
arthicha 0:a291977ec0b1 162 }
arthicha 0:a291977ec0b1 163
arthicha 0:a291977ec0b1 164 void LChackmape(){ //
arthicha 0:a291977ec0b1 165 if (t.read() > 1) {
arthicha 0:a291977ec0b1 166
arthicha 0:a291977ec0b1 167
arthicha 0:a291977ec0b1 168 duino.putc(lis[23]);
arthicha 0:a291977ec0b1 169
arthicha 0:a291977ec0b1 170 timer.reset();
arthicha 0:a291977ec0b1 171 }
arthicha 0:a291977ec0b1 172 }
arthicha 0:a291977ec0b1 173
arthicha 0:a291977ec0b1 174 void LWin(){ //ชนะ
arthicha 0:a291977ec0b1 175 if ( timer.read() > 1) {
arthicha 0:a291977ec0b1 176 //pc.printf("Writing!\n\r");
arthicha 0:a291977ec0b1 177 duino.putc( lis[0]);
arthicha 0:a291977ec0b1 178 duino.putc( lis[1]);
arthicha 0:a291977ec0b1 179 duino.putc( lis[2]);
arthicha 0:a291977ec0b1 180 timer.reset();
arthicha 0:a291977ec0b1 181 }
arthicha 0:a291977ec0b1 182 }
arthicha 0:a291977ec0b1 183
arthicha 0:a291977ec0b1 184 void LCountDown(int ixxii){ //นับเวลา
arthicha 0:a291977ec0b1 185 if ( timer.read() > 1) {
arthicha 0:a291977ec0b1 186 duino.putc(ixxii);
arthicha 0:a291977ec0b1 187 timer.reset();
arthicha 0:a291977ec0b1 188 }
arthicha 0:a291977ec0b1 189 }
arthicha 0:a291977ec0b1 190
arthicha 0:a291977ec0b1 191
arthicha 0:a291977ec0b1 192 void LCorrect(){ //เช็คถูก
arthicha 0:a291977ec0b1 193 if ( timer.read() > 1) {
arthicha 0:a291977ec0b1 194 //pc.printf("Writing!\n\r");
arthicha 0:a291977ec0b1 195 duino.putc(lis[18]);
arthicha 0:a291977ec0b1 196 timer.reset();
arthicha 0:a291977ec0b1 197 }
arthicha 0:a291977ec0b1 198 }
arthicha 0:a291977ec0b1 199
arthicha 0:a291977ec0b1 200 void LError(){ //
arthicha 0:a291977ec0b1 201 if ( timer.read() > 1) {
arthicha 0:a291977ec0b1 202 //pc.printf("Writing!\n\r");
arthicha 0:a291977ec0b1 203 duino.putc( lis[19]);
arthicha 0:a291977ec0b1 204 timer.reset();
arthicha 0:a291977ec0b1 205 }
arthicha 0:a291977ec0b1 206 }
arthicha 0:a291977ec0b1 207 void LMapComplete(){ //ลงค่าเสร็จ
arthicha 0:a291977ec0b1 208 if ( timer.read() > 1) {
arthicha 0:a291977ec0b1 209 //pc.printf("Writing!\n\r");
arthicha 0:a291977ec0b1 210
arthicha 0:a291977ec0b1 211
arthicha 0:a291977ec0b1 212 duino.putc( lis[20]);
arthicha 0:a291977ec0b1 213
arthicha 0:a291977ec0b1 214
arthicha 0:a291977ec0b1 215 timer.reset();}}
arthicha 0:a291977ec0b1 216
arthicha 0:a291977ec0b1 217 void LWbox(){ //ตัวหลัก
arthicha 0:a291977ec0b1 218 if ( timer.read() > 1) {
arthicha 0:a291977ec0b1 219 duino.putc( lis[21]);
arthicha 0:a291977ec0b1 220 timer.reset();
arthicha 0:a291977ec0b1 221 }
arthicha 0:a291977ec0b1 222 }
arthicha 0:a291977ec0b1 223
arthicha 0:a291977ec0b1 224 void LRotateComplete(){ //หมุน
arthicha 0:a291977ec0b1 225 if ( timer.read() > 1) {
arthicha 0:a291977ec0b1 226 //pc.printf("Writing!\n\r");
arthicha 0:a291977ec0b1 227
arthicha 0:a291977ec0b1 228
arthicha 0:a291977ec0b1 229 duino.putc( lis[22]);
arthicha 0:a291977ec0b1 230
arthicha 0:a291977ec0b1 231
arthicha 0:a291977ec0b1 232 timer.reset();
arthicha 0:a291977ec0b1 233 }
arthicha 0:a291977ec0b1 234 }
arthicha 0:a291977ec0b1 235
arthicha 0:a291977ec0b1 236
arthicha 0:a291977ec0b1 237
arthicha 0:a291977ec0b1 238 void LLose(){ //แพ้
arthicha 0:a291977ec0b1 239
arthicha 0:a291977ec0b1 240 if ( t.read() > 1) {
arthicha 0:a291977ec0b1 241 //pc.printf("Writing!\n\r");
arthicha 0:a291977ec0b1 242 duino.putc( lis[3]);
arthicha 0:a291977ec0b1 243 duino.putc( lis[4]);
arthicha 0:a291977ec0b1 244 duino.putc( lis[5]);
arthicha 0:a291977ec0b1 245 duino.putc( lis[6]);
arthicha 0:a291977ec0b1 246 t.reset();
arthicha 0:a291977ec0b1 247 }
arthicha 0:a291977ec0b1 248 if ( duino.readable())
arthicha 0:a291977ec0b1 249 {
arthicha 0:a291977ec0b1 250 //pc.printf("recieving %c",duino.getc());
arthicha 0:a291977ec0b1 251 }
arthicha 0:a291977ec0b1 252
arthicha 0:a291977ec0b1 253 }
arthicha 0:a291977ec0b1 254
arthicha 0:a291977ec0b1 255 void Lose()
arthicha 0:a291977ec0b1 256 {
arthicha 0:a291977ec0b1 257 LLose();
arthicha 0:a291977ec0b1 258 Sent("los\n",0.1,4);
arthicha 0:a291977ec0b1 259 }
arthicha 0:a291977ec0b1 260
arthicha 0:a291977ec0b1 261 void Win()
arthicha 0:a291977ec0b1 262 {
arthicha 0:a291977ec0b1 263 LWin();
arthicha 0:a291977ec0b1 264 Sent("lwin\n",0.1,4);
arthicha 0:a291977ec0b1 265 }
arthicha 0:a291977ec0b1 266
arthicha 0:a291977ec0b1 267
arthicha 0:a291977ec0b1 268
arthicha 0:a291977ec0b1 269 bool chooseAnother()
arthicha 0:a291977ec0b1 270 {
arthicha 0:a291977ec0b1 271 Sent("cho\n",0.1,4);
arthicha 0:a291977ec0b1 272 while(1)
arthicha 0:a291977ec0b1 273 {
arthicha 0:a291977ec0b1 274 if ((dete[0]=='c')&&(dete[1]=='h')&&(dete[2]=='0'))
arthicha 0:a291977ec0b1 275 {
arthicha 0:a291977ec0b1 276 return true;
arthicha 0:a291977ec0b1 277 }else if ((dete[0]=='c')&&(dete[1]=='h')&&(dete[2]=='x'))
arthicha 0:a291977ec0b1 278 {
arthicha 0:a291977ec0b1 279 return false;
arthicha 0:a291977ec0b1 280 }
arthicha 0:a291977ec0b1 281 }
arthicha 0:a291977ec0b1 282 }
arthicha 0:a291977ec0b1 283
arthicha 0:a291977ec0b1 284 bool anotherRotate()
arthicha 0:a291977ec0b1 285 {
arthicha 0:a291977ec0b1 286 Sent("rot\n",0.1,4);
arthicha 0:a291977ec0b1 287 while(1)
arthicha 0:a291977ec0b1 288 {
arthicha 0:a291977ec0b1 289 if ((dete[0]=='r')&&(dete[1]=='o')&&(dete[2]=='t'))
arthicha 0:a291977ec0b1 290 {
arthicha 0:a291977ec0b1 291 return true;
arthicha 0:a291977ec0b1 292 }else if ((dete[0]=='r')&&(dete[1]=='o')&&(dete[2]=='x'))
arthicha 0:a291977ec0b1 293 {
arthicha 0:a291977ec0b1 294 return false;
arthicha 0:a291977ec0b1 295 }
arthicha 0:a291977ec0b1 296 }
arthicha 0:a291977ec0b1 297 }
arthicha 0:a291977ec0b1 298
arthicha 0:a291977ec0b1 299
arthicha 0:a291977ec0b1 300 int GetCoordinateX()
arthicha 0:a291977ec0b1 301 {
arthicha 0:a291977ec0b1 302 if ( WMbox[0] != -1)
arthicha 0:a291977ec0b1 303 {
arthicha 0:a291977ec0b1 304 return WMbox[0];
arthicha 0:a291977ec0b1 305 }else
arthicha 0:a291977ec0b1 306 {
arthicha 0:a291977ec0b1 307 return WMbox[3];
arthicha 0:a291977ec0b1 308 }
arthicha 0:a291977ec0b1 309 }
arthicha 0:a291977ec0b1 310
arthicha 0:a291977ec0b1 311 int GetCoordinateY()
arthicha 0:a291977ec0b1 312 {
arthicha 0:a291977ec0b1 313 if ( WMbox[1] != -1)
arthicha 0:a291977ec0b1 314 {
arthicha 0:a291977ec0b1 315 return WMbox[1];
arthicha 0:a291977ec0b1 316 }else
arthicha 0:a291977ec0b1 317 {
arthicha 0:a291977ec0b1 318 return WMbox[4];
arthicha 0:a291977ec0b1 319 }
arthicha 0:a291977ec0b1 320 }
arthicha 0:a291977ec0b1 321
arthicha 0:a291977ec0b1 322 int GetCoordinateZ()
arthicha 0:a291977ec0b1 323 {
arthicha 0:a291977ec0b1 324 if ( WMbox[2] != -1)
arthicha 0:a291977ec0b1 325 {
arthicha 0:a291977ec0b1 326 return WMbox[2];
arthicha 0:a291977ec0b1 327 }else
arthicha 0:a291977ec0b1 328 {
arthicha 0:a291977ec0b1 329 return WMbox[5];
arthicha 0:a291977ec0b1 330 }
arthicha 0:a291977ec0b1 331 }
arthicha 0:a291977ec0b1 332
arthicha 0:a291977ec0b1 333 void ChangeOX(int xx, int yy,int zz,int lx,int ly,int lz)
arthicha 0:a291977ec0b1 334 {
arthicha 0:a291977ec0b1 335 if (xx != -1)
arthicha 0:a291977ec0b1 336 {
arthicha 0:a291977ec0b1 337 WMbox[0] = xx;
arthicha 0:a291977ec0b1 338 WMbox[1] = yy;
arthicha 0:a291977ec0b1 339 WMbox[2] = zz;
arthicha 0:a291977ec0b1 340 WMbox[3] = lx;
arthicha 0:a291977ec0b1 341 WMbox[4] = ly;
arthicha 0:a291977ec0b1 342 WMbox[5] = lz;
arthicha 0:a291977ec0b1 343 }else
arthicha 0:a291977ec0b1 344 {
arthicha 0:a291977ec0b1 345 WMbox[0] = lx;
arthicha 0:a291977ec0b1 346 WMbox[1] = ly;
arthicha 0:a291977ec0b1 347 WMbox[2] = lz;
arthicha 0:a291977ec0b1 348 WMbox[3] = xx;
arthicha 0:a291977ec0b1 349 WMbox[4] = yy;
arthicha 0:a291977ec0b1 350 WMbox[5] = zz;
arthicha 0:a291977ec0b1 351 }
arthicha 0:a291977ec0b1 352 }
arthicha 0:a291977ec0b1 353
arthicha 0:a291977ec0b1 354 void Choose(int box)
arthicha 0:a291977ec0b1 355 {
arthicha 0:a291977ec0b1 356 if (box)
arthicha 0:a291977ec0b1 357 {
arthicha 0:a291977ec0b1 358 WMbox[0] = -1;
arthicha 0:a291977ec0b1 359 WMbox[1] = -1;
arthicha 0:a291977ec0b1 360 WMbox[2] = -1;
arthicha 0:a291977ec0b1 361 }else
arthicha 0:a291977ec0b1 362 {
arthicha 0:a291977ec0b1 363 WMbox[3] = -1;
arthicha 0:a291977ec0b1 364 WMbox[4] = -1;
arthicha 0:a291977ec0b1 365 WMbox[5] = -1;
arthicha 0:a291977ec0b1 366 }
arthicha 0:a291977ec0b1 367 }
arthicha 0:a291977ec0b1 368
arthicha 0:a291977ec0b1 369 void MapRotate(int role,int pitch,int yaw)
arthicha 0:a291977ec0b1 370 {
arthicha 0:a291977ec0b1 371 if ((pitch == 90) || (pitch == -90)) //{{1,0,0},{0,cosy,-siny},{0,siny,cosy}}{{cosx,0,+sinx},{0,1,0},{-sinx,0,cosx}}
arthicha 0:a291977ec0b1 372 {
arthicha 0:a291977ec0b1 373 for (int i=0;i<5;i++)
arthicha 0:a291977ec0b1 374 {
arthicha 0:a291977ec0b1 375 for (int j=0;j<5;j++)
arthicha 0:a291977ec0b1 376 {
arthicha 0:a291977ec0b1 377 for(int k=0;k<5;k++)
arthicha 0:a291977ec0b1 378 {
arthicha 0:a291977ec0b1 379 A = ((i-2)* Cross(pitch))+((k-2)* Zine(pitch));
arthicha 0:a291977ec0b1 380 B = ((j-2)* Cross(yaw))+((i-2)* Zine(pitch)* Zine(yaw))-((k-2)* Zine(yaw)* Cross(pitch));
arthicha 0:a291977ec0b1 381 C = ((j-2)* Zine(yaw))-((i-2)* Cross(yaw)* Zine(pitch))+((k-2)* Cross(yaw)* Cross(pitch));
arthicha 0:a291977ec0b1 382 rmap[i+2][j+2][k+2]= map[i][j][k];
arthicha 0:a291977ec0b1 383 }
arthicha 0:a291977ec0b1 384 }
arthicha 0:a291977ec0b1 385 }
arthicha 0:a291977ec0b1 386 }else if ((role == 90) || (role == -90)) // {{cosy,0,+siny},{0,1,0},{-siny,0,cosy}}{{1,0,0},{0,cosx,-sinx},{0,sinx,cosx}}
arthicha 0:a291977ec0b1 387 {
arthicha 0:a291977ec0b1 388 for (int i=0;i<5;i++)
arthicha 0:a291977ec0b1 389 {
arthicha 0:a291977ec0b1 390 for (int j=0;j<5;j++)
arthicha 0:a291977ec0b1 391 {
arthicha 0:a291977ec0b1 392 for(int k=0;k<5;k++)
arthicha 0:a291977ec0b1 393 {
arthicha 0:a291977ec0b1 394 A = ((i-2)* Cross(yaw)) + ((j-2)* Zine(role)* Zine(yaw)) +((k-2)* Zine(yaw)* Cross(role));
arthicha 0:a291977ec0b1 395 B = ((j-2)* Cross(role))-((k-2)* Zine(role));
arthicha 0:a291977ec0b1 396 C = -((i-2)* Zine(yaw))+((j-2)* Cross(yaw)* Zine(role))+((k-2)* Cross(yaw)* Cross(role));
arthicha 0:a291977ec0b1 397 rmap[i+2][j+2][k+2]= map[i][j][k];
arthicha 0:a291977ec0b1 398 }
arthicha 0:a291977ec0b1 399 }
arthicha 0:a291977ec0b1 400 }
arthicha 0:a291977ec0b1 401 }else
arthicha 0:a291977ec0b1 402 {
arthicha 0:a291977ec0b1 403 for (int i=0;i<5;i++)
arthicha 0:a291977ec0b1 404 {
arthicha 0:a291977ec0b1 405 for (int j=0;j<5;j++)
arthicha 0:a291977ec0b1 406 {
arthicha 0:a291977ec0b1 407 for(int k=0;k<5;k++)
arthicha 0:a291977ec0b1 408 {
arthicha 0:a291977ec0b1 409 A = ((i-2)* Cross(yaw))-((j-2)* Zine(yaw));
arthicha 0:a291977ec0b1 410 B = ((i-2)* Zine(yaw))+((j-2)* Cross(yaw));
arthicha 0:a291977ec0b1 411 C = (k-2);
arthicha 0:a291977ec0b1 412 rmap[i+2][j+2][k+2]= map[i][j][k];
arthicha 0:a291977ec0b1 413 }
arthicha 0:a291977ec0b1 414 }
arthicha 0:a291977ec0b1 415 }
arthicha 0:a291977ec0b1 416 }
arthicha 0:a291977ec0b1 417 for (int i=0;i<5;i++)
arthicha 0:a291977ec0b1 418 {
arthicha 0:a291977ec0b1 419 for (int j=0;j<5;j++)
arthicha 0:a291977ec0b1 420 {
arthicha 0:a291977ec0b1 421 for(int k=0;k<5;k++)
arthicha 0:a291977ec0b1 422 {
arthicha 0:a291977ec0b1 423 map[i][j][k]= rmap[i][j][k];
arthicha 0:a291977ec0b1 424 }
arthicha 0:a291977ec0b1 425 }
arthicha 0:a291977ec0b1 426 }
arthicha 0:a291977ec0b1 427 }
arthicha 0:a291977ec0b1 428
arthicha 0:a291977ec0b1 429 void UpdateMap(int i,int j,int k,int turner){
arthicha 0:a291977ec0b1 430 if(1){
arthicha 0:a291977ec0b1 431
arthicha 0:a291977ec0b1 432 switch(turner%2){ // เลือกตำแหน่งคนลง
arthicha 0:a291977ec0b1 433
arthicha 0:a291977ec0b1 434 case '0':
arthicha 0:a291977ec0b1 435 i-- , j-- , k--;
arthicha 0:a291977ec0b1 436 if( map[i][j][k] == 0){
arthicha 0:a291977ec0b1 437 map[i][j][k] = 5; // 5 = 'x'
arthicha 0:a291977ec0b1 438 turn++;
arthicha 0:a291977ec0b1 439 }
arthicha 0:a291977ec0b1 440 else{
arthicha 0:a291977ec0b1 441 Lose(); // ลงไมไ่ด้
arthicha 0:a291977ec0b1 442 }
arthicha 0:a291977ec0b1 443 case '1':
arthicha 0:a291977ec0b1 444 i-- , j-- , k--;
arthicha 0:a291977ec0b1 445 if( map[i][j][k] == 0){
arthicha 0:a291977ec0b1 446 map[i][j][k] = 6; // 6 = 'o'
arthicha 0:a291977ec0b1 447 turn++;
arthicha 0:a291977ec0b1 448 }
arthicha 0:a291977ec0b1 449 else{
arthicha 0:a291977ec0b1 450 Lose(); // ลงไมไ่ด้
arthicha 0:a291977ec0b1 451 }
arthicha 0:a291977ec0b1 452 }
arthicha 0:a291977ec0b1 453 if(i == 5 or j == 5 or k == 5){ // ลงเกิน map
arthicha 0:a291977ec0b1 454 Lose();
arthicha 0:a291977ec0b1 455 }
arthicha 0:a291977ec0b1 456
arthicha 0:a291977ec0b1 457 for(i=0 ; i<5 ; i++){ //แนว k
arthicha 0:a291977ec0b1 458 for(j=0 ; j<5 ; j++){
arthicha 0:a291977ec0b1 459 for(k=0 ; k<3 ; k++){
arthicha 0:a291977ec0b1 460 if( map[i][j][k] and map[i][j][k+1] and map[i][j][k+2] == 5){
arthicha 0:a291977ec0b1 461 Win();
arthicha 0:a291977ec0b1 462 }
arthicha 0:a291977ec0b1 463 else if( map[i][j][k] and map[i][j][k+1] and map[i][j][k+2] == 6){
arthicha 0:a291977ec0b1 464 Win();
arthicha 0:a291977ec0b1 465 }
arthicha 0:a291977ec0b1 466 }
arthicha 0:a291977ec0b1 467 }
arthicha 0:a291977ec0b1 468 }
arthicha 0:a291977ec0b1 469
arthicha 0:a291977ec0b1 470 for(i=0 ; i<5 ; i++){ // แนว j
arthicha 0:a291977ec0b1 471 for(k=0 ; k<5 ; k++){
arthicha 0:a291977ec0b1 472 for(j=0 ; j<3 ; j++){
arthicha 0:a291977ec0b1 473 if( map[i][j][k] and map[i][j+1][k] and map[i][j+2][k] == 5){
arthicha 0:a291977ec0b1 474 Win();
arthicha 0:a291977ec0b1 475 }
arthicha 0:a291977ec0b1 476 else if( map[i][j][k] and map[i][j+1][k] and map[i][j+2][k] == 6){
arthicha 0:a291977ec0b1 477 Win();
arthicha 0:a291977ec0b1 478 }
arthicha 0:a291977ec0b1 479 }
arthicha 0:a291977ec0b1 480 }
arthicha 0:a291977ec0b1 481 }
arthicha 0:a291977ec0b1 482
arthicha 0:a291977ec0b1 483 for(j=0 ; j<5 ; j++){ // แนว i
arthicha 0:a291977ec0b1 484 for(k=0 ; k<5 ; k++){
arthicha 0:a291977ec0b1 485 for(i=0 ; i<3 ; i++){
arthicha 0:a291977ec0b1 486 if( map[i][j][k] and map[i+1][j][k] and map[i+2][j][k] == 5){
arthicha 0:a291977ec0b1 487 Win();
arthicha 0:a291977ec0b1 488 }
arthicha 0:a291977ec0b1 489 else if( map[i][j][k] and map[i+1][j][k] and map[i+2][j][k] == 6){
arthicha 0:a291977ec0b1 490 Win();
arthicha 0:a291977ec0b1 491 }
arthicha 0:a291977ec0b1 492 }
arthicha 0:a291977ec0b1 493 }
arthicha 0:a291977ec0b1 494 }
arthicha 0:a291977ec0b1 495
arthicha 0:a291977ec0b1 496 for(i=0 ; i<5 ; i++){ // แนวทแยง i เป็นฐานขึ้น
arthicha 0:a291977ec0b1 497 for(j=0 ; j<3 ; j++){
arthicha 0:a291977ec0b1 498 for(k=0 ; k<3 ; k++){
arthicha 0:a291977ec0b1 499 if( map[i][j][k] and map[i][j+1][k+1] and map[i][j+2][k+2] == 5){
arthicha 0:a291977ec0b1 500 Win();
arthicha 0:a291977ec0b1 501 }
arthicha 0:a291977ec0b1 502 else if( map[i][j][k] and map[i][j+1][k+1] and map[i][j+2][k+2] == 6){
arthicha 0:a291977ec0b1 503 Win();
arthicha 0:a291977ec0b1 504 }
arthicha 0:a291977ec0b1 505 }
arthicha 0:a291977ec0b1 506 }
arthicha 0:a291977ec0b1 507 }
arthicha 0:a291977ec0b1 508
arthicha 0:a291977ec0b1 509 for(j=0 ; j<5 ; j++){ // แนวทแยง j เป็นฐานขึ้น
arthicha 0:a291977ec0b1 510 for(i=0 ; i<3 ; i++){
arthicha 0:a291977ec0b1 511 for(k=0 ; k<3 ; k++){
arthicha 0:a291977ec0b1 512 if( map[i][j][k] and map[i+1][j][k+1] and map[i+2][j][k+2] == 5){
arthicha 0:a291977ec0b1 513 Win();
arthicha 0:a291977ec0b1 514 }
arthicha 0:a291977ec0b1 515 else if( map[i][j][k] and map[i+1][j][k+1] and map[i+2][j][k+2] == 6){
arthicha 0:a291977ec0b1 516 Win();
arthicha 0:a291977ec0b1 517 }
arthicha 0:a291977ec0b1 518 }
arthicha 0:a291977ec0b1 519 }
arthicha 0:a291977ec0b1 520 }
arthicha 0:a291977ec0b1 521
arthicha 0:a291977ec0b1 522 for(k=0 ; k<5 ; k++){ // แนวทแยง k เป็นฐานขึ้น
arthicha 0:a291977ec0b1 523 for(i=0 ; i<3 ; i++){
arthicha 0:a291977ec0b1 524 for(j=0 ; j<3 ; j++){
arthicha 0:a291977ec0b1 525 if( map[i][j][k] and map[i+1][j+1][k] and map[i+2][j+2][k] == 5){
arthicha 0:a291977ec0b1 526 Win();
arthicha 0:a291977ec0b1 527 }
arthicha 0:a291977ec0b1 528 else if( map[i][j][k] and map[i+1][j+1][k] and map[i+2][j+2][k] == 6){
arthicha 0:a291977ec0b1 529 Win();
arthicha 0:a291977ec0b1 530 }
arthicha 0:a291977ec0b1 531 }
arthicha 0:a291977ec0b1 532 }
arthicha 0:a291977ec0b1 533 }
arthicha 0:a291977ec0b1 534
arthicha 0:a291977ec0b1 535 for(i=0 ; i<5 ; i++){ // แนวทแยง i เป็นฐานลง
arthicha 0:a291977ec0b1 536 for(j=0 ; j<3 ; j++){
arthicha 0:a291977ec0b1 537 for(k=4 ; k>1 ; k--){
arthicha 0:a291977ec0b1 538 if( map[i][j][k] and map[i][j+1][k-1] and map[i][j+2][k-2] == 5){
arthicha 0:a291977ec0b1 539 Win();
arthicha 0:a291977ec0b1 540 }
arthicha 0:a291977ec0b1 541 else if( map[i][j][k] and map[i][j+1][k-1] and map[i][j+2][k-2] == 6){
arthicha 0:a291977ec0b1 542 Win();
arthicha 0:a291977ec0b1 543 }
arthicha 0:a291977ec0b1 544 }
arthicha 0:a291977ec0b1 545 }
arthicha 0:a291977ec0b1 546 }
arthicha 0:a291977ec0b1 547
arthicha 0:a291977ec0b1 548 for(j=0 ; j<5 ; j++){ // แนวทแยง j เป็นฐานลง
arthicha 0:a291977ec0b1 549 for(i=0 ; i<3 ; i++){
arthicha 0:a291977ec0b1 550 for(k=4 ; k>1 ; k--){
arthicha 0:a291977ec0b1 551 if( map[i][j][k] and map[i+1][j][k-1] and map[i+2][j][k-2] == 5){
arthicha 0:a291977ec0b1 552 Win();
arthicha 0:a291977ec0b1 553 }
arthicha 0:a291977ec0b1 554 else if( map[i][j][k] and map[i+1][j][k-1] and map[i+2][j][k-2] == 6){
arthicha 0:a291977ec0b1 555 Win();
arthicha 0:a291977ec0b1 556 }
arthicha 0:a291977ec0b1 557 }
arthicha 0:a291977ec0b1 558 }
arthicha 0:a291977ec0b1 559 }
arthicha 0:a291977ec0b1 560
arthicha 0:a291977ec0b1 561 for(k=0 ; k<5 ; k++){ // แนวทแยง k เป็นฐานลง
arthicha 0:a291977ec0b1 562 for(i=0 ; i<3 ; i++){
arthicha 0:a291977ec0b1 563 for(j=4 ; j>1 ; j--){
arthicha 0:a291977ec0b1 564 if( map[i][j][k] and map[i+1][j-1][k] and map[i+2][j-2][k] == 5){
arthicha 0:a291977ec0b1 565 Win();
arthicha 0:a291977ec0b1 566 }
arthicha 0:a291977ec0b1 567 else if( map[i][j][k] and map[i+1][j-1][k] and map[i+2][j-2][k] == 6){
arthicha 0:a291977ec0b1 568 Win();
arthicha 0:a291977ec0b1 569 }
arthicha 0:a291977ec0b1 570 }
arthicha 0:a291977ec0b1 571 }
arthicha 0:a291977ec0b1 572 }
arthicha 0:a291977ec0b1 573 for(i=0 ; i<3 ; i++){ // แนวทแยงแบบตัด1
arthicha 0:a291977ec0b1 574 for(j=0 ; j<3 ; j++){
arthicha 0:a291977ec0b1 575 for(k=0 ; k<3 ; k++){
arthicha 0:a291977ec0b1 576 if( map[i][j][k] and map[i+1][j+1][k+1] and map[i+2][j+2][k+2] == 5){
arthicha 0:a291977ec0b1 577 Win();
arthicha 0:a291977ec0b1 578 }
arthicha 0:a291977ec0b1 579 else if( map[i][j][k] and map[i+1][j+1][k+1] and map[i+2][j+2][k+2] == 6){
arthicha 0:a291977ec0b1 580 Win();
arthicha 0:a291977ec0b1 581 }
arthicha 0:a291977ec0b1 582 }
arthicha 0:a291977ec0b1 583 }
arthicha 0:a291977ec0b1 584 }
arthicha 0:a291977ec0b1 585 for(i=4 ; i>1 ; i--){ // แนวทแยงแบบตัด2
arthicha 0:a291977ec0b1 586 for(j=0 ; j<3 ; j++){
arthicha 0:a291977ec0b1 587 for(k=0 ; k<3 ; k++){
arthicha 0:a291977ec0b1 588 if( map[i][j][k] and map[i-1][j+1][k+1] and map[i-2][j+2][k+2] == 5){
arthicha 0:a291977ec0b1 589 Win();
arthicha 0:a291977ec0b1 590 }
arthicha 0:a291977ec0b1 591 else if( map[i][j][k] and map[i-1][j+1][k+1] and map[i-2][j+2][k+2] == 6){
arthicha 0:a291977ec0b1 592 Win();
arthicha 0:a291977ec0b1 593 }
arthicha 0:a291977ec0b1 594 }
arthicha 0:a291977ec0b1 595 }
arthicha 0:a291977ec0b1 596 }
arthicha 0:a291977ec0b1 597 for(i=4 ; i>1 ; i--){ // แนวทแยงแบบตัด 3
arthicha 0:a291977ec0b1 598 for(j=4 ; j>1 ; j--){
arthicha 0:a291977ec0b1 599 for(k=0 ; k<3 ; k++){
arthicha 0:a291977ec0b1 600 if( map[i][j][k] and map[i-1][j-1][k+1] and map[i-2][j-2][k+2] == 5){
arthicha 0:a291977ec0b1 601 Win();
arthicha 0:a291977ec0b1 602 }
arthicha 0:a291977ec0b1 603 else if( map[i][j][k] and map[i-1][j-1][k+1] and map[i-2][j-2][k+2] == 6){
arthicha 0:a291977ec0b1 604 Win();
arthicha 0:a291977ec0b1 605 }
arthicha 0:a291977ec0b1 606 }
arthicha 0:a291977ec0b1 607 }
arthicha 0:a291977ec0b1 608 }
arthicha 0:a291977ec0b1 609 for(i=0 ; i<3 ; i++){ // แนวทแยงแบบตัด4
arthicha 0:a291977ec0b1 610 for(j=4 ; j>1 ; j--){
arthicha 0:a291977ec0b1 611 for(k=0 ; k<3 ; k++){
arthicha 0:a291977ec0b1 612 if( map[i][j][k] and map[i+1][j-1][k+1] and map[i+2][j-2][k+2] == 5){
arthicha 0:a291977ec0b1 613 Win();
arthicha 0:a291977ec0b1 614 }
arthicha 0:a291977ec0b1 615 else if( map[i][j][k] and map[i+1][j-1][k+1] and map[i+2][j-2][k+2] == 6){
arthicha 0:a291977ec0b1 616 Win();
arthicha 0:a291977ec0b1 617 }
arthicha 0:a291977ec0b1 618 }
arthicha 0:a291977ec0b1 619 }
arthicha 0:a291977ec0b1 620 }
arthicha 0:a291977ec0b1 621
arthicha 0:a291977ec0b1 622
arthicha 0:a291977ec0b1 623 }
arthicha 0:a291977ec0b1 624
arthicha 0:a291977ec0b1 625 }
arthicha 0:a291977ec0b1 626
arthicha 0:a291977ec0b1 627
arthicha 0:a291977ec0b1 628
arthicha 0:a291977ec0b1 629 void Check_map(){ //แสดง map
arthicha 0:a291977ec0b1 630 for( A=0 ; A<5 ; A++){
arthicha 0:a291977ec0b1 631 for( B=0 ; B<5 ; B++){
arthicha 0:a291977ec0b1 632 for( C=0 ; C<5 ; C++){
arthicha 0:a291977ec0b1 633 if( map[ A][ B][ C] == 5){
arthicha 0:a291977ec0b1 634 Check1[ A][ B][ C] = 1;
arthicha 0:a291977ec0b1 635 }
arthicha 0:a291977ec0b1 636 else if( map[ A][ B][ C] == 6){
arthicha 0:a291977ec0b1 637 Check2[ A][ B][ C] = 1;
arthicha 0:a291977ec0b1 638 }
arthicha 0:a291977ec0b1 639 else{
arthicha 0:a291977ec0b1 640 Check1[ A][ B][ C] = 0;
arthicha 0:a291977ec0b1 641 Check2[ A][ B][ C] = 0;
arthicha 0:a291977ec0b1 642 }
arthicha 0:a291977ec0b1 643 }
arthicha 0:a291977ec0b1 644 }
arthicha 0:a291977ec0b1 645 }
arthicha 0:a291977ec0b1 646 }
arthicha 0:a291977ec0b1 647
arthicha 0:a291977ec0b1 648
arthicha 0:a291977ec0b1 649 void LInitial(){ //ลงค่าเสร็จ
arthicha 0:a291977ec0b1 650 if (timer.read() > 1) {
arthicha 0:a291977ec0b1 651
arthicha 0:a291977ec0b1 652
arthicha 0:a291977ec0b1 653 duino.putc(lis[20]);
arthicha 0:a291977ec0b1 654
arthicha 0:a291977ec0b1 655
arthicha 0:a291977ec0b1 656 timer.reset();
arthicha 0:a291977ec0b1 657 }
arthicha 0:a291977ec0b1 658 if (duino.readable()){
arthicha 0:a291977ec0b1 659 }
arthicha 0:a291977ec0b1 660 }
arthicha 0:a291977ec0b1 661
arthicha 0:a291977ec0b1 662
arthicha 0:a291977ec0b1 663 int main() // main program of
arthicha 0:a291977ec0b1 664 {
arthicha 0:a291977ec0b1 665 lim.rise(&Rise1);
arthicha 0:a291977ec0b1 666 lim.fall(&Fall1);
arthicha 0:a291977ec0b1 667 rotate.rise(&Rise2);
arthicha 0:a291977ec0b1 668 rotate.fall(&Fall2);
arthicha 0:a291977ec0b1 669 choose.rise(&Rise3);
arthicha 0:a291977ec0b1 670 choose.fall(&Fall3);
arthicha 0:a291977ec0b1 671
arthicha 0:a291977ec0b1 672 LInitial(); // set initial pattern in led matrix.
arthicha 0:a291977ec0b1 673 wait(5); // wait 5 second for putting the box down to set initial position of imu.
arthicha 0:a291977ec0b1 674 //imu.Initial(); // set initial for imu.
arthicha 0:a291977ec0b1 675 Update.attach(&UUpdate,1.0); //timer interrupt for recieving bluetooth.
arthicha 0:a291977ec0b1 676 while((dete[0]!='i')||(dete[1]!='n')||(dete[2]!='i')||(dete[3]!='t'))
arthicha 0:a291977ec0b1 677 {
arthicha 0:a291977ec0b1 678 }
arthicha 0:a291977ec0b1 679 LWbox(); // set led to tell the player that the game is ready and this is the woman-box.
arthicha 0:a291977ec0b1 680 t.start();
arthicha 0:a291977ec0b1 681 while (1) // void loop.
arthicha 0:a291977ec0b1 682 {
arthicha 0:a291977ec0b1 683 state = 1;
arthicha 0:a291977ec0b1 684
arthicha 0:a291977ec0b1 685 if (buttonState[0] == 1) // if the boxes are connected.
arthicha 0:a291977ec0b1 686 {
arthicha 0:a291977ec0b1 687 x = GetCoordinateX();
arthicha 0:a291977ec0b1 688 y = GetCoordinateY();
arthicha 0:a291977ec0b1 689 z = GetCoordinateZ(); // get the position of main box which linking with map or the box not be choosen.
arthicha 0:a291977ec0b1 690 /*Sent("helr\n",0.1,5); // tell the other box, that we want helper roll.
arthicha 0:a291977ec0b1 691 while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='r'))
arthicha 0:a291977ec0b1 692 {
arthicha 0:a291977ec0b1 693 pc.printf("srt %s\n\r",dete);
arthicha 0:a291977ec0b1 694 }
arthicha 0:a291977ec0b1 695 helperRoll = int( ((int(dete[4+ii])-48)*100)+((int(dete[5+ii])-48)*10)+(int(dete[6+ii])-48) ); //get roll from another box.
arthicha 0:a291977ec0b1 696 Sent("help\n",0.1,5); // tell the other box, that we want helper roll.
arthicha 0:a291977ec0b1 697 while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='p'))
arthicha 0:a291977ec0b1 698 {
arthicha 0:a291977ec0b1 699 pc.printf("srt %s\n\r",dete);
arthicha 0:a291977ec0b1 700 }
arthicha 0:a291977ec0b1 701 helperPitch = int( ((int(dete[4+ii])-48)*100)+((int(dete[5+ii])-48)*10)+(int(dete[6+ii])-48) ); //get roll from another box.
arthicha 0:a291977ec0b1 702 Sent("hely\n",0.1,5); // tell the other box, that we want helper roll.
arthicha 0:a291977ec0b1 703 while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='y'))
arthicha 0:a291977ec0b1 704 {
arthicha 0:a291977ec0b1 705 pc.printf("srt %s\n\r",dete);
arthicha 0:a291977ec0b1 706 }
arthicha 0:a291977ec0b1 707 helperYaw = int( ((int(dete[4+ii])-48)*100)+((int(dete[5+ii])-48)*10)+(int(dete[6+ii])-48) ); //get roll from another box.*/
arthicha 0:a291977ec0b1 708 //Sent("ok\n",0.1,3); //free the otherbox.
arthicha 0:a291977ec0b1 709
arthicha 0:a291977ec0b1 710 errorie = imu.Getposition(x,y,z);//,helperRoll,helperPitch,helperYaw); // calculate position from imu.
arthicha 0:a291977ec0b1 711 if (errorie)
arthicha 0:a291977ec0b1 712 {
arthicha 0:a291977ec0b1 713 LError();
arthicha 0:a291977ec0b1 714 while(buttonState[0] == 1)
arthicha 0:a291977ec0b1 715 {
arthicha 0:a291977ec0b1 716
arthicha 0:a291977ec0b1 717 }
arthicha 0:a291977ec0b1 718 }else
arthicha 0:a291977ec0b1 719 {
arthicha 0:a291977ec0b1 720 lx = x;
arthicha 0:a291977ec0b1 721 ly = y;
arthicha 0:a291977ec0b1 722 lz = z;
arthicha 0:a291977ec0b1 723 x = imu.X_pos;
arthicha 0:a291977ec0b1 724 y = imu.Y_pos;
arthicha 0:a291977ec0b1 725 z = imu.Z_pos; // get new possition.
arthicha 0:a291977ec0b1 726
arthicha 0:a291977ec0b1 727 UpdateMap(x,y,z,turn);//,helperRoll); //update new position in map.
arthicha 0:a291977ec0b1 728
arthicha 0:a291977ec0b1 729 ChangeOX(x,y,z,lx,ly,lz);
arthicha 0:a291977ec0b1 730
arthicha 0:a291977ec0b1 731 Sent("mapc\n",0.1,4); //map complete.
arthicha 0:a291977ec0b1 732 LMapComplete(); // show that this position is ok.
arthicha 0:a291977ec0b1 733 while(1)
arthicha 0:a291977ec0b1 734 {
arthicha 0:a291977ec0b1 735 anach = chooseAnother() ;// wheather thr others box is choosen.
arthicha 0:a291977ec0b1 736 if ((buttonState[2]==1) && (!anach))
arthicha 0:a291977ec0b1 737 {
arthicha 0:a291977ec0b1 738 Sent("ch1\n",0.1,4);
arthicha 0:a291977ec0b1 739
arthicha 0:a291977ec0b1 740 Choose(1); // choose main box.
arthicha 0:a291977ec0b1 741 }else if((buttonState[2]==0) && (anach))
arthicha 0:a291977ec0b1 742 {
arthicha 0:a291977ec0b1 743 Choose(0); // choose another box.
arthicha 0:a291977ec0b1 744
arthicha 0:a291977ec0b1 745 }else if(buttonState[2]==0)
arthicha 0:a291977ec0b1 746 {
arthicha 0:a291977ec0b1 747 if((buttonState[1]==0)||(anotherRotate())) // click buutton some rotate box.
arthicha 0:a291977ec0b1 748 {
arthicha 0:a291977ec0b1 749 state = 3;
arthicha 0:a291977ec0b1 750 while((buttonState[1]==0)&&(anotherRotate()))
arthicha 0:a291977ec0b1 751 {
arthicha 0:a291977ec0b1 752
arthicha 0:a291977ec0b1 753 }
arthicha 0:a291977ec0b1 754
arthicha 0:a291977ec0b1 755 yyaaww = imu.Rotate(); // check wheater rotating is error.
arthicha 0:a291977ec0b1 756 MapRotate(0,0,yyaaww);//imu.Rotate_Yaw); // private variable/arttribute in imu class.
arthicha 0:a291977ec0b1 757 LRotateComplete();
arthicha 0:a291977ec0b1 758 }
arthicha 0:a291977ec0b1 759 } //close choose or rotate.
arthicha 0:a291977ec0b1 760 } //close while true.
arthicha 0:a291977ec0b1 761 turn ++;
arthicha 0:a291977ec0b1 762 Sent("T++\n",0.1,4);
arthicha 0:a291977ec0b1 763 t.start();
arthicha 0:a291977ec0b1 764 state = 2;
arthicha 0:a291977ec0b1 765 //LChooseComplete();
arthicha 0:a291977ec0b1 766 } // close not error.
arthicha 0:a291977ec0b1 767
arthicha 0:a291977ec0b1 768 }else // if the boxes aren't connect.
arthicha 0:a291977ec0b1 769 {
arthicha 0:a291977ec0b1 770 if (county == 0) // time out.
arthicha 0:a291977ec0b1 771 {
arthicha 0:a291977ec0b1 772 t.stop();
arthicha 0:a291977ec0b1 773 Lose(); // lose state
arthicha 0:a291977ec0b1 774 }else if(turn != 0)
arthicha 0:a291977ec0b1 775 {
arthicha 0:a291977ec0b1 776 t.stop();
arthicha 0:a291977ec0b1 777 if (t.read() > 1.0);
arthicha 0:a291977ec0b1 778 {
arthicha 0:a291977ec0b1 779 t.start();
arthicha 0:a291977ec0b1 780 county -= 1; // time decreasing.
arthicha 0:a291977ec0b1 781 }
arthicha 0:a291977ec0b1 782 } // close check time
arthicha 0:a291977ec0b1 783 LCountDown(county);
arthicha 0:a291977ec0b1 784 Sent("Tur\n",0.1,4);
arthicha 0:a291977ec0b1 785 } // close boxes aren't connect.
arthicha 0:a291977ec0b1 786
arthicha 0:a291977ec0b1 787 } // close loop.
arthicha 0:a291977ec0b1 788
arthicha 0:a291977ec0b1 789
arthicha 0:a291977ec0b1 790
arthicha 0:a291977ec0b1 791 } // close main.*/