Tony YI
/
ESDC2014
123123123123123123123123123
communication.cpp
- Committer:
- terryLAI
- Date:
- 2014-07-03
- Revision:
- 1:cbec1283a16a
- Parent:
- 0:3417ca0a36c0
- Child:
- 2:442902ec3aa1
File content as of revision 1:cbec1283a16a:
/****************************************************** ****┏┓ ┏┓ **┏┛┻━━━━━━┛┻┓ **┃ ┃ **┃ ━━━ ┃ **┃ ┳┛ ┗┳ ┃ **┃ ┃ **┃ ''' ┻ ''' ┃ **┃ ┃ **┗━━┓ ┏━━┛ *******┃ ┃ *******┃ ┃ *******┃ ┃ *******┃ ┗━━━━━━━━┓ *******┃ ┃━┓ *******┃ NO BUG ┏━┛ *******┃ ┃ *******┗━┓ ┓ ┏━┏━┓ ━┛ ***********┃ ┛ ┛ ┃ ┛ ┛ ***********┃ ┃ ┃ ┃ ┃ ┃ ***********┗━┛━┛ ┗━┛━┛ This part is added by project ESDC2014 of CUHK team. All the code with this header are under GPL open source license. This program is running on Mbed Platform 'mbed LPC1768' avaliable in 'http://mbed.org'. **********************************************************/ #include <communication.h> Communication::Communication(MySerial* _DEBUG, MySerial *_IntelToMbed, MySerial *_MbedToArduino) { this->_DEBUG = _DEBUG; this->_IntelToMbed = _IntelToMbed; this->_MbedToArduino = _MbedToArduino; this->compass=new COMPASS(COMPASS_TX,COMPASS_RX); init(); } Communication::~Communication() { delete[] buffer_IntelToMbed; delete[] buffer_MbedToArduino; delete _DEBUG; delete _IntelToMbed; delete _MbedToArduino; } void Communication::init() { buffer_IntelToMbed = new uint8_t[BUFFER_SIZE]; buffer_MbedToArduino = new uint8_t[BUFFER_SIZE]; in_IntelToMbed = 0; out_IntelToMbed = 0; in_MbedToArduino = 0; out_MbedToArduino = 0; state_IntelToMbed = 0; state_MbedToArduino = 0; check_sum = 0; info_ok_IntelToMbed = 0; info_ok_MbedToArduino = 0; } uint8_t Communication::getByte(uint8_t communication_type) { uint8_t _x = 0; if(communication_type == 0) { _x = buffer_IntelToMbed[out_IntelToMbed++]; if(out_IntelToMbed == BUFFER_SIZE) { out_IntelToMbed &= 0x0000; } } else if(communication_type == 1) { _x = buffer_MbedToArduino[out_MbedToArduino++]; if(out_MbedToArduino == BUFFER_SIZE) { out_MbedToArduino &= 0x0000; } } return _x; } uint16_t Communication::get2Bytes(uint8_t communication_type) { uint8_t byte1 = getByte(communication_type); uint8_t byte2 = getByte(communication_type); return uint16_t((byte1 << 8) | byte2); } void Communication::putByte(uint8_t _x, uint8_t _i) { //Serial.write(_x);//For Arduino //For Mbed if(_i == 0) //_DEBUG { _DEBUG->putc(_x); } else if(_i == 1) //IntelToMbed { _IntelToMbed->putc(_x); } else if(_i == 2) //MbedToArduino { _MbedToArduino->putc(_x); } } void Communication::put2Bytes(uint16_t _x, uint8_t _i) { putByte(uint8_t(_x >> 8), _i); putByte(uint8_t(_x & 0x0f), _i); } void Communication::putToBuffer(uint8_t _x, uint8_t communication_type) { if(communication_type == 0) { buffer_IntelToMbed[in_IntelToMbed++] = _x; if(in_IntelToMbed == BUFFER_SIZE) { in_IntelToMbed &= 0x00; } } else if(communication_type == 1) { buffer_MbedToArduino[in_MbedToArduino++] = _x; if(in_MbedToArduino == BUFFER_SIZE) { in_MbedToArduino &= 0x00; } } } void Communication::parseMessage() { if(in_IntelToMbed != out_IntelToMbed) { uint8_t _x = getByte(0); switch(state_IntelToMbed) { case 0: //checking starter { if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). Checking STARTER...\r\n"); } check_sum = 0; if(_x == STARTER || _x == COMPASS_STARTER) { state_IntelToMbed++; } else { state_IntelToMbed = 0; if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). ERROR when checking STARTER: %x.\r\n", _x); } } break; } case 1: //checking action_type { if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). Checking ACTION_TYPE...\r\n"); } check_sum += _x; action_type = _x; if(action_type == 0 || action_type == 1 || action_type == 2 || action_type == 3) { state_IntelToMbed++; } else { state_IntelToMbed = 0; if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). ERROR when checking ACTION_TYPE: %x.\r\n", action_type); } } break; } case 2: //move_dis upper 4 bits { if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIS upper 4 bits...\r\n"); } check_sum += _x; move_dis = _x << 8; state_IntelToMbed++; break; } case 3: //move_dis lower 4 bits { if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIS lower 4 bits...\r\n"); } check_sum += _x; move_dis |= _x; state_IntelToMbed++; break; } case 4: //move_dir { if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIR...\r\n"); } check_sum += _x; move_dir = _x; 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) { state_IntelToMbed++; } else { state_IntelToMbed = 0; if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). ERROR when checking MOVE_DIR: %x.\r\n", move_dir); } } break; } case 5: //rotate_dis upper 4 bits { if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). Checking ROTATE_DIS upper 4 bits...\r\n"); } check_sum += _x; rotate_dis = _x << 8; state_IntelToMbed++; break; } case 6: //rotate_dis lower 4 bits { if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). Checking ROTATE_DIS lower 4 bits...\r\n"); } check_sum += _x; rotate_dis |= _x; state_IntelToMbed++; break; } case 7: //rotate_dir { if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIR...\r\n"); } check_sum += _x; rotate_dir = _x; if(action_type == 3 || (action_type == 1 && ((rotate_dir >> 6) == 0)) || ((action_type == 0 || action_type == 2) && ((rotate_dir >> 6) == 3))) { state_IntelToMbed++; } else { state_IntelToMbed = 0; if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). ERROR when checking ROTATE_DIR: %x.\r\n", rotate_dir); } } break; } case 8: //check_sum { if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). Checking CHECK_SUM...\r\n"); } if(check_sum == _x) { switch(action_type) { case 0: //car movement info_ok_IntelToMbed = 1; break; case 1: //lifter info_ok_IntelToMbed = 2; break; case 2: //camera platform info_ok_IntelToMbed = 3; break; case 3: //compass info_ok_IntelToMbed = 4; break; default: info_ok_IntelToMbed = 0; //not ok break; } } else { if(DEBUG_ON) { _DEBUG->printf("Communication::parseMessage(). ERROR when checking CHECK_SUM: %x.\r\n", check_sum); } } state_IntelToMbed = 0; break; } default: { state_IntelToMbed = 0; break; } } } } void Communication::forwardMessage() { //message structure is defined in source/motion_platform/intel_board/lib/message.h uint8_t i = out_IntelToMbed - 9; //message size is 9 bytes putByte(buffer_IntelToMbed[i++], 2); //starter, 2 means MbedToArduino putByte(buffer_IntelToMbed[i++], 2); //action_type putByte(buffer_IntelToMbed[i++], 2); //move_dis putByte(buffer_IntelToMbed[i++], 2); putByte(buffer_IntelToMbed[i++], 2); //move_dir putByte(buffer_IntelToMbed[i++], 2); //rotate_dis putByte(buffer_IntelToMbed[i++], 2); putByte(buffer_IntelToMbed[i++], 2); //rotate_dir putByte(buffer_IntelToMbed[i++], 2); //checksum } void Communication::ACK(Lifter* lifter, Camera_platform* camera_platform) { if(action_type == 0) //car movement { while(info_ok_MbedToArduino != 1) { if(in_MbedToArduino != out_MbedToArduino) { uint8_t _x = getByte(1); switch(state_MbedToArduino) { case 0: //checking starter { //putByte('0', 1); if(_x == STARTER) { state_MbedToArduino++; } else { state_MbedToArduino = 0; } break; } case 1: //checking 'O' { //putByte('1', 1); if(_x == 0x4f) //O { state_MbedToArduino++; } else { state_MbedToArduino = 0; } break; } case 2: //checking 'K' { //putByte('2', 1); if(_x == 0x4b) //K { state_MbedToArduino++; } else { state_MbedToArduino = 0; } break; } case 3: //checking check_sum_MbedToArduino { //putByte('3', 1); if(_x == 0x9a) //checksum { info_ok_MbedToArduino = 1; } state_MbedToArduino = 0; break; } default: { state_MbedToArduino = 0; info_ok_MbedToArduino = 0; break; } } } } } else if(action_type == 1) //lifter { uint32_t pulseCountOld = 0; uint32_t pulseCountNew = 0; while(!lifter->isStopped()) { pulseCountOld = lifter->pulseCount; wait_ms(50); pulseCountNew = lifter->pulseCount; if(pulseCountOld == pulseCountNew) { break; } } } else if(action_type == 3) { campass_degree = compass->Read(); uint8_t temp1,temp2; temp1 = campass_degree; temp2 = campass_degree>>8; putByte(COMPASS_STARTER ,1); //1 means IntelToMbed putByte(temp1 ,1); //O putByte(temp2 ,1); //K putByte(0x9a ,1); //check_sum = 0xaf + 0x4b = 0x9a return; } putByte(STARTER ,1); //1 means IntelToMbed putByte(0x4f ,1); //O putByte(0x4b ,1); //K putByte(0x9a ,1); //check_sum = 0xaf + 0x4b = 0x9a } uint8_t Communication::getInfoOK(uint8_t communication_type) { if(communication_type == 0) { return info_ok_IntelToMbed; } else if(communication_type == 1) { return info_ok_MbedToArduino; } return 0; //error } void Communication::resetInfoOK(uint8_t communication_type) { if(communication_type == 0) { info_ok_IntelToMbed = 0; } else if(communication_type == 1) { info_ok_MbedToArduino = 0; } } uint16_t Communication::getMoveDis() { return move_dis; } uint16_t Communication::getRotateDis() { return rotate_dis; } uint8_t Communication::getMoveDir() { return move_dir; } uint8_t Communication::getRotateDir() { return rotate_dir; }