communication.cpp

00001 /******************************************************
00002 
00003 ****┏┓          ┏┓
00004 **┏┛┻━━━━━━┛┻┓
00005 **┃                 ┃
00006 **┃      ━━━      ┃
00007 **┃  ┳┛       ┗┳ ┃
00008 **┃                 ┃
00009 **┃ '''    ┻   ''' ┃
00010 **┃                 ┃
00011 **┗━━┓       ┏━━┛
00012 *******┃       ┃
00013 *******┃       ┃
00014 *******┃       ┃
00015 *******┃       ┗━━━━━━━━┓
00016 *******┃                      ┃━┓
00017 *******┃      NO BUG          ┏━┛
00018 *******┃                      ┃
00019 *******┗━┓  ┓  ┏━┏━┓  ━┛
00020 ***********┃  ┛  ┛    ┃  ┛  ┛
00021 ***********┃  ┃  ┃    ┃  ┃  ┃
00022 ***********┗━┛━┛     ┗━┛━┛
00023 
00024 This part is added by project ESDC2014 of CUHK team.
00025 All the code with this header are under GPL open source license.
00026 This program is running on Mbed Platform 'mbed LPC1768' avaliable in 'http://mbed.org'.
00027 **********************************************************/
00028 #include <communication.h>
00029 
00030 Communication::Communication(MySerial* _DEBUG, MySerial *_IntelToMbed, MySerial *_MbedToArduino, COMPASS* _compass)
00031 {
00032     this->_DEBUG = _DEBUG;
00033     this->_IntelToMbed = _IntelToMbed;
00034     this->_MbedToArduino = _MbedToArduino;
00035     this->_compass = _compass;
00036     init();
00037 }
00038 
00039 Communication::~Communication()
00040 {
00041     delete[] buffer_IntelToMbed;
00042     delete[] buffer_MbedToArduino;
00043     delete[] forward_msg_buffer;
00044     delete _DEBUG;
00045     delete _IntelToMbed;
00046     delete _MbedToArduino;
00047     delete _compass;
00048 }
00049 
00050 void Communication::init()
00051 {
00052     buffer_IntelToMbed = new uint8_t[BUFFER_SIZE];
00053     buffer_MbedToArduino = new uint8_t[BUFFER_SIZE];
00054     
00055     forward_msg_buffer = new uint8_t[9]; //the message struct is 9 byte
00056     
00057     in_IntelToMbed = 0;
00058     out_IntelToMbed = 0;
00059     in_MbedToArduino = 0;
00060     out_MbedToArduino = 0;
00061     state_IntelToMbed = 0;
00062     state_MbedToArduino = 0;
00063     check_sum = 0;
00064     info_ok_IntelToMbed = 0;
00065     info_ok_MbedToArduino = 0;
00066 }
00067 
00068 uint8_t Communication::getByte(uint8_t communication_type)
00069 {
00070     uint8_t _x = 0;
00071     if(communication_type == 0)
00072     {
00073         _x = buffer_IntelToMbed[out_IntelToMbed++];
00074         if(out_IntelToMbed == BUFFER_SIZE)
00075         {
00076             out_IntelToMbed &= 0x0000;
00077         }
00078     }
00079     else if(communication_type == 1)
00080     {
00081         _x = buffer_MbedToArduino[out_MbedToArduino++];
00082         if(out_MbedToArduino == BUFFER_SIZE)
00083         {
00084             out_MbedToArduino &= 0x0000;
00085         }
00086     }
00087     return _x;
00088 }
00089 
00090 uint16_t Communication::get2Bytes(uint8_t communication_type)
00091 {
00092     uint8_t byte1 = getByte(communication_type);
00093     uint8_t byte2 = getByte(communication_type);
00094     return uint16_t((byte1 << 8) | byte2);
00095 }
00096 
00097 void Communication::putByte(uint8_t _x, uint8_t _i)
00098 {
00099     //Serial.write(_x);//For Arduino
00100      //For Mbed
00101     if(_i == 0) //_DEBUG
00102     {
00103         _DEBUG->putc(_x);
00104     }
00105     else if(_i == 1) //IntelToMbed
00106     {
00107         _IntelToMbed->putc(_x);
00108     }
00109     else if(_i == 2) //MbedToArduino
00110     {
00111         _MbedToArduino->putc(_x);
00112     }
00113 }
00114 
00115 void Communication::put2Bytes(uint16_t _x, uint8_t _i)
00116 {
00117     putByte(uint8_t(_x >> 8), _i);
00118     putByte(uint8_t(_x & 0x0f), _i);
00119 }
00120 
00121 void Communication::putToBuffer(uint8_t _x, uint8_t communication_type)
00122 {
00123     if(communication_type == 0)
00124     {
00125         buffer_IntelToMbed[in_IntelToMbed++] = _x;
00126         if(in_IntelToMbed == BUFFER_SIZE)
00127         {
00128             in_IntelToMbed &= 0x0000;
00129         }
00130     }
00131     else if(communication_type == 1)
00132     {
00133         buffer_MbedToArduino[in_MbedToArduino++] = _x;
00134         if(in_MbedToArduino == BUFFER_SIZE)
00135         {
00136             in_MbedToArduino &= 0x0000;
00137         }
00138     }
00139 }
00140 
00141 void Communication::parseMessage()
00142 {
00143     if(in_IntelToMbed != out_IntelToMbed)
00144     {
00145         uint8_t _x = getByte(0);
00146         switch(state_IntelToMbed)
00147         {
00148             case 0: //checking starter
00149             {
00150                 if(DEBUG_ON)
00151                 {
00152                     _DEBUG->printf("Communication::parseMessage(). Checking STARTER...\r\n");
00153                 }
00154                 check_sum = 0;
00155 
00156                 if(_x == STARTER || _x == COMPASS_STARTER)
00157                 {
00158                     state_IntelToMbed++;
00159                     forward_msg_buffer[0] = _x;
00160                 }
00161                 else
00162                 {
00163                     state_IntelToMbed = 0;
00164                     if(DEBUG_ON)
00165                     {
00166                         _DEBUG->printf("Communication::parseMessage(). ERROR when checking STARTER: %x.\r\n", _x);
00167                     }
00168                 }
00169                 break;
00170             }
00171 
00172             case 1: //checking action_type
00173             {
00174                 if(DEBUG_ON)
00175                 {
00176                     _DEBUG->printf("Communication::parseMessage(). Checking ACTION_TYPE...\r\n");
00177                 }
00178                 check_sum += _x;
00179                 action_type = _x;
00180                 if(action_type == 0 || action_type == 1 || action_type == 2 || action_type == 3 || action_type == 4)
00181                 {
00182                     state_IntelToMbed++;
00183                     forward_msg_buffer[1] = _x;
00184                 }
00185                 else
00186                 {
00187                     state_IntelToMbed = 0;
00188                     if(DEBUG_ON)
00189                     {
00190                         _DEBUG->printf("Communication::parseMessage(). ERROR when checking ACTION_TYPE: %x.\r\n", action_type);
00191                     }
00192                 }
00193                 break;
00194             }
00195 
00196             case 2: //move_dis upper 4 bits
00197             {
00198                 if(DEBUG_ON)
00199                 {
00200                     _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIS upper 4 bits...\r\n");
00201                 }
00202                 check_sum += _x;
00203                 move_dis = _x << 8;
00204                 state_IntelToMbed++;
00205                 forward_msg_buffer[2] = _x;
00206                 break;
00207             }
00208 
00209             case 3: //move_dis lower 4 bits
00210             {
00211                 if(DEBUG_ON)
00212                 {
00213                     _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIS lower 4 bits...\r\n");
00214                 }
00215                 check_sum += _x;
00216                 move_dis |=  _x;
00217                 state_IntelToMbed++;
00218                 forward_msg_buffer[3] = _x;
00219                 break;
00220             }
00221 
00222             case 4: //move_dir
00223             {
00224                 if(DEBUG_ON)
00225                 {
00226                     _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIR...\r\n");
00227                 }
00228                 check_sum += _x;
00229                 move_dir = _x;
00230                 if((action_type == 0 && (move_dir == 0 || move_dir == 1 || move_dir == 2 || move_dir == 3)) || (action_type == 1 && (move_dir == 0 || move_dir == 2)) || action_type == 2 || action_type == 3 || action_type == 4)
00231                 {
00232                     state_IntelToMbed++;
00233                     forward_msg_buffer[4] = _x;
00234                 }
00235                 else
00236                 {
00237                     state_IntelToMbed = 0;
00238                     if(DEBUG_ON)
00239                     {
00240                         _DEBUG->printf("Communication::parseMessage(). ERROR when checking MOVE_DIR: %x.\r\n", move_dir);
00241                     }
00242                 }
00243                 break;
00244             }
00245 
00246             case 5: //rotate_dis upper 4 bits
00247             {
00248                 if(DEBUG_ON)
00249                 {
00250                     _DEBUG->printf("Communication::parseMessage(). Checking ROTATE_DIS upper 4 bits...\r\n");
00251                 }
00252                 check_sum += _x;
00253                 rotate_dis = _x << 8;
00254                 state_IntelToMbed++;
00255                 forward_msg_buffer[5] = _x;
00256                 break;
00257             }
00258 
00259             case 6: //rotate_dis lower 4 bits
00260             {
00261                 if(DEBUG_ON)
00262                 {
00263                     _DEBUG->printf("Communication::parseMessage(). Checking ROTATE_DIS lower 4 bits...\r\n");
00264                 }
00265                 check_sum += _x;
00266                 rotate_dis |= _x;
00267                 state_IntelToMbed++;
00268                 forward_msg_buffer[6] = _x;
00269                 break;
00270             }
00271 
00272             case 7: //rotate_dir
00273             {
00274                 if(DEBUG_ON)
00275                 {
00276                     _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIR...\r\n");
00277                 }
00278                 check_sum += _x;
00279                 rotate_dir = _x;
00280                 if(action_type == 3 || action_type == 4 || (action_type == 1 && ((rotate_dir >> 6) == 0)) || ((action_type == 0 || action_type == 2) && ((rotate_dir >> 6) == 3)))
00281                 {
00282                     state_IntelToMbed++;
00283                     forward_msg_buffer[7] = _x;
00284                 }
00285                 else
00286                 {
00287                     state_IntelToMbed = 0;
00288                     if(DEBUG_ON)
00289                     {
00290                         _DEBUG->printf("Communication::parseMessage(). ERROR when checking ROTATE_DIR: %x.\r\n", rotate_dir);
00291                     }
00292                 }
00293                 break;
00294             }
00295 
00296             case 8: //check_sum
00297             {
00298                 if(DEBUG_ON)
00299                 {
00300                     _DEBUG->printf("Communication::parseMessage(). Checking CHECK_SUM...\r\n");
00301                 }
00302                 if(check_sum == _x)
00303                 {
00304                     forward_msg_buffer[8] = _x;
00305                     switch(action_type)
00306                     {
00307                         case 0: //car movement
00308                         info_ok_IntelToMbed = 1;
00309                         break;
00310 
00311                         case 1: //lifter
00312                         info_ok_IntelToMbed = 2;
00313                         break;
00314 
00315                         case 2: //camera platform
00316                         info_ok_IntelToMbed = 3;
00317                         break;
00318                         
00319                         case 3: //compass
00320                         info_ok_IntelToMbed = 4;
00321                         break;
00322                         
00323                         case 4: //buzzer
00324                         info_ok_IntelToMbed = 5;
00325                         break;
00326 
00327                         default:
00328                         info_ok_IntelToMbed = 0; //not ok
00329                         break;
00330                     }
00331                 }
00332                 else
00333                 {
00334                     if(DEBUG_ON)
00335                     {
00336                         _DEBUG->printf("Communication::parseMessage(). ERROR when checking CHECK_SUM: %x.\r\n", check_sum);
00337                     }
00338                 }
00339                 state_IntelToMbed  = 0;
00340                 break;
00341             }
00342 
00343             default:
00344             {
00345                 state_IntelToMbed = 0;
00346                 break;
00347             }
00348         }
00349     }
00350 }
00351 
00352 void Communication::forwardMessage()
00353 {
00354     //message structure is defined in source/motion_platform/intel_board/lib/message.h
00355     putByte(forward_msg_buffer[0], 2); //starter, 2 means MbedToArduino
00356     putByte(forward_msg_buffer[1], 2); //action_type
00357     putByte(forward_msg_buffer[2], 2); //move_dis
00358     putByte(forward_msg_buffer[3], 2);
00359     putByte(forward_msg_buffer[4], 2); //move_dir
00360     putByte(forward_msg_buffer[5], 2); //rotate_dis
00361     putByte(forward_msg_buffer[6], 2);
00362     putByte(forward_msg_buffer[7], 2); //rotate_dir
00363     putByte(forward_msg_buffer[8], 2); //checksum
00364 }
00365 
00366 void Communication::ACK(Lifter* lifter, Camera_platform* camera_platform)
00367 {
00368     if(action_type == 0) //car movement
00369     {
00370         for(int i = 0; i < 9; i++)
00371         {
00372             uint8_t _y = getByte(1);
00373             printf("Communication::ACK(). Get byte: %x\r\n", _y);
00374         }
00375         
00376         while(info_ok_MbedToArduino != 1)
00377         {
00378             if(in_MbedToArduino != out_MbedToArduino)
00379             {
00380                 uint8_t _x = getByte(1);
00381                 switch(state_MbedToArduino)
00382                 {
00383                     case 0: //checking starter
00384                     {
00385                         if(DEBUG_ON)
00386                         {
00387                             _DEBUG->printf("Communication::ACK(). Checking SARTER...\r\n");
00388                         }
00389                         
00390                         if(_x == STARTER)
00391                         {
00392                             state_MbedToArduino++;
00393                         }
00394                         else
00395                         {
00396                             if(DEBUG_ON)
00397                             {
00398                                 _DEBUG->printf("Communication::ACK(). ERROR when checking SARTER: %x\r\n", _x);
00399                             }
00400                             state_MbedToArduino = 0;
00401                         }
00402                         break;
00403                     }
00404 
00405                     case 1: //checking 'O'
00406                     {
00407                         if(DEBUG_ON)
00408                         {
00409                             _DEBUG->printf("Communication::ACK(). Checking O...\r\n");
00410                         }
00411                         
00412                         if(_x == 0x4f) //O
00413                         {
00414                             state_MbedToArduino++;
00415                         }
00416                         else
00417                         {
00418                             if(DEBUG_ON)
00419                             {
00420                                 _DEBUG->printf("Communication::ACK(). ERROR when checking O: %x\r\n", _x);
00421                             }
00422                             state_MbedToArduino = 0;
00423                         }
00424                         break;
00425                     }
00426 
00427                     case 2: //checking 'K'
00428                     {
00429                         if(DEBUG_ON)
00430                         {
00431                             _DEBUG->printf("Communication::ACK(). Checking K...\r\n");
00432                         }
00433                         
00434                         if(_x == 0x4b) //K
00435                         {
00436                             state_MbedToArduino++;
00437                         }
00438                         else
00439                         {
00440                             if(DEBUG_ON)
00441                             {
00442                                 _DEBUG->printf("Communication::ACK(). ERROR when checking K: %x\r\n", _x);
00443                             }
00444                             state_MbedToArduino = 0;
00445                         }
00446                         break;
00447                     }
00448 
00449                     case 3: //checking check_sum_MbedToArduino
00450                     {
00451                         if(DEBUG_ON)
00452                         {
00453                             _DEBUG->printf("Communication::ACK(). Checking CHECK_SUM...\r\n");
00454                         }
00455                         
00456                         if(_x == 0x9a) //checksum
00457                         {
00458                             info_ok_MbedToArduino = 1;
00459                         }
00460                         else
00461                         {
00462                             if(DEBUG_ON)
00463                             {
00464                                 _DEBUG->printf("Communication::ACK(). ERROR when checking CHECK_SUM: %x\r\n", _x);
00465                             }
00466                         }
00467                         
00468                         state_MbedToArduino = 0;
00469                         break;
00470                     }
00471 
00472                     default:
00473                     {
00474                         state_MbedToArduino = 0;
00475                         info_ok_MbedToArduino = 0;
00476                         break;
00477                     }
00478                 }
00479             }
00480         }
00481     }
00482     else if(action_type == 1) //lifter
00483     {
00484         uint32_t pulseCountOld = 0;
00485         uint32_t pulseCountNew = 0;
00486         while(!lifter->isStopped())
00487         {
00488             pulseCountOld = lifter->pulseCount;
00489             wait_ms(50);
00490             pulseCountNew = lifter->pulseCount;
00491             if(pulseCountOld == pulseCountNew)
00492             {
00493                 break;
00494             }
00495         }
00496     }
00497     else if(action_type == 3)
00498     {
00499         
00500         campass_degree = compass->read();
00501 
00502         uint8_t temp1,temp2;
00503         temp1 = campass_degree;
00504         temp2 = campass_degree>>8;
00505         putByte(COMPASS_STARTER ,1); //1 means IntelToMbed
00506         putByte(temp1 ,1); //O
00507         putByte(temp2 ,1); //K
00508         putByte(0x9a ,1); //check_sum = 0xaf + 0x4b = 0x9a
00509     
00510         return;
00511     } 
00512      else if(action_type == 4)
00513     {
00514         
00515         putByte(BUZZER_STARTER ,1); //1 means IntelToMbed
00516         putByte(0,1); //O
00517         putByte(0,1); //K
00518         putByte(0,1); //check_sum = 0xaf + 0x4b = 0x9a
00519     
00520         return;
00521     } 
00522     
00523     putByte(STARTER ,1); //1 means IntelToMbed
00524     putByte(0x4f ,1); //O
00525     putByte(0x4b ,1); //K
00526     putByte(0x9a ,1); //check_sum = 0xaf + 0x4b = 0x9a
00527 }
00528 
00529 uint8_t Communication::getInfoOK(uint8_t communication_type)
00530 {
00531     if(communication_type == 0)
00532     {
00533         return info_ok_IntelToMbed;
00534     }
00535     else if(communication_type == 1)
00536     {
00537         return info_ok_MbedToArduino;
00538     }
00539     return 0; //error
00540 }
00541 
00542 void Communication::resetInfoOK(uint8_t communication_type)
00543 {
00544     if(communication_type == 0)
00545     {
00546         info_ok_IntelToMbed = 0;
00547     }
00548     else if(communication_type == 1)
00549     {
00550         info_ok_MbedToArduino = 0;
00551     }
00552 }
00553 
00554 uint16_t Communication::getMoveDis()
00555 {
00556     return move_dis;
00557 }
00558 
00559 uint16_t Communication::getRotateDis()
00560 {
00561     return rotate_dis;
00562 }
00563 
00564 uint8_t Communication::getMoveDir()
00565 {
00566     return move_dir;
00567 }
00568 
00569 uint8_t Communication::getRotateDir()
00570 {
00571     return rotate_dir;
00572 }