Tony YI / Mbed 2 deprecated ESDC2014

123123123123123123123123123

Dependencies:   mbed

communication.cpp@3:4306d042af6f, 2014-07-03 (annotated)

Committer:
TonyYI
Date:
Thu Jul 03 14:52:44 2014 +0000
Revision:
3:4306d042af6f
Parent:
2:442902ec3aa1 
123123

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TonyYI 0:3417ca0a36c0 1 /******************************************************
TonyYI 0:3417ca0a36c0 2
TonyYI 0:3417ca0a36c0 3 ****┏┓ ┏┓
TonyYI 0:3417ca0a36c0 4 **┏┛┻━━━━━━┛┻┓
TonyYI 0:3417ca0a36c0 5 **┃ ┃
TonyYI 0:3417ca0a36c0 6 **┃ ━━━ ┃
TonyYI 0:3417ca0a36c0 7 **┃ ┳┛ ┗┳ ┃
TonyYI 0:3417ca0a36c0 8 **┃ ┃
TonyYI 0:3417ca0a36c0 9 **┃ ''' ┻ ''' ┃
TonyYI 0:3417ca0a36c0 10 **┃ ┃
TonyYI 0:3417ca0a36c0 11 **┗━━┓ ┏━━┛
TonyYI 0:3417ca0a36c0 12 *******┃ ┃
TonyYI 0:3417ca0a36c0 13 *******┃ ┃
TonyYI 0:3417ca0a36c0 14 *******┃ ┃
TonyYI 0:3417ca0a36c0 15 *******┃ ┗━━━━━━━━┓
TonyYI 0:3417ca0a36c0 16 *******┃ ┃━┓
TonyYI 0:3417ca0a36c0 17 *******┃ NO BUG ┏━┛
TonyYI 0:3417ca0a36c0 18 *******┃ ┃
TonyYI 0:3417ca0a36c0 19 *******┗━┓ ┓ ┏━┏━┓ ━┛
TonyYI 0:3417ca0a36c0 20 ***********┃ ┛ ┛ ┃ ┛ ┛
TonyYI 0:3417ca0a36c0 21 ***********┃ ┃ ┃ ┃ ┃ ┃
TonyYI 0:3417ca0a36c0 22 ***********┗━┛━┛ ┗━┛━┛
TonyYI 0:3417ca0a36c0 23
TonyYI 0:3417ca0a36c0 24 This part is added by project ESDC2014 of CUHK team.
TonyYI 0:3417ca0a36c0 25 All the code with this header are under GPL open source license.
TonyYI 0:3417ca0a36c0 26 This program is running on Mbed Platform 'mbed LPC1768' avaliable in 'http://mbed.org'.
TonyYI 0:3417ca0a36c0 27 **********************************************************/
TonyYI 0:3417ca0a36c0 28 #include <communication.h>
TonyYI 0:3417ca0a36c0 29
TonyYI 3:4306d042af6f 30 Communication::Communication(MySerial* _DEBUG, MySerial *_IntelToMbed, MySerial *_MbedToArduino, COMPASS* _compass)
TonyYI 0:3417ca0a36c0 31 {
TonyYI 0:3417ca0a36c0 32 this->_DEBUG = _DEBUG;
TonyYI 0:3417ca0a36c0 33 this->_IntelToMbed = _IntelToMbed;
TonyYI 0:3417ca0a36c0 34 this->_MbedToArduino = _MbedToArduino;
TonyYI 3:4306d042af6f 35 this->_compass = _compass;
TonyYI 0:3417ca0a36c0 36 init();
TonyYI 0:3417ca0a36c0 37 }
TonyYI 0:3417ca0a36c0 38
TonyYI 0:3417ca0a36c0 39 Communication::~Communication()
TonyYI 0:3417ca0a36c0 40 {
TonyYI 0:3417ca0a36c0 41 delete[] buffer_IntelToMbed;
TonyYI 0:3417ca0a36c0 42 delete[] buffer_MbedToArduino;
TonyYI 3:4306d042af6f 43 delete[] forward_msg_buffer;
TonyYI 0:3417ca0a36c0 44 delete _DEBUG;
TonyYI 0:3417ca0a36c0 45 delete _IntelToMbed;
TonyYI 0:3417ca0a36c0 46 delete _MbedToArduino;
TonyYI 3:4306d042af6f 47 delete _compass;
TonyYI 0:3417ca0a36c0 48 }
TonyYI 0:3417ca0a36c0 49
TonyYI 0:3417ca0a36c0 50 void Communication::init()
TonyYI 0:3417ca0a36c0 51 {
TonyYI 0:3417ca0a36c0 52 buffer_IntelToMbed = new uint8_t[BUFFER_SIZE];
TonyYI 0:3417ca0a36c0 53 buffer_MbedToArduino = new uint8_t[BUFFER_SIZE];
TonyYI 3:4306d042af6f 54
TonyYI 3:4306d042af6f 55 forward_msg_buffer = new uint8_t[9]; //the message struct is 9 byte
TonyYI 3:4306d042af6f 56
TonyYI 0:3417ca0a36c0 57 in_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 58 out_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 59 in_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 60 out_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 61 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 62 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 63 check_sum = 0;
TonyYI 0:3417ca0a36c0 64 info_ok_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 65 info_ok_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 66 }
TonyYI 0:3417ca0a36c0 67
TonyYI 0:3417ca0a36c0 68 uint8_t Communication::getByte(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 69 {
TonyYI 0:3417ca0a36c0 70 uint8_t _x = 0;
TonyYI 0:3417ca0a36c0 71 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 72 {
TonyYI 0:3417ca0a36c0 73 _x = buffer_IntelToMbed[out_IntelToMbed++];
TonyYI 0:3417ca0a36c0 74 if(out_IntelToMbed == BUFFER_SIZE)
TonyYI 0:3417ca0a36c0 75 {
TonyYI 0:3417ca0a36c0 76 out_IntelToMbed &= 0x0000;
TonyYI 0:3417ca0a36c0 77 }
TonyYI 0:3417ca0a36c0 78 }
TonyYI 0:3417ca0a36c0 79 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 80 {
TonyYI 0:3417ca0a36c0 81 _x = buffer_MbedToArduino[out_MbedToArduino++];
TonyYI 0:3417ca0a36c0 82 if(out_MbedToArduino == BUFFER_SIZE)
TonyYI 0:3417ca0a36c0 83 {
TonyYI 0:3417ca0a36c0 84 out_MbedToArduino &= 0x0000;
TonyYI 0:3417ca0a36c0 85 }
TonyYI 0:3417ca0a36c0 86 }
TonyYI 0:3417ca0a36c0 87 return _x;
TonyYI 0:3417ca0a36c0 88 }
TonyYI 0:3417ca0a36c0 89
TonyYI 0:3417ca0a36c0 90 uint16_t Communication::get2Bytes(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 91 {
TonyYI 0:3417ca0a36c0 92 uint8_t byte1 = getByte(communication_type);
TonyYI 0:3417ca0a36c0 93 uint8_t byte2 = getByte(communication_type);
TonyYI 0:3417ca0a36c0 94 return uint16_t((byte1 << 8) | byte2);
TonyYI 0:3417ca0a36c0 95 }
TonyYI 0:3417ca0a36c0 96
TonyYI 0:3417ca0a36c0 97 void Communication::putByte(uint8_t _x, uint8_t _i)
TonyYI 0:3417ca0a36c0 98 {
TonyYI 0:3417ca0a36c0 99 //Serial.write(_x);//For Arduino
TonyYI 0:3417ca0a36c0 100 //For Mbed
TonyYI 0:3417ca0a36c0 101 if(_i == 0) //_DEBUG
TonyYI 0:3417ca0a36c0 102 {
TonyYI 0:3417ca0a36c0 103 _DEBUG->putc(_x);
TonyYI 0:3417ca0a36c0 104 }
TonyYI 0:3417ca0a36c0 105 else if(_i == 1) //IntelToMbed
TonyYI 0:3417ca0a36c0 106 {
TonyYI 0:3417ca0a36c0 107 _IntelToMbed->putc(_x);
TonyYI 0:3417ca0a36c0 108 }
TonyYI 0:3417ca0a36c0 109 else if(_i == 2) //MbedToArduino
TonyYI 0:3417ca0a36c0 110 {
TonyYI 0:3417ca0a36c0 111 _MbedToArduino->putc(_x);
TonyYI 0:3417ca0a36c0 112 }
TonyYI 0:3417ca0a36c0 113 }
TonyYI 0:3417ca0a36c0 114
TonyYI 0:3417ca0a36c0 115 void Communication::put2Bytes(uint16_t _x, uint8_t _i)
TonyYI 0:3417ca0a36c0 116 {
TonyYI 0:3417ca0a36c0 117 putByte(uint8_t(_x >> 8), _i);
TonyYI 0:3417ca0a36c0 118 putByte(uint8_t(_x & 0x0f), _i);
TonyYI 0:3417ca0a36c0 119 }
TonyYI 0:3417ca0a36c0 120
TonyYI 0:3417ca0a36c0 121 void Communication::putToBuffer(uint8_t _x, uint8_t communication_type)
TonyYI 0:3417ca0a36c0 122 {
TonyYI 0:3417ca0a36c0 123 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 124 {
TonyYI 0:3417ca0a36c0 125 buffer_IntelToMbed[in_IntelToMbed++] = _x;
TonyYI 0:3417ca0a36c0 126 if(in_IntelToMbed == BUFFER_SIZE)
TonyYI 0:3417ca0a36c0 127 {
TonyYI 3:4306d042af6f 128 in_IntelToMbed &= 0x0000;
TonyYI 0:3417ca0a36c0 129 }
TonyYI 0:3417ca0a36c0 130 }
TonyYI 0:3417ca0a36c0 131 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 132 {
TonyYI 0:3417ca0a36c0 133 buffer_MbedToArduino[in_MbedToArduino++] = _x;
TonyYI 0:3417ca0a36c0 134 if(in_MbedToArduino == BUFFER_SIZE)
TonyYI 0:3417ca0a36c0 135 {
TonyYI 3:4306d042af6f 136 in_MbedToArduino &= 0x0000;
TonyYI 0:3417ca0a36c0 137 }
TonyYI 0:3417ca0a36c0 138 }
TonyYI 0:3417ca0a36c0 139 }
TonyYI 0:3417ca0a36c0 140
TonyYI 0:3417ca0a36c0 141 void Communication::parseMessage()
TonyYI 0:3417ca0a36c0 142 {
TonyYI 0:3417ca0a36c0 143 if(in_IntelToMbed != out_IntelToMbed)
TonyYI 0:3417ca0a36c0 144 {
TonyYI 0:3417ca0a36c0 145 uint8_t _x = getByte(0);
TonyYI 0:3417ca0a36c0 146 switch(state_IntelToMbed)
TonyYI 0:3417ca0a36c0 147 {
TonyYI 0:3417ca0a36c0 148 case 0: //checking starter
TonyYI 0:3417ca0a36c0 149 {
TonyYI 0:3417ca0a36c0 150 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 151 {
TonyYI 0:3417ca0a36c0 152 _DEBUG->printf("Communication::parseMessage(). Checking STARTER...\r\n");
TonyYI 0:3417ca0a36c0 153 }
TonyYI 0:3417ca0a36c0 154 check_sum = 0;
TonyYI 0:3417ca0a36c0 155
TonyYI 0:3417ca0a36c0 156 if(_x == STARTER || _x == COMPASS_STARTER)
TonyYI 0:3417ca0a36c0 157 {
TonyYI 0:3417ca0a36c0 158 state_IntelToMbed++;
TonyYI 3:4306d042af6f 159 forward_msg_buffer[0] = _x;
TonyYI 0:3417ca0a36c0 160 }
TonyYI 0:3417ca0a36c0 161 else
TonyYI 0:3417ca0a36c0 162 {
TonyYI 0:3417ca0a36c0 163 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 164 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 165 {
TonyYI 0:3417ca0a36c0 166 _DEBUG->printf("Communication::parseMessage(). ERROR when checking STARTER: %x.\r\n", _x);
TonyYI 0:3417ca0a36c0 167 }
TonyYI 0:3417ca0a36c0 168 }
TonyYI 0:3417ca0a36c0 169 break;
TonyYI 0:3417ca0a36c0 170 }
TonyYI 0:3417ca0a36c0 171
TonyYI 0:3417ca0a36c0 172 case 1: //checking action_type
TonyYI 0:3417ca0a36c0 173 {
TonyYI 0:3417ca0a36c0 174 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 175 {
TonyYI 0:3417ca0a36c0 176 _DEBUG->printf("Communication::parseMessage(). Checking ACTION_TYPE...\r\n");
TonyYI 0:3417ca0a36c0 177 }
TonyYI 0:3417ca0a36c0 178 check_sum += _x;
TonyYI 0:3417ca0a36c0 179 action_type = _x;
terryLAI 2:442902ec3aa1 180 if(action_type == 0 || action_type == 1 || action_type == 2 || action_type == 3 || action_type == 4)
TonyYI 0:3417ca0a36c0 181 {
TonyYI 0:3417ca0a36c0 182 state_IntelToMbed++;
TonyYI 3:4306d042af6f 183 forward_msg_buffer[1] = _x;
TonyYI 0:3417ca0a36c0 184 }
TonyYI 0:3417ca0a36c0 185 else
TonyYI 0:3417ca0a36c0 186 {
TonyYI 0:3417ca0a36c0 187 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 188 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 189 {
TonyYI 0:3417ca0a36c0 190 _DEBUG->printf("Communication::parseMessage(). ERROR when checking ACTION_TYPE: %x.\r\n", action_type);
TonyYI 0:3417ca0a36c0 191 }
TonyYI 0:3417ca0a36c0 192 }
TonyYI 0:3417ca0a36c0 193 break;
TonyYI 0:3417ca0a36c0 194 }
TonyYI 0:3417ca0a36c0 195
TonyYI 0:3417ca0a36c0 196 case 2: //move_dis upper 4 bits
TonyYI 0:3417ca0a36c0 197 {
TonyYI 0:3417ca0a36c0 198 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 199 {
TonyYI 0:3417ca0a36c0 200 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIS upper 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 201 }
TonyYI 0:3417ca0a36c0 202 check_sum += _x;
TonyYI 0:3417ca0a36c0 203 move_dis = _x << 8;
TonyYI 0:3417ca0a36c0 204 state_IntelToMbed++;
TonyYI 3:4306d042af6f 205 forward_msg_buffer[2] = _x;
TonyYI 0:3417ca0a36c0 206 break;
TonyYI 0:3417ca0a36c0 207 }
TonyYI 0:3417ca0a36c0 208
TonyYI 0:3417ca0a36c0 209 case 3: //move_dis lower 4 bits
TonyYI 0:3417ca0a36c0 210 {
TonyYI 0:3417ca0a36c0 211 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 212 {
TonyYI 0:3417ca0a36c0 213 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIS lower 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 214 }
TonyYI 0:3417ca0a36c0 215 check_sum += _x;
TonyYI 0:3417ca0a36c0 216 move_dis |= _x;
TonyYI 0:3417ca0a36c0 217 state_IntelToMbed++;
TonyYI 3:4306d042af6f 218 forward_msg_buffer[3] = _x;
TonyYI 0:3417ca0a36c0 219 break;
TonyYI 0:3417ca0a36c0 220 }
TonyYI 0:3417ca0a36c0 221
TonyYI 0:3417ca0a36c0 222 case 4: //move_dir
TonyYI 0:3417ca0a36c0 223 {
TonyYI 0:3417ca0a36c0 224 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 225 {
TonyYI 0:3417ca0a36c0 226 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIR...\r\n");
TonyYI 0:3417ca0a36c0 227 }
TonyYI 0:3417ca0a36c0 228 check_sum += _x;
TonyYI 0:3417ca0a36c0 229 move_dir = _x;
terryLAI 2:442902ec3aa1 230 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)
TonyYI 0:3417ca0a36c0 231 {
TonyYI 0:3417ca0a36c0 232 state_IntelToMbed++;
TonyYI 3:4306d042af6f 233 forward_msg_buffer[4] = _x;
TonyYI 0:3417ca0a36c0 234 }
TonyYI 0:3417ca0a36c0 235 else
TonyYI 0:3417ca0a36c0 236 {
TonyYI 0:3417ca0a36c0 237 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 238 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 239 {
TonyYI 0:3417ca0a36c0 240 _DEBUG->printf("Communication::parseMessage(). ERROR when checking MOVE_DIR: %x.\r\n", move_dir);
TonyYI 0:3417ca0a36c0 241 }
TonyYI 0:3417ca0a36c0 242 }
TonyYI 0:3417ca0a36c0 243 break;
TonyYI 0:3417ca0a36c0 244 }
TonyYI 0:3417ca0a36c0 245
TonyYI 0:3417ca0a36c0 246 case 5: //rotate_dis upper 4 bits
TonyYI 0:3417ca0a36c0 247 {
TonyYI 0:3417ca0a36c0 248 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 249 {
TonyYI 0:3417ca0a36c0 250 _DEBUG->printf("Communication::parseMessage(). Checking ROTATE_DIS upper 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 251 }
TonyYI 0:3417ca0a36c0 252 check_sum += _x;
TonyYI 0:3417ca0a36c0 253 rotate_dis = _x << 8;
TonyYI 0:3417ca0a36c0 254 state_IntelToMbed++;
TonyYI 3:4306d042af6f 255 forward_msg_buffer[5] = _x;
TonyYI 0:3417ca0a36c0 256 break;
TonyYI 0:3417ca0a36c0 257 }
TonyYI 0:3417ca0a36c0 258
TonyYI 0:3417ca0a36c0 259 case 6: //rotate_dis lower 4 bits
TonyYI 0:3417ca0a36c0 260 {
TonyYI 0:3417ca0a36c0 261 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 262 {
TonyYI 0:3417ca0a36c0 263 _DEBUG->printf("Communication::parseMessage(). Checking ROTATE_DIS lower 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 264 }
TonyYI 0:3417ca0a36c0 265 check_sum += _x;
TonyYI 0:3417ca0a36c0 266 rotate_dis |= _x;
TonyYI 0:3417ca0a36c0 267 state_IntelToMbed++;
TonyYI 3:4306d042af6f 268 forward_msg_buffer[6] = _x;
TonyYI 0:3417ca0a36c0 269 break;
TonyYI 0:3417ca0a36c0 270 }
TonyYI 0:3417ca0a36c0 271
TonyYI 0:3417ca0a36c0 272 case 7: //rotate_dir
TonyYI 0:3417ca0a36c0 273 {
TonyYI 0:3417ca0a36c0 274 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 275 {
TonyYI 0:3417ca0a36c0 276 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIR...\r\n");
TonyYI 0:3417ca0a36c0 277 }
TonyYI 0:3417ca0a36c0 278 check_sum += _x;
TonyYI 0:3417ca0a36c0 279 rotate_dir = _x;
terryLAI 2:442902ec3aa1 280 if(action_type == 3 || action_type == 4 || (action_type == 1 && ((rotate_dir >> 6) == 0)) || ((action_type == 0 || action_type == 2) && ((rotate_dir >> 6) == 3)))
TonyYI 0:3417ca0a36c0 281 {
TonyYI 0:3417ca0a36c0 282 state_IntelToMbed++;
TonyYI 3:4306d042af6f 283 forward_msg_buffer[7] = _x;
TonyYI 0:3417ca0a36c0 284 }
TonyYI 0:3417ca0a36c0 285 else
TonyYI 0:3417ca0a36c0 286 {
TonyYI 0:3417ca0a36c0 287 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 288 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 289 {
TonyYI 0:3417ca0a36c0 290 _DEBUG->printf("Communication::parseMessage(). ERROR when checking ROTATE_DIR: %x.\r\n", rotate_dir);
TonyYI 0:3417ca0a36c0 291 }
TonyYI 0:3417ca0a36c0 292 }
TonyYI 0:3417ca0a36c0 293 break;
TonyYI 0:3417ca0a36c0 294 }
TonyYI 0:3417ca0a36c0 295
TonyYI 0:3417ca0a36c0 296 case 8: //check_sum
TonyYI 0:3417ca0a36c0 297 {
TonyYI 0:3417ca0a36c0 298 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 299 {
TonyYI 0:3417ca0a36c0 300 _DEBUG->printf("Communication::parseMessage(). Checking CHECK_SUM...\r\n");
TonyYI 0:3417ca0a36c0 301 }
TonyYI 0:3417ca0a36c0 302 if(check_sum == _x)
TonyYI 0:3417ca0a36c0 303 {
TonyYI 3:4306d042af6f 304 forward_msg_buffer[8] = _x;
TonyYI 0:3417ca0a36c0 305 switch(action_type)
TonyYI 0:3417ca0a36c0 306 {
TonyYI 0:3417ca0a36c0 307 case 0: //car movement
TonyYI 0:3417ca0a36c0 308 info_ok_IntelToMbed = 1;
TonyYI 0:3417ca0a36c0 309 break;
TonyYI 0:3417ca0a36c0 310
TonyYI 0:3417ca0a36c0 311 case 1: //lifter
TonyYI 0:3417ca0a36c0 312 info_ok_IntelToMbed = 2;
TonyYI 0:3417ca0a36c0 313 break;
TonyYI 0:3417ca0a36c0 314
TonyYI 0:3417ca0a36c0 315 case 2: //camera platform
TonyYI 0:3417ca0a36c0 316 info_ok_IntelToMbed = 3;
TonyYI 0:3417ca0a36c0 317 break;
TonyYI 0:3417ca0a36c0 318
TonyYI 0:3417ca0a36c0 319 case 3: //compass
TonyYI 0:3417ca0a36c0 320 info_ok_IntelToMbed = 4;
TonyYI 0:3417ca0a36c0 321 break;
terryLAI 2:442902ec3aa1 322
terryLAI 2:442902ec3aa1 323 case 4: //buzzer
terryLAI 2:442902ec3aa1 324 info_ok_IntelToMbed = 5;
terryLAI 2:442902ec3aa1 325 break;
TonyYI 0:3417ca0a36c0 326
TonyYI 0:3417ca0a36c0 327 default:
TonyYI 0:3417ca0a36c0 328 info_ok_IntelToMbed = 0; //not ok
TonyYI 0:3417ca0a36c0 329 break;
TonyYI 0:3417ca0a36c0 330 }
TonyYI 0:3417ca0a36c0 331 }
TonyYI 0:3417ca0a36c0 332 else
TonyYI 0:3417ca0a36c0 333 {
TonyYI 0:3417ca0a36c0 334 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 335 {
TonyYI 0:3417ca0a36c0 336 _DEBUG->printf("Communication::parseMessage(). ERROR when checking CHECK_SUM: %x.\r\n", check_sum);
TonyYI 0:3417ca0a36c0 337 }
TonyYI 0:3417ca0a36c0 338 }
TonyYI 0:3417ca0a36c0 339 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 340 break;
TonyYI 0:3417ca0a36c0 341 }
TonyYI 0:3417ca0a36c0 342
TonyYI 0:3417ca0a36c0 343 default:
TonyYI 0:3417ca0a36c0 344 {
TonyYI 0:3417ca0a36c0 345 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 346 break;
TonyYI 0:3417ca0a36c0 347 }
TonyYI 0:3417ca0a36c0 348 }
TonyYI 0:3417ca0a36c0 349 }
TonyYI 0:3417ca0a36c0 350 }
TonyYI 0:3417ca0a36c0 351
TonyYI 0:3417ca0a36c0 352 void Communication::forwardMessage()
TonyYI 0:3417ca0a36c0 353 {
TonyYI 0:3417ca0a36c0 354 //message structure is defined in source/motion_platform/intel_board/lib/message.h
TonyYI 3:4306d042af6f 355 putByte(forward_msg_buffer[0], 2); //starter, 2 means MbedToArduino
TonyYI 3:4306d042af6f 356 putByte(forward_msg_buffer[1], 2); //action_type
TonyYI 3:4306d042af6f 357 putByte(forward_msg_buffer[2], 2); //move_dis
TonyYI 3:4306d042af6f 358 putByte(forward_msg_buffer[3], 2);
TonyYI 3:4306d042af6f 359 putByte(forward_msg_buffer[4], 2); //move_dir
TonyYI 3:4306d042af6f 360 putByte(forward_msg_buffer[5], 2); //rotate_dis
TonyYI 3:4306d042af6f 361 putByte(forward_msg_buffer[6], 2);
TonyYI 3:4306d042af6f 362 putByte(forward_msg_buffer[7], 2); //rotate_dir
TonyYI 3:4306d042af6f 363 putByte(forward_msg_buffer[8], 2); //checksum
TonyYI 0:3417ca0a36c0 364 }
TonyYI 0:3417ca0a36c0 365
TonyYI 0:3417ca0a36c0 366 void Communication::ACK(Lifter* lifter, Camera_platform* camera_platform)
TonyYI 0:3417ca0a36c0 367 {
TonyYI 0:3417ca0a36c0 368 if(action_type == 0) //car movement
TonyYI 0:3417ca0a36c0 369 {
TonyYI 3:4306d042af6f 370 for(int i = 0; i < 9; i++)
TonyYI 3:4306d042af6f 371 {
TonyYI 3:4306d042af6f 372 uint8_t _y = getByte(1);
TonyYI 3:4306d042af6f 373 printf("Communication::ACK(). Get byte: %x\r\n", _y);
TonyYI 3:4306d042af6f 374 }
TonyYI 3:4306d042af6f 375
TonyYI 0:3417ca0a36c0 376 while(info_ok_MbedToArduino != 1)
TonyYI 0:3417ca0a36c0 377 {
TonyYI 0:3417ca0a36c0 378 if(in_MbedToArduino != out_MbedToArduino)
TonyYI 0:3417ca0a36c0 379 {
TonyYI 0:3417ca0a36c0 380 uint8_t _x = getByte(1);
TonyYI 0:3417ca0a36c0 381 switch(state_MbedToArduino)
TonyYI 0:3417ca0a36c0 382 {
TonyYI 0:3417ca0a36c0 383 case 0: //checking starter
TonyYI 0:3417ca0a36c0 384 {
TonyYI 3:4306d042af6f 385 if(DEBUG_ON)
TonyYI 3:4306d042af6f 386 {
TonyYI 3:4306d042af6f 387 _DEBUG->printf("Communication::ACK(). Checking SARTER...\r\n");
TonyYI 3:4306d042af6f 388 }
TonyYI 3:4306d042af6f 389
TonyYI 0:3417ca0a36c0 390 if(_x == STARTER)
TonyYI 0:3417ca0a36c0 391 {
TonyYI 0:3417ca0a36c0 392 state_MbedToArduino++;
TonyYI 0:3417ca0a36c0 393 }
TonyYI 0:3417ca0a36c0 394 else
TonyYI 0:3417ca0a36c0 395 {
TonyYI 3:4306d042af6f 396 if(DEBUG_ON)
TonyYI 3:4306d042af6f 397 {
TonyYI 3:4306d042af6f 398 _DEBUG->printf("Communication::ACK(). ERROR when checking SARTER: %x\r\n", _x);
TonyYI 3:4306d042af6f 399 }
TonyYI 0:3417ca0a36c0 400 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 401 }
TonyYI 0:3417ca0a36c0 402 break;
TonyYI 0:3417ca0a36c0 403 }
TonyYI 0:3417ca0a36c0 404
TonyYI 0:3417ca0a36c0 405 case 1: //checking 'O'
TonyYI 0:3417ca0a36c0 406 {
TonyYI 3:4306d042af6f 407 if(DEBUG_ON)
TonyYI 3:4306d042af6f 408 {
TonyYI 3:4306d042af6f 409 _DEBUG->printf("Communication::ACK(). Checking O...\r\n");
TonyYI 3:4306d042af6f 410 }
TonyYI 3:4306d042af6f 411
TonyYI 0:3417ca0a36c0 412 if(_x == 0x4f) //O
TonyYI 0:3417ca0a36c0 413 {
TonyYI 0:3417ca0a36c0 414 state_MbedToArduino++;
TonyYI 0:3417ca0a36c0 415 }
TonyYI 0:3417ca0a36c0 416 else
TonyYI 0:3417ca0a36c0 417 {
TonyYI 3:4306d042af6f 418 if(DEBUG_ON)
TonyYI 3:4306d042af6f 419 {
TonyYI 3:4306d042af6f 420 _DEBUG->printf("Communication::ACK(). ERROR when checking O: %x\r\n", _x);
TonyYI 3:4306d042af6f 421 }
TonyYI 0:3417ca0a36c0 422 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 423 }
TonyYI 0:3417ca0a36c0 424 break;
TonyYI 0:3417ca0a36c0 425 }
TonyYI 0:3417ca0a36c0 426
TonyYI 0:3417ca0a36c0 427 case 2: //checking 'K'
TonyYI 0:3417ca0a36c0 428 {
TonyYI 3:4306d042af6f 429 if(DEBUG_ON)
TonyYI 3:4306d042af6f 430 {
TonyYI 3:4306d042af6f 431 _DEBUG->printf("Communication::ACK(). Checking K...\r\n");
TonyYI 3:4306d042af6f 432 }
TonyYI 3:4306d042af6f 433
TonyYI 0:3417ca0a36c0 434 if(_x == 0x4b) //K
TonyYI 0:3417ca0a36c0 435 {
TonyYI 0:3417ca0a36c0 436 state_MbedToArduino++;
TonyYI 0:3417ca0a36c0 437 }
TonyYI 0:3417ca0a36c0 438 else
TonyYI 0:3417ca0a36c0 439 {
TonyYI 3:4306d042af6f 440 if(DEBUG_ON)
TonyYI 3:4306d042af6f 441 {
TonyYI 3:4306d042af6f 442 _DEBUG->printf("Communication::ACK(). ERROR when checking K: %x\r\n", _x);
TonyYI 3:4306d042af6f 443 }
TonyYI 0:3417ca0a36c0 444 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 445 }
TonyYI 0:3417ca0a36c0 446 break;
TonyYI 0:3417ca0a36c0 447 }
TonyYI 0:3417ca0a36c0 448
TonyYI 0:3417ca0a36c0 449 case 3: //checking check_sum_MbedToArduino
TonyYI 0:3417ca0a36c0 450 {
TonyYI 3:4306d042af6f 451 if(DEBUG_ON)
TonyYI 3:4306d042af6f 452 {
TonyYI 3:4306d042af6f 453 _DEBUG->printf("Communication::ACK(). Checking CHECK_SUM...\r\n");
TonyYI 3:4306d042af6f 454 }
TonyYI 3:4306d042af6f 455
TonyYI 0:3417ca0a36c0 456 if(_x == 0x9a) //checksum
TonyYI 0:3417ca0a36c0 457 {
TonyYI 0:3417ca0a36c0 458 info_ok_MbedToArduino = 1;
TonyYI 0:3417ca0a36c0 459 }
TonyYI 3:4306d042af6f 460 else
TonyYI 3:4306d042af6f 461 {
TonyYI 3:4306d042af6f 462 if(DEBUG_ON)
TonyYI 3:4306d042af6f 463 {
TonyYI 3:4306d042af6f 464 _DEBUG->printf("Communication::ACK(). ERROR when checking CHECK_SUM: %x\r\n", _x);
TonyYI 3:4306d042af6f 465 }
TonyYI 3:4306d042af6f 466 }
TonyYI 0:3417ca0a36c0 467
TonyYI 0:3417ca0a36c0 468 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 469 break;
TonyYI 0:3417ca0a36c0 470 }
TonyYI 0:3417ca0a36c0 471
TonyYI 0:3417ca0a36c0 472 default:
TonyYI 0:3417ca0a36c0 473 {
TonyYI 0:3417ca0a36c0 474 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 475 info_ok_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 476 break;
TonyYI 0:3417ca0a36c0 477 }
TonyYI 0:3417ca0a36c0 478 }
TonyYI 0:3417ca0a36c0 479 }
TonyYI 0:3417ca0a36c0 480 }
TonyYI 0:3417ca0a36c0 481 }
TonyYI 0:3417ca0a36c0 482 else if(action_type == 1) //lifter
TonyYI 0:3417ca0a36c0 483 {
TonyYI 0:3417ca0a36c0 484 uint32_t pulseCountOld = 0;
TonyYI 0:3417ca0a36c0 485 uint32_t pulseCountNew = 0;
TonyYI 0:3417ca0a36c0 486 while(!lifter->isStopped())
TonyYI 0:3417ca0a36c0 487 {
TonyYI 0:3417ca0a36c0 488 pulseCountOld = lifter->pulseCount;
TonyYI 0:3417ca0a36c0 489 wait_ms(50);
TonyYI 0:3417ca0a36c0 490 pulseCountNew = lifter->pulseCount;
TonyYI 0:3417ca0a36c0 491 if(pulseCountOld == pulseCountNew)
TonyYI 0:3417ca0a36c0 492 {
TonyYI 0:3417ca0a36c0 493 break;
TonyYI 0:3417ca0a36c0 494 }
TonyYI 0:3417ca0a36c0 495 }
TonyYI 0:3417ca0a36c0 496 }
TonyYI 0:3417ca0a36c0 497 else if(action_type == 3)
TonyYI 0:3417ca0a36c0 498 {
terryLAI 1:cbec1283a16a 499
terryLAI 2:442902ec3aa1 500 campass_degree = compass->read();
TonyYI 0:3417ca0a36c0 501
TonyYI 0:3417ca0a36c0 502 uint8_t temp1,temp2;
TonyYI 0:3417ca0a36c0 503 temp1 = campass_degree;
TonyYI 0:3417ca0a36c0 504 temp2 = campass_degree>>8;
TonyYI 0:3417ca0a36c0 505 putByte(COMPASS_STARTER ,1); //1 means IntelToMbed
TonyYI 0:3417ca0a36c0 506 putByte(temp1 ,1); //O
TonyYI 0:3417ca0a36c0 507 putByte(temp2 ,1); //K
TonyYI 0:3417ca0a36c0 508 putByte(0x9a ,1); //check_sum = 0xaf + 0x4b = 0x9a
TonyYI 0:3417ca0a36c0 509
TonyYI 0:3417ca0a36c0 510 return;
TonyYI 0:3417ca0a36c0 511 }
terryLAI 2:442902ec3aa1 512 else if(action_type == 4)
terryLAI 2:442902ec3aa1 513 {
terryLAI 2:442902ec3aa1 514
terryLAI 2:442902ec3aa1 515 putByte(BUZZER_STARTER ,1); //1 means IntelToMbed
terryLAI 2:442902ec3aa1 516 putByte(0,1); //O
terryLAI 2:442902ec3aa1 517 putByte(0,1); //K
terryLAI 2:442902ec3aa1 518 putByte(0,1); //check_sum = 0xaf + 0x4b = 0x9a
terryLAI 2:442902ec3aa1 519
terryLAI 2:442902ec3aa1 520 return;
terryLAI 2:442902ec3aa1 521 }
TonyYI 0:3417ca0a36c0 522
TonyYI 0:3417ca0a36c0 523 putByte(STARTER ,1); //1 means IntelToMbed
TonyYI 0:3417ca0a36c0 524 putByte(0x4f ,1); //O
TonyYI 0:3417ca0a36c0 525 putByte(0x4b ,1); //K
TonyYI 0:3417ca0a36c0 526 putByte(0x9a ,1); //check_sum = 0xaf + 0x4b = 0x9a
TonyYI 0:3417ca0a36c0 527 }
TonyYI 0:3417ca0a36c0 528
TonyYI 0:3417ca0a36c0 529 uint8_t Communication::getInfoOK(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 530 {
TonyYI 0:3417ca0a36c0 531 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 532 {
TonyYI 0:3417ca0a36c0 533 return info_ok_IntelToMbed;
TonyYI 0:3417ca0a36c0 534 }
TonyYI 0:3417ca0a36c0 535 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 536 {
TonyYI 0:3417ca0a36c0 537 return info_ok_MbedToArduino;
TonyYI 0:3417ca0a36c0 538 }
TonyYI 0:3417ca0a36c0 539 return 0; //error
TonyYI 0:3417ca0a36c0 540 }
TonyYI 0:3417ca0a36c0 541
TonyYI 0:3417ca0a36c0 542 void Communication::resetInfoOK(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 543 {
TonyYI 0:3417ca0a36c0 544 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 545 {
TonyYI 0:3417ca0a36c0 546 info_ok_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 547 }
TonyYI 0:3417ca0a36c0 548 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 549 {
TonyYI 0:3417ca0a36c0 550 info_ok_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 551 }
TonyYI 0:3417ca0a36c0 552 }
TonyYI 0:3417ca0a36c0 553
TonyYI 0:3417ca0a36c0 554 uint16_t Communication::getMoveDis()
TonyYI 0:3417ca0a36c0 555 {
TonyYI 0:3417ca0a36c0 556 return move_dis;
TonyYI 0:3417ca0a36c0 557 }
TonyYI 0:3417ca0a36c0 558
TonyYI 0:3417ca0a36c0 559 uint16_t Communication::getRotateDis()
TonyYI 0:3417ca0a36c0 560 {
TonyYI 0:3417ca0a36c0 561 return rotate_dis;
TonyYI 0:3417ca0a36c0 562 }
TonyYI 0:3417ca0a36c0 563
TonyYI 0:3417ca0a36c0 564 uint8_t Communication::getMoveDir()
TonyYI 0:3417ca0a36c0 565 {
TonyYI 0:3417ca0a36c0 566 return move_dir;
TonyYI 0:3417ca0a36c0 567 }
TonyYI 0:3417ca0a36c0 568
TonyYI 0:3417ca0a36c0 569 uint8_t Communication::getRotateDir()
TonyYI 0:3417ca0a36c0 570 {
TonyYI 0:3417ca0a36c0 571 return rotate_dir;
TonyYI 0:3417ca0a36c0 572 }