main.cpp

00001 #include "mbed.h"
00002 #include "bmu.h"
00003 
00004 InterruptIn lim(D3);
00005 InterruptIn rotate(D4);
00006 InterruptIn choose(D5);
00007 //Ticker flipper;
00008 Ticker Update;
00009 
00010 bmuimu imu;
00011 
00012 Serial pc(USBTX,USBRX);
00013 Serial mas(D8,D2);
00014 Serial duino(PA_11, PA_12);
00015 
00016 Timer t;
00017 Timer timer;
00018 
00019 //*********************************************************************************************************************************
00020 //*********************************************************************************************************************************
00021 //*********************************************************************************************************************************
00022 //                                                          variable
00023 //*********************************************************************************************************************************
00024 //*********************************************************************************************************************************
00025 //*********************************************************************************************************************************
00026 bool anach = false;
00027 bool buttonState[3] = {0,0,0}; // button limit switchm rotate and choose state respectively.
00028 unsigned int county = 10; // county down.
00029 char data[50];
00030 char elem;
00031 int yyaaww;
00032 bool errorie = false; // store error in ambigous imu data. 
00033 int helperRoll = 0, helperPitch=0,helperYaw=0;
00034 unsigned int indx = 0;
00035 unsigned int state = 0;
00036 int dete[50];
00037 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};
00038 
00039 // state    0 = initial / start.
00040 //          1 = waiting for limit switch.
00041 
00042 unsigned int turn = 0; // turn.
00043 
00044 unsigned int x = 0, y = 0, z = 0,lx=0,ly=0,lz=0;
00045 int winStatus = 0;
00046 
00047 
00048 
00049 int A , B , C , player , button; 
00050 int WMbox[6];
00051 int map[5][5][5],rmap[5][5][5]; //store map.
00052 int Check1[5][5][5] , Check2[5][5][5]; //convert to x or o map.
00053 //*********************************************************************************************************************************
00054 //*********************************************************************************************************************************
00055 //*********************************************************************************************************************************
00056 //                                                          function
00057 //*********************************************************************************************************************************
00058 //*********************************************************************************************************************************
00059 //*********************************************************************************************************************************
00060 
00061 int Cross(int degree)
00062         {
00063             if (degree == 0)
00064             {
00065                 return 1;
00066             }else if (degree == 180)
00067             {
00068                 return -1;
00069             }else
00070             {
00071                 return 0;   
00072             }
00073         }
00074     
00075 int Zine(int degree)
00076         {
00077             if (degree == 90)
00078             {
00079                 return 1;
00080             }else if (degree == -90)
00081             {
00082                 return -1;
00083             }else
00084             {
00085                 return 0;   
00086             }
00087         }
00088 
00089 void Rise1()
00090 {
00091     buttonState[0] = 1;
00092 }
00093 
00094 void Fall1()
00095 {
00096     buttonState[0] = 0;   
00097 }
00098 void Rise2()
00099 {
00100     buttonState[1] = 1;
00101 }
00102 
00103 void Fall2()
00104 {
00105     buttonState[1] = 0;   
00106 }
00107 void Rise3()
00108 {
00109     buttonState[2] = 1;
00110 }
00111 
00112 void Fall3()
00113 {
00114     buttonState[2] = 0;   
00115 }
00116 
00117 void UUpdate()
00118 {
00119     imu.bmuupdate();   
00120 }
00121 
00122 void Sent(char arrayi[],float it,int siz)
00123 {
00124     for (int i = 0;i<siz;i++)
00125         {
00126             mas.putc(char(arrayi[i]));UUpdate();
00127             wait(it);UUpdate();
00128         }   
00129 }
00130 
00131 void getStr()
00132 {
00133     UUpdate();
00134     while (mas.readable())
00135     {
00136         
00137         elem = mas.getc();UUpdate();
00138         pc.printf("char is %d\n\r",elem);UUpdate();
00139         if ((elem == '\n'))
00140         {
00141             pc.printf("string is %s\n\r",data);UUpdate();   
00142             for(int i=0;i<indx;i++)
00143             {
00144                 if (indx > 2)
00145                 {
00146                     dete[i] = data[i];UUpdate();      
00147                 }
00148             }
00149             for(int i=0;i<30;i++)
00150             {
00151                 data[i] = '\0';
00152             }
00153             indx = 0;UUpdate();
00154             break;
00155         }else
00156         {
00157             data[indx] = elem;UUpdate();
00158             indx += 1;UUpdate();
00159         }
00160             
00161     }  
00162 }
00163 
00164 void LChackmape(){  //
00165     if (t.read() > 1) {
00166             
00167             
00168         duino.putc(lis[23]);UUpdate();
00169             
00170         timer.reset();UUpdate();
00171     }
00172     }
00173  
00174 void LWin(){  //ชนะ
00175     if ( timer.read() > 1) {
00176     //pc.printf("Writing!\n\r");
00177     duino.putc( lis[0]);UUpdate();
00178     duino.putc( lis[1]);UUpdate();
00179     duino.putc( lis[2]);UUpdate();
00180     timer.reset();UUpdate();
00181  }
00182  }
00183  
00184 void LCountDown(int ixxii){  //นับเวลา
00185     if ( timer.read() > 1) {
00186         duino.putc(ixxii);UUpdate();
00187         timer.reset();UUpdate();
00188                 }
00189     }
00190          
00191          
00192 void LCorrect(){  //เช็คถูก
00193             if ( timer.read() > 1) {
00194                 //pc.printf("Writing!\n\r");UUpdate();
00195                  duino.putc(lis[18]);UUpdate();
00196                  timer.reset();UUpdate();
00197                 }
00198             }
00199             
00200 void LError(){  // 
00201             if ( timer.read() > 1) {
00202                 //pc.printf("Writing!\n\r");UUpdate();
00203                  duino.putc( lis[19]);UUpdate();
00204                  timer.reset();UUpdate();
00205                 }
00206             }
00207 void LMapComplete(){  //ลงค่าเสร็จ
00208         if ( timer.read() > 1) {
00209                 //pc.printf("Writing!\n\r");UUpdate();
00210                 
00211                 
00212                  duino.putc( lis[20]);UUpdate();
00213                 
00214                 
00215                  timer.reset();UUpdate();}}
00216                        
00217 void LWbox(){  //ตัวหลัก
00218             if ( timer.read() > 1) {
00219                  duino.putc( lis[21]);UUpdate();
00220                  timer.reset();UUpdate();
00221                     }
00222     }
00223             
00224 void LRotateComplete(){  //หมุน
00225             if ( timer.read() > 1) {
00226                 //pc.printf("Writing!\n\r");UUpdate();
00227                 
00228                 
00229                  duino.putc( lis[22]);UUpdate();
00230                 
00231                 
00232                  timer.reset();UUpdate();
00233                         }
00234             }
00235 
00236 
00237             
00238 void LLose(){ //แพ้
00239             
00240             if ( t.read() > 1) {
00241                 //pc.printf("Writing!\n\r");
00242                  duino.putc( lis[3]);UUpdate();
00243                  duino.putc( lis[4]);UUpdate();
00244                  duino.putc( lis[5]);UUpdate();
00245                  duino.putc( lis[6]);UUpdate();
00246                  t.reset();UUpdate();
00247                 }
00248             if ( duino.readable())
00249                 {
00250                 //pc.printf("recieving %c",duino.getc());
00251                 }
00252          
00253             }    
00254 
00255 void Lose()
00256 {
00257     LLose();UUpdate();
00258     Sent("los\n",0.1,4);
00259 }
00260 
00261 void Win()
00262 {
00263     LWin();UUpdate();
00264     Sent("lwin\n",0.1,4);
00265 }
00266 
00267 
00268 
00269 bool chooseAnother()
00270 {
00271     Sent("cho\n",0.1,4);
00272     while(1)
00273     {
00274         if ((dete[0]=='c')&&(dete[1]=='h')&&(dete[2]=='0'))   
00275         {
00276             return true;
00277         }else if ((dete[0]=='c')&&(dete[1]=='h')&&(dete[2]=='x')) 
00278         {
00279             return false;
00280         }
00281     }
00282 }
00283 
00284 bool anotherRotate()
00285 {
00286     Sent("rot\n",0.1,4);
00287     while(1)
00288     {
00289         if ((dete[0]=='r')&&(dete[1]=='o')&&(dete[2]=='t'))   
00290         {
00291             return true;
00292         }else if ((dete[0]=='r')&&(dete[1]=='o')&&(dete[2]=='x')) 
00293         {
00294             return false;
00295         }
00296     }
00297 }
00298 
00299 
00300 int    GetCoordinateX()
00301         {
00302             if ( WMbox[0] != -1)
00303             {
00304               return  WMbox[0];   
00305             }else
00306             {
00307                 return  WMbox[3];   
00308             }
00309         }
00310         
00311         int   GetCoordinateY()
00312         {
00313             if ( WMbox[1] != -1)
00314             {
00315               return  WMbox[1];   
00316             }else
00317             {
00318                 return  WMbox[4];   
00319             }
00320         }
00321         
00322         int    GetCoordinateZ()
00323         {
00324             if ( WMbox[2] != -1)
00325             {
00326               return  WMbox[2];   
00327             }else
00328             {
00329                 return  WMbox[5];   
00330             }
00331         }
00332         
00333         void   ChangeOX(int xx, int yy,int zz,int lx,int ly,int lz)
00334         {
00335             if (xx != -1)
00336             {
00337                  WMbox[0] = xx;UUpdate();
00338                  WMbox[1] = yy;UUpdate();
00339                  WMbox[2] = zz;UUpdate();
00340                  WMbox[3] = lx;UUpdate();
00341                  WMbox[4] = ly;UUpdate();
00342                  WMbox[5] = lz;UUpdate();   
00343             }else
00344             {
00345                  WMbox[0] = lx;UUpdate();
00346                  WMbox[1] = ly;UUpdate();
00347                  WMbox[2] = lz;UUpdate();
00348                  WMbox[3] = xx;UUpdate();
00349                  WMbox[4] = yy;UUpdate();
00350                  WMbox[5] = zz;UUpdate();
00351             }
00352         }
00353         
00354         void   Choose(int box)
00355         {
00356             if (box)
00357             {
00358                  WMbox[0] = -1;UUpdate();
00359                  WMbox[1] = -1;UUpdate();
00360                  WMbox[2] = -1;UUpdate();   
00361             }else
00362             {
00363                  WMbox[3] = -1;UUpdate();
00364                  WMbox[4] = -1;UUpdate();
00365                  WMbox[5] = -1;UUpdate();   
00366             }
00367         }
00368         
00369         void MapRotate(int role,int pitch,int yaw)
00370         {
00371             if ((pitch == 90) || (pitch == -90)) //{{1,0,0},{0,cosy,-siny},{0,siny,cosy}}{{cosx,0,+sinx},{0,1,0},{-sinx,0,cosx}}
00372             {
00373                 for (int i=0;i<5;i++)
00374                 {
00375                     for (int j=0;j<5;j++)
00376                     {
00377                         for(int k=0;k<5;k++)
00378                         {
00379                              A =  ((i-2)* Cross(pitch))+((k-2)* Zine(pitch));UUpdate();
00380                              B =  ((j-2)* Cross(yaw))+((i-2)* Zine(pitch)* Zine(yaw))-((k-2)* Zine(yaw)* Cross(pitch));UUpdate();
00381                              C =  ((j-2)* Zine(yaw))-((i-2)* Cross(yaw)* Zine(pitch))+((k-2)* Cross(yaw)* Cross(pitch));UUpdate();
00382                              rmap[i+2][j+2][k+2]= map[i][j][k];UUpdate();
00383                         }   
00384                     }   
00385                 }
00386             }else if ((role == 90) || (role == -90)) // {{cosy,0,+siny},{0,1,0},{-siny,0,cosy}}{{1,0,0},{0,cosx,-sinx},{0,sinx,cosx}}
00387             {
00388                 for (int i=0;i<5;i++)
00389                 {
00390                     for (int j=0;j<5;j++)
00391                     {
00392                         for(int k=0;k<5;k++)
00393                         {
00394                              A =  ((i-2)* Cross(yaw))   + ((j-2)* Zine(role)* Zine(yaw)) +((k-2)* Zine(yaw)* Cross(role));UUpdate();
00395                              B =  ((j-2)* Cross(role))-((k-2)* Zine(role));UUpdate();
00396                              C =  -((i-2)* Zine(yaw))+((j-2)* Cross(yaw)* Zine(role))+((k-2)* Cross(yaw)* Cross(role));UUpdate();
00397                              rmap[i+2][j+2][k+2]= map[i][j][k];UUpdate();
00398                         }   
00399                     }   
00400                 } 
00401             }else
00402             {
00403                 for (int i=0;i<5;i++)
00404                 {
00405                     for (int j=0;j<5;j++)
00406                     {
00407                         for(int k=0;k<5;k++)
00408                         {
00409                              A =  ((i-2)* Cross(yaw))-((j-2)* Zine(yaw));UUpdate();
00410                              B =  ((i-2)* Zine(yaw))+((j-2)* Cross(yaw));UUpdate();
00411                              C =  (k-2);UUpdate();
00412                              rmap[i+2][j+2][k+2]= map[i][j][k];UUpdate();
00413                         }   
00414                     }   
00415                 } 
00416             }
00417             for (int i=0;i<5;i++)
00418                 {
00419                     for (int j=0;j<5;j++)
00420                     {
00421                         for(int k=0;k<5;k++)
00422                         {
00423                              map[i][j][k]= rmap[i][j][k];UUpdate();
00424                         }   
00425                     }   
00426                 }
00427         }
00428         
00429         void UpdateMap(int i,int j,int k,int turner){
00430         if(1){
00431             
00432             switch(turner%2){                             // เลือกตำแหน่งคนลง
00433                 
00434                 case '0':
00435                 i-- , j-- , k--;
00436                     if( map[i][j][k] == 0){
00437                          map[i][j][k] = 5;UUpdate();               // 5 = 'x'
00438                          turn++;UUpdate();
00439                         }
00440                     else{
00441                          Lose();UUpdate();                  // ลงไมไ่ด้
00442                         }
00443                 case '1':
00444                     i-- , j-- , k--;
00445                     if( map[i][j][k] == 0){
00446                          map[i][j][k] = 6;UUpdate();               // 6 = 'o'
00447                          turn++;UUpdate();
00448                         }
00449                     else{
00450                          Lose();UUpdate();                  // ลงไมไ่ด้
00451                         }
00452                 }
00453             if(i == 5 or j == 5 or k == 5){             // ลงเกิน map
00454                  Lose();UUpdate();
00455                 }
00456             
00457             for(i=0 ; i<5 ; i++){          //แนว k
00458                 for(j=0 ; j<5 ; j++){
00459                     for(k=0 ; k<3 ; k++){
00460                         if( map[i][j][k] and  map[i][j][k+1] and  map[i][j][k+2] == 5){
00461                              Win();UUpdate();
00462                                 }
00463                         else if( map[i][j][k] and  map[i][j][k+1] and  map[i][j][k+2] == 6){
00464                              Win();UUpdate();
00465                                 }
00466                             }
00467                         }
00468                     }
00469             
00470             for(i=0 ; i<5 ; i++){           // แนว j
00471                 for(k=0 ; k<5 ; k++){
00472                     for(j=0 ; j<3 ; j++){   
00473                         if( map[i][j][k] and  map[i][j+1][k] and  map[i][j+2][k] == 5){
00474                              Win();UUpdate();
00475                                 }
00476                         else if( map[i][j][k] and  map[i][j+1][k] and  map[i][j+2][k] == 6){
00477                              Win();UUpdate();
00478                                 }
00479                             }
00480                         }
00481                     }  
00482             
00483             for(j=0 ; j<5 ; j++){           // แนว i
00484                 for(k=0 ; k<5 ; k++){
00485                     for(i=0 ; i<3 ; i++){
00486                         if( map[i][j][k] and  map[i+1][j][k] and  map[i+2][j][k] == 5){
00487                              Win();UUpdate();
00488                                 }
00489                         else if( map[i][j][k] and  map[i+1][j][k] and  map[i+2][j][k] == 6){
00490                              Win();UUpdate();
00491                                 }
00492                             }
00493                         }
00494                     } 
00495                     
00496             for(i=0 ; i<5 ; i++){       // แนวทแยง i เป็นฐานขึ้น
00497                 for(j=0 ; j<3 ; j++){
00498                     for(k=0 ; k<3 ; k++){
00499                         if( map[i][j][k] and  map[i][j+1][k+1] and  map[i][j+2][k+2] == 5){
00500                               Win();UUpdate();
00501                             }
00502                         else if( map[i][j][k] and  map[i][j+1][k+1] and  map[i][j+2][k+2] == 6){
00503                               Win();UUpdate();
00504                             }
00505                         }
00506                     }
00507                 }
00508                 
00509             for(j=0 ; j<5 ; j++){       // แนวทแยง j เป็นฐานขึ้น
00510                 for(i=0 ; i<3 ; i++){
00511                     for(k=0 ; k<3 ; k++){
00512                         if( map[i][j][k] and  map[i+1][j][k+1] and  map[i+2][j][k+2] == 5){
00513                               Win();UUpdate();
00514                             }
00515                         else if( map[i][j][k] and  map[i+1][j][k+1] and  map[i+2][j][k+2] == 6){
00516                               Win();UUpdate();
00517                             }
00518                         }
00519                     }
00520                 }
00521                 
00522             for(k=0 ; k<5 ; k++){       // แนวทแยง k เป็นฐานขึ้น
00523                 for(i=0 ; i<3 ; i++){
00524                     for(j=0 ; j<3 ; j++){
00525                         if( map[i][j][k] and  map[i+1][j+1][k] and  map[i+2][j+2][k] == 5){
00526                              Win();UUpdate();
00527                             }
00528                         else if( map[i][j][k] and  map[i+1][j+1][k] and  map[i+2][j+2][k] == 6){
00529                               Win();UUpdate();
00530                             }
00531                         }
00532                     }
00533                 }
00534                 
00535             for(i=0 ; i<5 ; i++){       // แนวทแยง i เป็นฐานลง
00536                 for(j=0 ; j<3 ; j++){
00537                     for(k=4 ; k>1 ; k--){
00538                         if( map[i][j][k] and  map[i][j+1][k-1] and  map[i][j+2][k-2] == 5){
00539                               Win();UUpdate();
00540                             }
00541                         else if( map[i][j][k] and  map[i][j+1][k-1] and  map[i][j+2][k-2] == 6){
00542                               Win();UUpdate();
00543                             }
00544                         }
00545                     }
00546                 }
00547                 
00548             for(j=0 ; j<5 ; j++){       // แนวทแยง j เป็นฐานลง
00549                 for(i=0 ; i<3 ; i++){
00550                     for(k=4 ; k>1 ; k--){
00551                         if( map[i][j][k] and  map[i+1][j][k-1] and  map[i+2][j][k-2] == 5){
00552                               Win();UUpdate();
00553                             }
00554                         else if( map[i][j][k] and  map[i+1][j][k-1] and  map[i+2][j][k-2] == 6){
00555                               Win();UUpdate();
00556                             }
00557                         }
00558                     }
00559                 }
00560                 
00561              for(k=0 ; k<5 ; k++){      // แนวทแยง k เป็นฐานลง
00562                 for(i=0 ; i<3 ; i++){
00563                     for(j=4 ; j>1 ; j--){
00564                         if( map[i][j][k] and  map[i+1][j-1][k] and  map[i+2][j-2][k] == 5){
00565                               Win();UUpdate();
00566                             }
00567                         else if( map[i][j][k] and  map[i+1][j-1][k] and  map[i+2][j-2][k] == 6){
00568                               Win();UUpdate();
00569                             }
00570                         }
00571                     }
00572                 }
00573             for(i=0 ; i<3 ; i++){       // แนวทแยงแบบตัด1
00574                 for(j=0 ; j<3 ; j++){
00575                    for(k=0 ; k<3 ; k++){
00576                        if( map[i][j][k] and  map[i+1][j+1][k+1] and  map[i+2][j+2][k+2] == 5){
00577                              Win();UUpdate();
00578                            }   
00579                         else if( map[i][j][k] and  map[i+1][j+1][k+1] and  map[i+2][j+2][k+2] == 6){
00580                              Win();UUpdate();
00581                             }
00582                         }
00583                     }
00584                 }
00585             for(i=4 ; i>1 ; i--){       // แนวทแยงแบบตัด2
00586                 for(j=0 ; j<3 ; j++){
00587                    for(k=0 ; k<3 ; k++){
00588                        if( map[i][j][k] and  map[i-1][j+1][k+1] and  map[i-2][j+2][k+2] == 5){
00589                              Win();UUpdate();
00590                            }   
00591                         else if( map[i][j][k] and  map[i-1][j+1][k+1] and  map[i-2][j+2][k+2] == 6){
00592                              Win();UUpdate();
00593                             }
00594                         }
00595                     }
00596                 }
00597             for(i=4 ; i>1 ; i--){       // แนวทแยงแบบตัด 3
00598                 for(j=4 ; j>1 ; j--){
00599                    for(k=0 ; k<3 ; k++){
00600                        if( map[i][j][k] and  map[i-1][j-1][k+1] and  map[i-2][j-2][k+2] == 5){
00601                              Win();UUpdate();
00602                            }   
00603                         else if( map[i][j][k] and  map[i-1][j-1][k+1] and  map[i-2][j-2][k+2] == 6){
00604                              Win();UUpdate();
00605                             }
00606                         }
00607                     }
00608                 }
00609             for(i=0 ; i<3 ; i++){       // แนวทแยงแบบตัด4
00610                 for(j=4 ; j>1 ; j--){
00611                    for(k=0 ; k<3 ; k++){
00612                        if( map[i][j][k] and  map[i+1][j-1][k+1] and  map[i+2][j-2][k+2] == 5){
00613                              Win();UUpdate();
00614                            }   
00615                         else if( map[i][j][k] and  map[i+1][j-1][k+1] and  map[i+2][j-2][k+2] == 6){
00616                              Win();UUpdate();
00617                             }
00618                         }
00619                     }
00620                 }
00621                    
00622                     
00623                 }
00624                 
00625             }
00626      
00627         
00628         
00629         void   Check_map(){                   //แสดง map
00630             for( A=0 ;  A<5 ;  A++){
00631                 for( B=0 ;  B<5 ;  B++){ 
00632                     for( C=0 ;  C<5 ;  C++){
00633                         if( map[ A][ B][ C] == 5){
00634                              Check1[ A][ B][ C] = 1;UUpdate();
00635                             }
00636                         else if( map[ A][ B][ C] == 6){
00637                              Check2[ A][ B][ C] = 1;UUpdate();
00638                             }
00639                         else{
00640                              Check1[ A][ B][ C] = 0;UUpdate();
00641                              Check2[ A][ B][ C] = 0;UUpdate();
00642                             }
00643                         } 
00644                     }
00645                 }
00646             }
00647             
00648             
00649 
00650 
00651 int main() // main program of 
00652 {
00653     /*lim.rise(&Rise1);
00654     lim.fall(&Fall1);
00655     rotate.rise(&Rise2);
00656     rotate.fall(&Fall2);
00657     choose.rise(&Rise3);
00658     choose.fall(&Fall3);*/
00659     /*for(int i=0;i<200;i++)
00660     {
00661         UUpdate();
00662     }*/
00663     //LInitial(); // set initial pattern in led matrix.
00664     wait(5); // wait 5 second for putting the box down to set initial position of imu.
00665     //imu.Initial(); // set initial for imu.
00666     //Update.attach(&UUpdate,0.1); //timer interrupt for recieving bluetooth. 
00667     //while((dete[0]!='i')||(dete[1]!='n')||(dete[2]!='i')||(dete[3]!='t'))
00668     //{
00669     //}
00670     LWbox(); // set led to tell the player that the game is ready and this is the woman-box.
00671     t.start();
00672     while (1) // void loop.
00673     {
00674         state = 1;UUpdate();
00675         UUpdate();
00676         if (buttonState[0] == 1) // if the boxes are connected.
00677         {
00678             x = GetCoordinateX();UUpdate();
00679             y = GetCoordinateY();UUpdate(); 
00680             z = GetCoordinateZ();UUpdate(); // get the position of main box which linking with map or the box not be choosen.
00681             /*Sent("helr\n",0.1,5); // tell the other box, that we want helper roll.
00682             while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='r'))
00683             {
00684                 pc.printf("srt %s\n\r",dete);     
00685             }
00686             helperRoll = int(  ((int(dete[4+ii])-48)*100)+((int(dete[5+ii])-48)*10)+(int(dete[6+ii])-48)  ); //get roll from another box.
00687             Sent("help\n",0.1,5); // tell the other box, that we want helper roll.
00688             while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='p'))
00689             {
00690                 pc.printf("srt %s\n\r",dete);     
00691             }
00692             helperPitch = int(  ((int(dete[4+ii])-48)*100)+((int(dete[5+ii])-48)*10)+(int(dete[6+ii])-48)  ); //get roll from another box.
00693             Sent("hely\n",0.1,5); // tell the other box, that we want helper roll.
00694             while((dete[0]!='h')||(dete[1]!='e')||(dete[2]!='l')||(dete[3]!='y'))
00695             {
00696                 pc.printf("srt %s\n\r",dete);     
00697             }
00698             helperYaw = int(  ((int(dete[4+ii])-48)*100)+((int(dete[5+ii])-48)*10)+(int(dete[6+ii])-48)  ); //get roll from another box.*/
00699             //Sent("ok\n",0.1,3); //free the otherbox.
00700             
00701             errorie = imu.Getposition(x,y,z);//,helperRoll,helperPitch,helperYaw);UUpdate(); // calculate position from imu.
00702             if (errorie)
00703             {
00704                 LError();UUpdate();
00705                 while(buttonState[0] == 1)
00706                 {
00707                    UUpdate();    
00708                 }
00709             }else
00710             {
00711                 lx = x;UUpdate();
00712                 ly = y;UUpdate();
00713                 lz = z;UUpdate();
00714                 x = imu.X_pos;UUpdate();
00715                 y = imu.Y_pos;UUpdate();
00716                 z = imu.Z_pos;UUpdate(); // get new possition.
00717                 
00718                 UpdateMap(x,y,z,turn);UUpdate();//,helperRoll); //update new position in map.
00719                 
00720                 ChangeOX(x,y,z,lx,ly,lz);UUpdate();
00721                 
00722                     Sent("mapc\n",0.1,4); //map complete.
00723                     LMapComplete();UUpdate(); // show that this position is ok.
00724                     while(1)
00725                     {
00726                         UUpdate();
00727                         anach = chooseAnother() ;UUpdate();// wheather thr others box is choosen. 
00728                         if ((buttonState[2]==1) && (!anach)) 
00729                         {
00730                             Sent("ch1\n",0.1,4);
00731                             
00732                             Choose(1);UUpdate(); // choose main box.
00733                         }else if((buttonState[2]==0) && (anach))
00734                         {
00735                             Choose(0);UUpdate(); // choose another box.
00736                             
00737                         }else if(buttonState[2]==0) 
00738                         {
00739                             if((buttonState[1]==0)||(anotherRotate())) // click buutton some rotate box.
00740                             {
00741                                 state = 3;UUpdate();
00742                                 while((buttonState[1]==0)&&(anotherRotate()))
00743                                 {
00744                                     UUpdate();
00745                                 }
00746                                 
00747                                 yyaaww = imu.Rotate();UUpdate(); // check wheater rotating is error. 
00748                                 MapRotate(0,0,yyaaww);UUpdate();//imu.Rotate_Yaw); // private variable/arttribute in imu class.
00749                                 LRotateComplete();UUpdate();  
00750                             }  
00751                         } //close choose or rotate.
00752                      } //close while true.
00753                      turn ++;UUpdate(); 
00754                      Sent("T++\n",0.1,4);
00755                      t.start();UUpdate();
00756                      state = 2;UUpdate();
00757                      //LChooseComplete();
00758             } // close not error.
00759             
00760         }else // if the boxes aren't connect.
00761         {
00762             if (county == 0) // time out.
00763             {
00764                 t.stop();UUpdate();
00765                 Lose();UUpdate(); // lose state
00766             }else if(turn != 0)
00767             {
00768                 t.stop();UUpdate();
00769                 if (t.read() > 1.0)
00770                 {
00771                     t.start();UUpdate();
00772                     county -= 1;UUpdate(); // time decreasing.
00773                 }
00774             } // close check time
00775             LCountDown(county);UUpdate();
00776             Sent("Tur\n",0.1,4);
00777         } // close boxes aren't connect.
00778         
00779     } // close loop.
00780     
00781     
00782     
00783 } // close main.*/