Vytah.cpp

00001  #include "mbed.h"
00002  #include "Header.h"
00003  #include "adresarik.h"
00004 
00005 
00006 
00007 int main()
00008 {
00009      term.baud(57600); 
00010      term.attach(&prerusVysielanie, Serial::RxIrq);
00011      
00012      inicializuj();
00013      sleduj_cas.attach(&watchdog, 1.4);
00014      sleduj_lcd.attach(&lcd_display,0.5);
00015      acc.MotionDetection(&nudzovaBrzda);
00016      
00017      while(1) {
00018         tlacitkoStena();
00019         tlacitkoVytah();   
00020     }
00021  }
00022  
00023  /*****KOmunikacia******************************************************/
00024  /*sekcia ktora riadi komunikaciu*/
00025  
00026  void posliSpravu(int addrRx, int addrTx,int dataSize, int aData[])
00027  {
00028      
00029      int aPacket[5+dataSize];
00030     aPacket[0] = 0xA0;
00031     aPacket[1] = addrRx;
00032     aPacket[2] = addrTx;
00033     aPacket[3] = dataSize;
00034     
00035     for(int i = 0; i < dataSize; i++){
00036         aPacket[4+i] = aData[i];
00037     }
00038     
00039     int crc = getCrc(addrRx, addrTx, dataSize, aData);
00040     aPacket[4+dataSize] = crc;
00041     
00042     
00043     prijmi = 0;
00044     
00045     for(int i = 0; i < ARRAY_SIZE(aPacket); i++){
00046         term.putc(aPacket[i]);       
00047     }
00048 }
00049 
00050 int getCrc(int addrRx, int addrTx, int dataSize, int aData[])
00051 {
00052     int aCRC[] =   {0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
00053                     157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
00054                     35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
00055                     190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
00056                     70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
00057                     219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
00058                     101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
00059                     248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
00060                     140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
00061                     17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
00062                     175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
00063                     50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
00064                     202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
00065                     87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
00066                     233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
00067                     116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53
00068                    };              
00069     int crc = 0;
00070     crc = aCRC[crc ^ addrRx];
00071     crc = aCRC[crc ^ addrTx];
00072     for(int i = 0; i < dataSize; i++){
00073         crc = aCRC[crc ^ aData[i]];
00074     }
00075     return crc;    
00076 }
00077 
00078 void inicializuj()
00079 {
00080     inicializacia = 1;
00081     resetuj_watchdog();
00082     uvolniNB();
00083     dole();   
00084 }
00085 
00086 void prerusVysielanie()
00087 {
00088    
00089     
00090     prijmi = 1;
00091     for(int i = 0; i < 4; i++) {
00092         adresa[i] = term.getc();
00093     }
00094     
00095    
00096     if(adresa[3] == 0x00) {
00097         sprava[0] = term.getc();
00098     }else{        
00099         for(int i = 0; i < adresa[3]; i++){
00100             sprava[i]=term.getc();  
00101         }
00102         adresa[4]=term.getc();
00103         if(adresa[4] == getCrc(adresa[1], adresa[2],adresa[3],sprava)){
00104           
00105         }
00106     }
00107     
00108     riadeniePohybu();
00109     
00110    
00111 }
00112 /****Pohy**************************************************************************/
00113 /***sekcia Pohybu******************************************************************/
00114 
00115 
00116 
00117 int dajPoziciu()
00118 {
00119     int pozicia;
00120     switch(adresa[2]){
00121         case SWITCH_P:{
00122             pozicia = SWITCH_P;
00123            cisloPochodia_dislpej = 0;
00124             break;
00125         }
00126         case SWITCH_1:{
00127             pozicia = SWITCH_1;
00128            cisloPochodia_dislpej = 1;
00129             break;
00130         }
00131         case SWITCH_2:{
00132             pozicia = SWITCH_2;
00133            cisloPochodia_dislpej = 2;
00134             break;
00135         }
00136         case SWITCH_3:{
00137             pozicia = SWITCH_3;
00138            cisloPochodia_dislpej = 3;
00139             break;
00140         }
00141         case SWITCH_4:{
00142             pozicia = SWITCH_4;
00143            cisloPochodia_dislpej = 4;
00144             break;
00145         }
00146     }  
00147     return pozicia;  
00148 }
00149 
00150 void tlacitkoVytah()
00151 {
00152     if(prijmi == 1){            
00153         switch(adresa[2]){
00154             case C_P_BUTTON:{
00155                 
00156                 p_0 = 1;
00157                 init_kabina++;
00158                 if(kere_poschodie > 0){
00159                     dole();    
00160                 }else if(kere_poschodie < 0){
00161                     hore();    
00162                 }else{
00163                     if(init_pohyb == 0){
00164                         p_0 = 0;
00165                         
00166                     }    
00167                 }
00168                 prijmi = 0;
00169                 break;
00170             }
00171             case C_1_BUTTON:{
00172                
00173                 p_1 = 1; 
00174                 init_kabina++;
00175                 if(kere_poschodie > 1){
00176                     dole();    
00177                 }else if(kere_poschodie < 1){
00178                     hore();    
00179                 }else{
00180                     if(init_pohyb == 0){
00181                         p_1 = 0;
00182                        
00183                     }  
00184                 }
00185                 prijmi = 0;
00186                 break;
00187             }
00188             case C_2_BUTTON:{
00189                
00190                 p_2 = 1;
00191                 init_kabina++;
00192                 if(kere_poschodie > 2){
00193                     dole();    
00194                 }else if(kere_poschodie < 2){
00195                     hore();    
00196                 }else{
00197                     if(init_pohyb == 0){
00198                         p_2 = 0;
00199                          
00200                     }  
00201                 }
00202                 prijmi = 0;
00203                 break;
00204             }
00205             case C_3_BUTTON:{
00206                 
00207                 p_3 = 1;
00208                 init_kabina++;
00209                 if(kere_poschodie > 3){
00210                     dole();    
00211                 }else if(kere_poschodie < 3){
00212                     hore();    
00213                 }else{
00214                     if(init_pohyb == 0){
00215                         p_3 = 0;
00216                        
00217                     }   
00218                 }
00219                 prijmi = 0;
00220                 break;
00221             }
00222             case C_4_BUTTON:{
00223               
00224                 p_4 = 1;
00225                 init_kabina++;
00226                 if(kere_poschodie > 4){
00227                     dole();    
00228                 }else if(kere_poschodie < 4){
00229                     hore();    
00230                 }else{
00231                     if(init_pohyb == 0){
00232                         p_4 = 0;
00233                         
00234                     } 
00235                 }
00236                 prijmi = 0;
00237                 break;
00238             }
00239         }              
00240     }
00241 } 
00242 
00243 void tlacitkoStena()
00244 {
00245     if(prijmi == 1){            
00246         switch(adresa[2]){
00247             case W_P_BUTTON:{
00248               
00249                 p_0 = 1;
00250                 stena++;
00251                 if(kere_poschodie > 0){
00252                     dole();    
00253                 }else if(kere_poschodie < 0){
00254                     hore();    
00255                 }else{
00256                     if(init_pohyb == 0){
00257                         p_0 = 0;
00258                         
00259                     }   
00260                 }
00261                 prijmi = 0;
00262                 break;
00263             }
00264             case W_1_BUTTON:{
00265                 
00266                 p_1 = 1; 
00267                 stena++;
00268                 if(kere_poschodie > 1){
00269                     dole();    
00270                 }else if(kere_poschodie < 1){
00271                     hore();    
00272                 }else{
00273                     if(init_pohyb == 0){
00274                         p_1 = 0;
00275                         
00276                     }    
00277                 }
00278                 prijmi = 0;
00279                 break;
00280             }
00281             case W_2_BUTTON:{
00282                
00283                 p_2 = 1;
00284                 stena++;
00285                 if(kere_poschodie > 2){
00286                     dole();    
00287                 }else if(kere_poschodie < 2){
00288                     hore();    
00289                 }else{
00290                     if(init_pohyb == 0){
00291                         p_2 = 0;
00292                        
00293                     }    
00294                 }
00295                 prijmi = 0;
00296                 break;
00297             }
00298             case W_3_BUTTON:{
00299                
00300                 p_3 = 1;
00301                 stena++;
00302                 if(kere_poschodie > 3){
00303                     dole();    
00304                 }else if(kere_poschodie < 3){
00305                     hore();    
00306                 }else{
00307                     if(init_pohyb == 0){
00308                         p_3 = 0;
00309                         
00310                     }   
00311                 }
00312                 prijmi = 0;
00313                 break;
00314             }
00315             case W_4_BUTTON:{
00316                
00317                 p_4 = 1;
00318                 stena++;
00319                 if(kere_poschodie > 4){
00320                     dole();    
00321                 }else if(kere_poschodie < 4){
00322                     hore();    
00323                 }else{
00324                     if(init_pohyb == 0){
00325                         p_4 = 0;
00326                        
00327                     }   
00328                 }
00329                 prijmi = 0;
00330                 break;
00331             }
00332         }              
00333     }
00334 }
00335 
00336 void dockajNaPoschodi(){
00337     dockaj++;
00338     if(dockaj > 5){
00339         dockaj  = 0;
00340         cakajNaPoschodi.detach();
00341         pohniSa();
00342     }     
00343 }
00344 
00345 void zamkni()
00346 {
00347     int aDataIn[] = {0x01};
00348     int dataSize = ARRAY_SIZE(aDataIn);
00349     posliSpravu(C_LOCK,0x00,dataSize, aDataIn);  
00350 } 
00351 void unzamkni()
00352 {
00353     int aDataIn[] = {0x00};
00354     int dataSize = ARRAY_SIZE(aDataIn);
00355     posliSpravu(C_LOCK,0x00,dataSize, aDataIn);  
00356 } 
00357 
00358 void hore()
00359 {  
00360     if(dockaj == 0){
00361         pohyb++;
00362         if(init_pohyb == 0){
00363             init_pohyb = 1;
00364             zamkni();
00365             int aDataIn[] = {0x2, 0x64, 0x00, 0x00, 0x00};
00366             int dataSize = ARRAY_SIZE(aDataIn);
00367             posliSpravu(MOTOR,0x00,dataSize, aDataIn); 
00368             smer = 1;
00369         }
00370         if(pohyb > 2){
00371             pohyb = 0;
00372             init_pohyb = 1;
00373             zamkni();
00374             int aDataIn[] = {0x2, 0x64, 0x00, 0x00, 0x00};
00375             int dataSize = ARRAY_SIZE(aDataIn);
00376             posliSpravu(MOTOR,0x00,dataSize, aDataIn); 
00377             smer = 1;    
00378         }
00379     }           
00380 }
00381  
00382 void dole()
00383 {
00384     if(dockaj == 0){
00385         pohyb++;
00386         if(init_pohyb == 0){
00387             init_pohyb = -1;
00388             zamkni();
00389             int aDataIn[] = {0x2, 0x9C, 0xff, 0xff, 0xff};
00390             int dataSize = ARRAY_SIZE(aDataIn);
00391             posliSpravu(MOTOR,0x00,dataSize, aDataIn); 
00392             smer = -1;
00393         }
00394         if(pohyb > 2){
00395             pohyb = 0;
00396             init_pohyb = -1;
00397             zamkni();
00398             int aDataIn[] = {0x2, 0x9C, 0xff, 0xff, 0xff};
00399             int dataSize = ARRAY_SIZE(aDataIn);
00400             posliSpravu(MOTOR,0x00,dataSize, aDataIn); 
00401             smer = -1;   
00402         }
00403     }   
00404 }
00405 
00406 void zastav()
00407 {
00408    if(dockaj == 0){
00409         init_pohyb = 0;
00410         int aDataIn[] = {0x1};
00411         int dataSize = ARRAY_SIZE(aDataIn);
00412         posliSpravu(MOTOR,0x00,dataSize, aDataIn); 
00413         delayer.attach(&zamkni, 0.4);  
00414     } 
00415 }
00416 
00417 
00418 void pohniSa()
00419 {
00420     switch(kere_poschodie){
00421         case 0:{
00422             if (p_1 == 1 || p_2 == 1 || p_3 == 1 || p_4 == 1){
00423                 hore();    
00424             }  
00425             break; 
00426         } 
00427         case 1:{
00428             if(smer == -1){
00429                 if(p_0 == 1){
00430                     dole();    
00431                 }else if (p_2 == 1 || p_3 == 1 || p_4 == 1){
00432                     hore();    
00433                 }    
00434             }else if(smer == 1){
00435                 if (p_2 == 1 || p_3 == 1 || p_4 == 1){
00436                     hore();    
00437                 }else if(p_0 == 1){
00438                     dole();    
00439                 }    
00440             }  
00441             break; 
00442         }
00443         case 2:{
00444             if(smer == -1){
00445                 if(p_0 == 1 || p_1 == 1){
00446                     dole();
00447                 }else if(p_3 == 1 || p_4 == 1){
00448                     hore();    
00449                 }   
00450             }else if(smer == 1){
00451                 if(p_3 == 1 || p_4 == 1){
00452                     hore();    
00453                 }else if(p_0 == 1 || p_1 == 1){
00454                     dole();    
00455                 }   
00456             }    
00457             break; 
00458         }
00459         case 3:{
00460             if(smer == -1){
00461                 if (p_0 == 1 || p_1 == 1 || p_2 == 1){
00462                     dole();    
00463                 }else if(p_4 == 1){
00464                     hore();    
00465                 }         
00466             }else if(smer == 1){
00467                 if(p_4 == 1){
00468                     hore();    
00469                 }else if(p_0 == 1 || p_1 == 1 || p_2 == 1){
00470                     dole();    
00471                 }   
00472             }   
00473             break; 
00474         }
00475         case 4:{
00476             if(p_0 == 1 || p_1 == 1 || p_2 == 1 || p_3 == 1){
00477                 dole();    
00478             }  
00479             break; 
00480         }       
00481     }    
00482 }
00483  
00484 
00485 
00486 void nudzovaBrzda()
00487 {
00488     int aDataIn[] = {0x01};
00489     int dataSize = ARRAY_SIZE(aDataIn);
00490     posliSpravu(EM_BRAKE,0x00,dataSize, aDataIn);         
00491 }
00492  
00493 void uvolniNB()
00494 {
00495     int aDataIn[] = {0x00};
00496     int dataSize = ARRAY_SIZE(aDataIn);
00497     posliSpravu(EM_BRAKE,0x00,dataSize, aDataIn);         
00498 }
00499 
00500 void lcd_display()
00501 {
00502     int aDataIn[2];
00503     int dataSize = ARRAY_SIZE(aDataIn);  
00504     switch(init_pohyb){
00505         case 0:{
00506             aDataIn[0] = 0x03;
00507             break;    
00508         }
00509         case 1:{
00510             aDataIn[0] = 0x01;
00511             break;    
00512         } 
00513         case -1:{
00514             aDataIn[0] = 0x02; 
00515             break;   
00516         }     
00517     }
00518     switch(cisloPochodia_dislpej){
00519         case 0:{
00520             aDataIn[1] = 0x50;   
00521             break; 
00522         } 
00523         case 1:{
00524             aDataIn[1] = 0x31;   
00525             break; 
00526         }
00527         case 2:{
00528             aDataIn[1] = 0x32;   
00529             break; 
00530         }
00531         case 3:{
00532             aDataIn[1] = 0x33;   
00533             break; 
00534         }
00535         case 4:{
00536             aDataIn[1] = 0x34;   
00537             break; 
00538         }   
00539     }
00540     if(cisloPochodia_dislpej != disp_poschodie){
00541         posliSpravu(C_LCD,0x00,dataSize, aDataIn); 
00542     }else if(init_pohyb != disp_poschodieSmer){
00543         posliSpravu(C_LCD,0x00,dataSize, aDataIn);     
00544     }
00545     disp_poschodie = cisloPochodia_dislpej; 
00546     disp_poschodieSmer = init_pohyb; 
00547 }
00548 
00549 void riadeniePohybu()
00550 {
00551     if (dajPoziciu() != pozicia){
00552         if(inicializacia == 1){
00553             if(dajPoziciu() == SWITCH_P){
00554                 inicializacia = 0;
00555                 zastav();
00556             }
00557         }else{
00558             if(smer == 1){
00559                 if(p_0 == 1){
00560                     if(dajPoziciu() == SWITCH_P){
00561                         zastav(); 
00562                         if(init_kabina > 0){
00563                            
00564                             init_kabina--;    
00565                         }
00566                         if(stena > 0){
00567                           
00568                             stena--;                       
00569                         }
00570                         p_0 = 0;
00571                         kere_poschodie = 0;
00572                         cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5); 
00573                     }    
00574                 }else if(p_1 == 1){
00575                     if(dajPoziciu() == SWITCH_1){
00576                         zastav();
00577                         if(init_kabina  > 0){
00578                            
00579                             init_kabina--;    
00580                         }
00581                         if(stena > 0){
00582                            
00583                             stena--;                       
00584                         }
00585                         p_1 = 0; 
00586                         kere_poschodie = 1; 
00587                         cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);                        
00588                     }    
00589                 }else if(p_2 == 1){
00590                     if(dajPoziciu() == SWITCH_2){
00591                         zastav();
00592                         if(init_kabina > 0){
00593                            
00594                             init_kabina--;    
00595                         }
00596                         if(stena > 0){
00597                           
00598                             stena--;                       
00599                         } 
00600                         p_2 = 0;  
00601                         kere_poschodie = 2;
00602                         cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);                    
00603                     }    
00604                 }else if(p_3 == 1){
00605                     if(dajPoziciu() == SWITCH_3){
00606                         zastav(); 
00607                         if(init_kabina > 0){
00608                            
00609                             init_kabina--;    
00610                         }
00611                         if(stena > 0){
00612                             
00613                             stena--;                       
00614                         }      
00615                         p_3 = 0; 
00616                         kere_poschodie = 3; 
00617                         cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);                  
00618                     }    
00619                 }else if(p_4 == 1){
00620                     if(dajPoziciu() == SWITCH_4){
00621                         zastav(); 
00622                         if(init_kabina > 0){
00623                            
00624                             init_kabina--;    
00625                         }
00626                         if(stena > 0){
00627                             
00628                             stena--;                       
00629                         }
00630                         p_4 = 0;
00631                         kere_poschodie = 4;    
00632                         cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);                
00633                     }    
00634                 }
00635             }else if(smer == -1){ 
00636                 if(p_4 == 1){
00637                     if(dajPoziciu() == SWITCH_4){
00638                         zastav(); 
00639                         if(init_kabina > 0){
00640                             
00641                             init_kabina--;    
00642                         }
00643                         if(stena > 0){
00644                            
00645                             stena--;                       
00646                         }
00647                         p_4 = 0;
00648                         kere_poschodie = 4;
00649                         cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5); 
00650                     }    
00651                 }else if(p_3 == 1){
00652                     if(dajPoziciu() == SWITCH_3){
00653                         zastav();
00654                         if(init_kabina  > 0){
00655                             
00656                             init_kabina--;    
00657                         }
00658                         if(stena > 0){
00659                            
00660                             stena--;                       
00661                         }
00662                         p_3 = 0; 
00663                         kere_poschodie = 3; 
00664                         cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);                        
00665                     }    
00666                 }else if(p_2 == 1){
00667                     if(dajPoziciu() == SWITCH_2){
00668                         zastav();
00669                         if(init_kabina > 0){
00670                             
00671                             init_kabina--;    
00672                         }
00673                         if(stena > 0){
00674                            
00675                             stena--;                       
00676                         } 
00677                         p_2 = 0;  
00678                         kere_poschodie = 2;
00679                         cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);                    
00680                     }    
00681                 }else if(p_1 == 1){
00682                     if(dajPoziciu() == SWITCH_1){
00683                         zastav(); 
00684                         if(init_kabina > 0){
00685                             
00686                             init_kabina--;    
00687                         }
00688                         if(stena > 0){
00689                            
00690                             stena--;                       
00691                         }   
00692                         p_1 = 0; 
00693                         kere_poschodie = 1; 
00694                         cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);                  
00695                     }    
00696                 }else if(p_0 == 1){
00697                     if(dajPoziciu() == SWITCH_P){
00698                         zastav(); 
00699                         if(init_kabina > 0){
00700                             
00701                             init_kabina--;    
00702                         }
00703                         if(stena > 0){
00704                             
00705                             stena--;                       
00706                         }
00707                         p_0 = 0;
00708                         kere_poschodie = 0;    
00709                         cakajNaPoschodi.attach(&dockajNaPoschodi, 0.5);                
00710                     }    
00711                 }
00712             }       
00713         }              
00714     }
00715     pozicia = dajPoziciu();        
00716 }
00717 
00718 void watchdog()
00719 {
00720     int aDataIn[] = {0x00};
00721     int dataSize = ARRAY_SIZE(aDataIn);
00722     posliSpravu(WATCHDOG,0x00,dataSize, aDataIn);       
00723 }
00724  
00725 void resetuj_watchdog()
00726 {
00727     int aDataIn[] = {0x01};
00728     int dataSize = ARRAY_SIZE(aDataIn);
00729     posliSpravu(WATCHDOG,0x00,dataSize, aDataIn);         
00730 }
00731