Tony YI
/
ESDC2014
123123123123123123123123123
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Thu Jul 14 2022 22:42:47 by 1.7.2