OX / Mbed 2 deprecated Ox_ver_function

hahaha

Dependencies:   mbed

main.cpp@2:ce3ee4bc8cf7, 2016-12-06 (annotated)

Committer:
siwakon
Date:
Tue Dec 06 12:19:49 2016 +0000
Revision:
2:ce3ee4bc8cf7
Parent:
1:d8ce226c8c2e 
ddd

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 InterruptIn lim(D3);
arthicha 0:a291977ec0b1 5 InterruptIn rotate(D4);
arthicha 0:a291977ec0b1 6 InterruptIn choose(D5);
arthicha 1:d8ce226c8c2e 7 //Ticker flipper;
arthicha 0:a291977ec0b1 8 Ticker Update;
arthicha 0:a291977ec0b1 9
arthicha 0:a291977ec0b1 10 bmuimu imu;
arthicha 0:a291977ec0b1 11
arthicha 0:a291977ec0b1 12 Serial pc(USBTX,USBRX);
arthicha 0:a291977ec0b1 13 Serial mas(D8,D2);
arthicha 0:a291977ec0b1 14 Serial duino(PA_11, PA_12);
arthicha 0:a291977ec0b1 15
arthicha 0:a291977ec0b1 16 Timer t;
arthicha 0:a291977ec0b1 17 Timer timer;
arthicha 0:a291977ec0b1 18
arthicha 0:a291977ec0b1 19 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 20 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 21 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 22 // variable
arthicha 0:a291977ec0b1 23 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 24 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 25 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 26 bool anach = false;
arthicha 0:a291977ec0b1 27 bool buttonState[3] = {0,0,0}; // button limit switchm rotate and choose state respectively.
arthicha 0:a291977ec0b1 28 unsigned int county = 10; // county down.
arthicha 0:a291977ec0b1 29 char data[50];
arthicha 0:a291977ec0b1 30 char elem;
arthicha 0:a291977ec0b1 31 int yyaaww;
arthicha 0:a291977ec0b1 32 bool errorie = false; // store error in ambigous imu data.
arthicha 0:a291977ec0b1 33 int helperRoll = 0, helperPitch=0,helperYaw=0;
arthicha 0:a291977ec0b1 34 unsigned int indx = 0;
arthicha 0:a291977ec0b1 35 unsigned int state = 0;
arthicha 0:a291977ec0b1 36 int dete[50];
arthicha 0:a291977ec0b1 37 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 38
arthicha 0:a291977ec0b1 39 // state 0 = initial / start.
arthicha 0:a291977ec0b1 40 // 1 = waiting for limit switch.
arthicha 0:a291977ec0b1 41
arthicha 0:a291977ec0b1 42 unsigned int turn = 0; // turn.
arthicha 0:a291977ec0b1 43
arthicha 0:a291977ec0b1 44 unsigned int x = 0, y = 0, z = 0,lx=0,ly=0,lz=0;
arthicha 0:a291977ec0b1 45 int winStatus = 0;
arthicha 0:a291977ec0b1 46
arthicha 0:a291977ec0b1 47
arthicha 0:a291977ec0b1 48
arthicha 0:a291977ec0b1 49 int A , B , C , player , button;
arthicha 0:a291977ec0b1 50 int WMbox[6];
arthicha 0:a291977ec0b1 51 int map[5][5][5],rmap[5][5][5]; //store map.
arthicha 0:a291977ec0b1 52 int Check1[5][5][5] , Check2[5][5][5]; //convert to x or o map.
arthicha 0:a291977ec0b1 53 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 54 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 55 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 56 // function
arthicha 0:a291977ec0b1 57 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 58 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 59 //*********************************************************************************************************************************
arthicha 0:a291977ec0b1 60
arthicha 0:a291977ec0b1 61 int Cross(int degree)
arthicha 0:a291977ec0b1 62 {
arthicha 0:a291977ec0b1 63 if (degree == 0)
arthicha 0:a291977ec0b1 64 {
arthicha 0:a291977ec0b1 65 return 1;
arthicha 0:a291977ec0b1 66 }else if (degree == 180)
arthicha 0:a291977ec0b1 67 {
arthicha 0:a291977ec0b1 68 return -1;
arthicha 0:a291977ec0b1 69 }else
arthicha 0:a291977ec0b1 70 {
arthicha 0:a291977ec0b1 71 return 0;
arthicha 0:a291977ec0b1 72 }
arthicha 0:a291977ec0b1 73 }
arthicha 0:a291977ec0b1 74
arthicha 0:a291977ec0b1 75 int Zine(int degree)
arthicha 0:a291977ec0b1 76 {
arthicha 0:a291977ec0b1 77 if (degree == 90)
arthicha 0:a291977ec0b1 78 {
arthicha 0:a291977ec0b1 79 return 1;
arthicha 0:a291977ec0b1 80 }else if (degree == -90)
arthicha 0:a291977ec0b1 81 {
arthicha 0:a291977ec0b1 82 return -1;
arthicha 0:a291977ec0b1 83 }else
arthicha 0:a291977ec0b1 84 {
arthicha 0:a291977ec0b1 85 return 0;
arthicha 0:a291977ec0b1 86 }
arthicha 0:a291977ec0b1 87 }
arthicha 0:a291977ec0b1 88
arthicha 0:a291977ec0b1 89 void Rise1()
arthicha 0:a291977ec0b1 90 {
arthicha 0:a291977ec0b1 91 buttonState[0] = 1;
arthicha 0:a291977ec0b1 92 }
arthicha 0:a291977ec0b1 93
arthicha 0:a291977ec0b1 94 void Fall1()
arthicha 0:a291977ec0b1 95 {
arthicha 0:a291977ec0b1 96 buttonState[0] = 0;
arthicha 0:a291977ec0b1 97 }
arthicha 0:a291977ec0b1 98 void Rise2()
arthicha 0:a291977ec0b1 99 {
arthicha 0:a291977ec0b1 100 buttonState[1] = 1;
arthicha 0:a291977ec0b1 101 }
arthicha 0:a291977ec0b1 102
arthicha 0:a291977ec0b1 103 void Fall2()
arthicha 0:a291977ec0b1 104 {
arthicha 0:a291977ec0b1 105 buttonState[1] = 0;
arthicha 0:a291977ec0b1 106 }
arthicha 0:a291977ec0b1 107 void Rise3()
arthicha 0:a291977ec0b1 108 {
arthicha 0:a291977ec0b1 109 buttonState[2] = 1;
arthicha 0:a291977ec0b1 110 }
arthicha 0:a291977ec0b1 111
arthicha 0:a291977ec0b1 112 void Fall3()
arthicha 0:a291977ec0b1 113 {
arthicha 0:a291977ec0b1 114 buttonState[2] = 0;
arthicha 0:a291977ec0b1 115 }
arthicha 0:a291977ec0b1 116
arthicha 0:a291977ec0b1 117 void UUpdate()
arthicha 0:a291977ec0b1 118 {
arthicha 1:d8ce226c8c2e 119 imu.bmuupdate();
arthicha 0:a291977ec0b1 120 }
arthicha 0:a291977ec0b1 121
arthicha 0:a291977ec0b1 122 void Sent(char arrayi[],float it,int siz)
arthicha 0:a291977ec0b1 123 {
arthicha 0:a291977ec0b1 124 for (int i = 0;i<siz;i++)
arthicha 0:a291977ec0b1 125 {
siwakon 2:ce3ee4bc8cf7 126 mas.putc(char(arrayi[i]));UUpdate();
siwakon 2:ce3ee4bc8cf7 127 wait(it);UUpdate();
arthicha 0:a291977ec0b1 128 }
arthicha 0:a291977ec0b1 129 }
arthicha 0:a291977ec0b1 130
arthicha 0:a291977ec0b1 131 void getStr()
arthicha 0:a291977ec0b1 132 {
siwakon 2:ce3ee4bc8cf7 133 UUpdate();
arthicha 0:a291977ec0b1 134 while (mas.readable())
arthicha 0:a291977ec0b1 135 {
arthicha 0:a291977ec0b1 136
siwakon 2:ce3ee4bc8cf7 137 elem = mas.getc();UUpdate();
siwakon 2:ce3ee4bc8cf7 138 pc.printf("char is %d\n\r",elem);UUpdate();
arthicha 0:a291977ec0b1 139 if ((elem == '\n'))
arthicha 0:a291977ec0b1 140 {
siwakon 2:ce3ee4bc8cf7 141 pc.printf("string is %s\n\r",data);UUpdate();
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 {
siwakon 2:ce3ee4bc8cf7 146 dete[i] = data[i];UUpdate();
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 }
siwakon 2:ce3ee4bc8cf7 153 indx = 0;UUpdate();
arthicha 0:a291977ec0b1 154 break;
arthicha 0:a291977ec0b1 155 }else
arthicha 0:a291977ec0b1 156 {
siwakon 2:ce3ee4bc8cf7 157 data[indx] = elem;UUpdate();
siwakon 2:ce3ee4bc8cf7 158 indx += 1;UUpdate();
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
siwakon 2:ce3ee4bc8cf7 168 duino.putc(lis[23]);UUpdate();
arthicha 0:a291977ec0b1 169
siwakon 2:ce3ee4bc8cf7 170 timer.reset();UUpdate();
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");
siwakon 2:ce3ee4bc8cf7 177 duino.putc( lis[0]);UUpdate();
siwakon 2:ce3ee4bc8cf7 178 duino.putc( lis[1]);UUpdate();
siwakon 2:ce3ee4bc8cf7 179 duino.putc( lis[2]);UUpdate();
siwakon 2:ce3ee4bc8cf7 180 timer.reset();UUpdate();
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) {
siwakon 2:ce3ee4bc8cf7 186 duino.putc(ixxii);UUpdate();
siwakon 2:ce3ee4bc8cf7 187 timer.reset();UUpdate();
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) {
siwakon 2:ce3ee4bc8cf7 194 //pc.printf("Writing!\n\r");UUpdate();
siwakon 2:ce3ee4bc8cf7 195 duino.putc(lis[18]);UUpdate();
siwakon 2:ce3ee4bc8cf7 196 timer.reset();UUpdate();
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) {
siwakon 2:ce3ee4bc8cf7 202 //pc.printf("Writing!\n\r");UUpdate();
siwakon 2:ce3ee4bc8cf7 203 duino.putc( lis[19]);UUpdate();
siwakon 2:ce3ee4bc8cf7 204 timer.reset();UUpdate();
arthicha 0:a291977ec0b1 205 }
arthicha 0:a291977ec0b1 206 }
arthicha 0:a291977ec0b1 207 void LMapComplete(){ //ลงค่าเสร็จ
arthicha 0:a291977ec0b1 208 if ( timer.read() > 1) {
siwakon 2:ce3ee4bc8cf7 209 //pc.printf("Writing!\n\r");UUpdate();
arthicha 0:a291977ec0b1 210
arthicha 0:a291977ec0b1 211
siwakon 2:ce3ee4bc8cf7 212 duino.putc( lis[20]);UUpdate();
arthicha 0:a291977ec0b1 213
arthicha 0:a291977ec0b1 214
siwakon 2:ce3ee4bc8cf7 215 timer.reset();UUpdate();}}
arthicha 0:a291977ec0b1 216
arthicha 0:a291977ec0b1 217 void LWbox(){ //ตัวหลัก
arthicha 0:a291977ec0b1 218 if ( timer.read() > 1) {
siwakon 2:ce3ee4bc8cf7 219 duino.putc( lis[21]);UUpdate();
siwakon 2:ce3ee4bc8cf7 220 timer.reset();UUpdate();
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) {
siwakon 2:ce3ee4bc8cf7 226 //pc.printf("Writing!\n\r");UUpdate();
arthicha 0:a291977ec0b1 227
arthicha 0:a291977ec0b1 228
siwakon 2:ce3ee4bc8cf7 229 duino.putc( lis[22]);UUpdate();
arthicha 0:a291977ec0b1 230
arthicha 0:a291977ec0b1 231
siwakon 2:ce3ee4bc8cf7 232 timer.reset();UUpdate();
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");
siwakon 2:ce3ee4bc8cf7 242 duino.putc( lis[3]);UUpdate();
siwakon 2:ce3ee4bc8cf7 243 duino.putc( lis[4]);UUpdate();
siwakon 2:ce3ee4bc8cf7 244 duino.putc( lis[5]);UUpdate();
siwakon 2:ce3ee4bc8cf7 245 duino.putc( lis[6]);UUpdate();
siwakon 2:ce3ee4bc8cf7 246 t.reset();UUpdate();
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 {
siwakon 2:ce3ee4bc8cf7 257 LLose();UUpdate();
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 {
siwakon 2:ce3ee4bc8cf7 263 LWin();UUpdate();
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 {
siwakon 2:ce3ee4bc8cf7 337 WMbox[0] = xx;UUpdate();
siwakon 2:ce3ee4bc8cf7 338 WMbox[1] = yy;UUpdate();
siwakon 2:ce3ee4bc8cf7 339 WMbox[2] = zz;UUpdate();
siwakon 2:ce3ee4bc8cf7 340 WMbox[3] = lx;UUpdate();
siwakon 2:ce3ee4bc8cf7 341 WMbox[4] = ly;UUpdate();
siwakon 2:ce3ee4bc8cf7 342 WMbox[5] = lz;UUpdate();
arthicha 0:a291977ec0b1 343 }else
arthicha 0:a291977ec0b1 344 {
siwakon 2:ce3ee4bc8cf7 345 WMbox[0] = lx;UUpdate();
siwakon 2:ce3ee4bc8cf7 346 WMbox[1] = ly;UUpdate();
siwakon 2:ce3ee4bc8cf7 347 WMbox[2] = lz;UUpdate();
siwakon 2:ce3ee4bc8cf7 348 WMbox[3] = xx;UUpdate();
siwakon 2:ce3ee4bc8cf7 349 WMbox[4] = yy;UUpdate();
siwakon 2:ce3ee4bc8cf7 350 WMbox[5] = zz;UUpdate();
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 {
siwakon 2:ce3ee4bc8cf7 358 WMbox[0] = -1;UUpdate();
siwakon 2:ce3ee4bc8cf7 359 WMbox[1] = -1;UUpdate();
siwakon 2:ce3ee4bc8cf7 360 WMbox[2] = -1;UUpdate();
arthicha 0:a291977ec0b1 361 }else
arthicha 0:a291977ec0b1 362 {
siwakon 2:ce3ee4bc8cf7 363 WMbox[3] = -1;UUpdate();
siwakon 2:ce3ee4bc8cf7 364 WMbox[4] = -1;UUpdate();
siwakon 2:ce3ee4bc8cf7 365 WMbox[5] = -1;UUpdate();
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 {
siwakon 2:ce3ee4bc8cf7 379 A = ((i-2)* Cross(pitch))+((k-2)* Zine(pitch));UUpdate();
siwakon 2:ce3ee4bc8cf7 380 B = ((j-2)* Cross(yaw))+((i-2)* Zine(pitch)* Zine(yaw))-((k-2)* Zine(yaw)* Cross(pitch));UUpdate();
siwakon 2:ce3ee4bc8cf7 381 C = ((j-2)* Zine(yaw))-((i-2)* Cross(yaw)* Zine(pitch))+((k-2)* Cross(yaw)* Cross(pitch));UUpdate();
siwakon 2:ce3ee4bc8cf7 382 rmap[i+2][j+2][k+2]= map[i][j][k];UUpdate();
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 {
siwakon 2:ce3ee4bc8cf7 394 A = ((i-2)* Cross(yaw)) + ((j-2)* Zine(role)* Zine(yaw)) +((k-2)* Zine(yaw)* Cross(role));UUpdate();
siwakon 2:ce3ee4bc8cf7 395 B = ((j-2)* Cross(role))-((k-2)* Zine(role));UUpdate();
siwakon 2:ce3ee4bc8cf7 396 C = -((i-2)* Zine(yaw))+((j-2)* Cross(yaw)* Zine(role))+((k-2)* Cross(yaw)* Cross(role));UUpdate();
siwakon 2:ce3ee4bc8cf7 397 rmap[i+2][j+2][k+2]= map[i][j][k];UUpdate();
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 {
siwakon 2:ce3ee4bc8cf7 409 A = ((i-2)* Cross(yaw))-((j-2)* Zine(yaw));UUpdate();
siwakon 2:ce3ee4bc8cf7 410 B = ((i-2)* Zine(yaw))+((j-2)* Cross(yaw));UUpdate();
siwakon 2:ce3ee4bc8cf7 411 C = (k-2);UUpdate();
siwakon 2:ce3ee4bc8cf7 412 rmap[i+2][j+2][k+2]= map[i][j][k];UUpdate();
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 {
siwakon 2:ce3ee4bc8cf7 423 map[i][j][k]= rmap[i][j][k];UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 437 map[i][j][k] = 5;UUpdate(); // 5 = 'x'
siwakon 2:ce3ee4bc8cf7 438 turn++;UUpdate();
arthicha 0:a291977ec0b1 439 }
arthicha 0:a291977ec0b1 440 else{
siwakon 2:ce3ee4bc8cf7 441 Lose();UUpdate(); // ลงไมไ่ด้
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){
siwakon 2:ce3ee4bc8cf7 446 map[i][j][k] = 6;UUpdate(); // 6 = 'o'
siwakon 2:ce3ee4bc8cf7 447 turn++;UUpdate();
arthicha 0:a291977ec0b1 448 }
arthicha 0:a291977ec0b1 449 else{
siwakon 2:ce3ee4bc8cf7 450 Lose();UUpdate(); // ลงไมไ่ด้
arthicha 0:a291977ec0b1 451 }
arthicha 0:a291977ec0b1 452 }
arthicha 0:a291977ec0b1 453 if(i == 5 or j == 5 or k == 5){ // ลงเกิน map
siwakon 2:ce3ee4bc8cf7 454 Lose();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 461 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 464 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 474 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 477 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 487 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 490 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 500 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 503 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 513 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 516 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 526 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 529 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 539 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 542 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 552 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 555 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 565 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 568 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 577 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 580 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 589 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 592 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 601 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 604 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 613 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 616 Win();UUpdate();
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){
siwakon 2:ce3ee4bc8cf7 634 Check1[ A][ B][ C] = 1;UUpdate();
arthicha 0:a291977ec0b1 635 }
arthicha 0:a291977ec0b1 636 else if( map[ A][ B][ C] == 6){
siwakon 2:ce3ee4bc8cf7 637 Check2[ A][ B][ C] = 1;UUpdate();
arthicha 0:a291977ec0b1 638 }
arthicha 0:a291977ec0b1 639 else{
siwakon 2:ce3ee4bc8cf7 640 Check1[ A][ B][ C] = 0;UUpdate();
siwakon 2:ce3ee4bc8cf7 641 Check2[ A][ B][ C] = 0;UUpdate();
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
arthicha 0:a291977ec0b1 650
arthicha 0:a291977ec0b1 651 int main() // main program of
arthicha 0:a291977ec0b1 652 {
arthicha 1:d8ce226c8c2e 653 /*lim.rise(&Rise1);
arthicha 0:a291977ec0b1 654 lim.fall(&Fall1);
arthicha 0:a291977ec0b1 655 rotate.rise(&Rise2);
arthicha 0:a291977ec0b1 656 rotate.fall(&Fall2);
arthicha 0:a291977ec0b1 657 choose.rise(&Rise3);
arthicha 1:d8ce226c8c2e 658 choose.fall(&Fall3);*/
arthicha 1:d8ce226c8c2e 659 /*for(int i=0;i<200;i++)
arthicha 1:d8ce226c8c2e 660 {
arthicha 1:d8ce226c8c2e 661 UUpdate();
arthicha 1:d8ce226c8c2e 662 }*/
arthicha 1:d8ce226c8c2e 663 //LInitial(); // set initial pattern in led matrix.
siwakon 2:ce3ee4bc8cf7 664 wait(5); // wait 5 second for putting the box down to set initial position of imu.
arthicha 0:a291977ec0b1 665 //imu.Initial(); // set initial for imu.
arthicha 1:d8ce226c8c2e 666 //Update.attach(&UUpdate,0.1); //timer interrupt for recieving bluetooth.
arthicha 1:d8ce226c8c2e 667 //while((dete[0]!='i')||(dete[1]!='n')||(dete[2]!='i')||(dete[3]!='t'))
arthicha 1:d8ce226c8c2e 668 //{
arthicha 1:d8ce226c8c2e 669 //}
arthicha 0:a291977ec0b1 670 LWbox(); // set led to tell the player that the game is ready and this is the woman-box.
arthicha 0:a291977ec0b1 671 t.start();
arthicha 0:a291977ec0b1 672 while (1) // void loop.
arthicha 0:a291977ec0b1 673 {
siwakon 2:ce3ee4bc8cf7 674 state = 1;UUpdate();
arthicha 1:d8ce226c8c2e 675 UUpdate();
arthicha 0:a291977ec0b1 676 if (buttonState[0] == 1) // if the boxes are connected.
arthicha 0:a291977ec0b1 677 {
siwakon 2:ce3ee4bc8cf7 678 x = GetCoordinateX();UUpdate();
siwakon 2:ce3ee4bc8cf7 679 y = GetCoordinateY();UUpdate();
siwakon 2:ce3ee4bc8cf7 680 z = GetCoordinateZ();UUpdate(); // get the position of main box which linking with map or the box not be choosen.
arthicha 0:a291977ec0b1 681 /*Sent("helr\n",0.1,5); // tell the other box, that we want helper roll.
arthicha 0:a291977ec0b1 682 while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='r'))
arthicha 0:a291977ec0b1 683 {
arthicha 0:a291977ec0b1 684 pc.printf("srt %s\n\r",dete);
arthicha 0:a291977ec0b1 685 }
arthicha 0:a291977ec0b1 686 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 687 Sent("help\n",0.1,5); // tell the other box, that we want helper roll.
arthicha 0:a291977ec0b1 688 while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='p'))
arthicha 0:a291977ec0b1 689 {
arthicha 0:a291977ec0b1 690 pc.printf("srt %s\n\r",dete);
arthicha 0:a291977ec0b1 691 }
arthicha 0:a291977ec0b1 692 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 693 Sent("hely\n",0.1,5); // tell the other box, that we want helper roll.
arthicha 0:a291977ec0b1 694 while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='y'))
arthicha 0:a291977ec0b1 695 {
arthicha 0:a291977ec0b1 696 pc.printf("srt %s\n\r",dete);
arthicha 0:a291977ec0b1 697 }
arthicha 0:a291977ec0b1 698 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 699 //Sent("ok\n",0.1,3); //free the otherbox.
arthicha 0:a291977ec0b1 700
siwakon 2:ce3ee4bc8cf7 701 errorie = imu.Getposition(x,y,z);//,helperRoll,helperPitch,helperYaw);UUpdate(); // calculate position from imu.
arthicha 0:a291977ec0b1 702 if (errorie)
arthicha 0:a291977ec0b1 703 {
siwakon 2:ce3ee4bc8cf7 704 LError();UUpdate();
arthicha 0:a291977ec0b1 705 while(buttonState[0] == 1)
arthicha 0:a291977ec0b1 706 {
arthicha 1:d8ce226c8c2e 707 UUpdate();
arthicha 0:a291977ec0b1 708 }
arthicha 0:a291977ec0b1 709 }else
arthicha 0:a291977ec0b1 710 {
siwakon 2:ce3ee4bc8cf7 711 lx = x;UUpdate();
siwakon 2:ce3ee4bc8cf7 712 ly = y;UUpdate();
siwakon 2:ce3ee4bc8cf7 713 lz = z;UUpdate();
siwakon 2:ce3ee4bc8cf7 714 x = imu.X_pos;UUpdate();
siwakon 2:ce3ee4bc8cf7 715 y = imu.Y_pos;UUpdate();
siwakon 2:ce3ee4bc8cf7 716 z = imu.Z_pos;UUpdate(); // get new possition.
arthicha 0:a291977ec0b1 717
siwakon 2:ce3ee4bc8cf7 718 UpdateMap(x,y,z,turn);UUpdate();//,helperRoll); //update new position in map.
arthicha 0:a291977ec0b1 719
siwakon 2:ce3ee4bc8cf7 720 ChangeOX(x,y,z,lx,ly,lz);UUpdate();
arthicha 0:a291977ec0b1 721
arthicha 0:a291977ec0b1 722 Sent("mapc\n",0.1,4); //map complete.
siwakon 2:ce3ee4bc8cf7 723 LMapComplete();UUpdate(); // show that this position is ok.
arthicha 0:a291977ec0b1 724 while(1)
arthicha 0:a291977ec0b1 725 {
arthicha 1:d8ce226c8c2e 726 UUpdate();
siwakon 2:ce3ee4bc8cf7 727 anach = chooseAnother() ;UUpdate();// wheather thr others box is choosen.
arthicha 0:a291977ec0b1 728 if ((buttonState[2]==1) && (!anach))
arthicha 0:a291977ec0b1 729 {
arthicha 0:a291977ec0b1 730 Sent("ch1\n",0.1,4);
arthicha 0:a291977ec0b1 731
siwakon 2:ce3ee4bc8cf7 732 Choose(1);UUpdate(); // choose main box.
arthicha 0:a291977ec0b1 733 }else if((buttonState[2]==0) && (anach))
arthicha 0:a291977ec0b1 734 {
siwakon 2:ce3ee4bc8cf7 735 Choose(0);UUpdate(); // choose another box.
arthicha 0:a291977ec0b1 736
arthicha 0:a291977ec0b1 737 }else if(buttonState[2]==0)
arthicha 0:a291977ec0b1 738 {
arthicha 0:a291977ec0b1 739 if((buttonState[1]==0)||(anotherRotate())) // click buutton some rotate box.
arthicha 0:a291977ec0b1 740 {
siwakon 2:ce3ee4bc8cf7 741 state = 3;UUpdate();
arthicha 0:a291977ec0b1 742 while((buttonState[1]==0)&&(anotherRotate()))
arthicha 0:a291977ec0b1 743 {
arthicha 1:d8ce226c8c2e 744 UUpdate();
arthicha 0:a291977ec0b1 745 }
arthicha 0:a291977ec0b1 746
siwakon 2:ce3ee4bc8cf7 747 yyaaww = imu.Rotate();UUpdate(); // check wheater rotating is error.
siwakon 2:ce3ee4bc8cf7 748 MapRotate(0,0,yyaaww);UUpdate();//imu.Rotate_Yaw); // private variable/arttribute in imu class.
siwakon 2:ce3ee4bc8cf7 749 LRotateComplete();UUpdate();
arthicha 0:a291977ec0b1 750 }
arthicha 0:a291977ec0b1 751 } //close choose or rotate.
arthicha 0:a291977ec0b1 752 } //close while true.
siwakon 2:ce3ee4bc8cf7 753 turn ++;UUpdate();
arthicha 0:a291977ec0b1 754 Sent("T++\n",0.1,4);
siwakon 2:ce3ee4bc8cf7 755 t.start();UUpdate();
siwakon 2:ce3ee4bc8cf7 756 state = 2;UUpdate();
arthicha 0:a291977ec0b1 757 //LChooseComplete();
arthicha 0:a291977ec0b1 758 } // close not error.
arthicha 0:a291977ec0b1 759
arthicha 0:a291977ec0b1 760 }else // if the boxes aren't connect.
arthicha 0:a291977ec0b1 761 {
arthicha 0:a291977ec0b1 762 if (county == 0) // time out.
arthicha 0:a291977ec0b1 763 {
siwakon 2:ce3ee4bc8cf7 764 t.stop();UUpdate();
siwakon 2:ce3ee4bc8cf7 765 Lose();UUpdate(); // lose state
arthicha 0:a291977ec0b1 766 }else if(turn != 0)
arthicha 0:a291977ec0b1 767 {
siwakon 2:ce3ee4bc8cf7 768 t.stop();UUpdate();
siwakon 2:ce3ee4bc8cf7 769 if (t.read() > 1.0)
arthicha 0:a291977ec0b1 770 {
siwakon 2:ce3ee4bc8cf7 771 t.start();UUpdate();
siwakon 2:ce3ee4bc8cf7 772 county -= 1;UUpdate(); // time decreasing.
arthicha 0:a291977ec0b1 773 }
arthicha 0:a291977ec0b1 774 } // close check time
siwakon 2:ce3ee4bc8cf7 775 LCountDown(county);UUpdate();
arthicha 0:a291977ec0b1 776 Sent("Tur\n",0.1,4);
arthicha 0:a291977ec0b1 777 } // close boxes aren't connect.
arthicha 0:a291977ec0b1 778
arthicha 0:a291977ec0b1 779 } // close loop.
arthicha 0:a291977ec0b1 780
arthicha 0:a291977ec0b1 781
arthicha 0:a291977ec0b1 782
arthicha 0:a291977ec0b1 783 } // close main.*/