Tony YI / Mbed 2 deprecated ESDC2014

123123123123123123123123123

Dependencies:   mbed

communication.cpp@1:cbec1283a16a, 2014-07-03 (annotated)

Committer:
terryLAI
Date:
Thu Jul 03 03:56:29 2014 +0000
Revision:
1:cbec1283a16a
Parent:
0:3417ca0a36c0
Child:
2:442902ec3aa1
11111

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 0:3417ca0a36c0 30 Communication::Communication(MySerial* _DEBUG, MySerial *_IntelToMbed, MySerial *_MbedToArduino)
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;
terryLAI 1:cbec1283a16a 35 this->compass=new COMPASS(COMPASS_TX,COMPASS_RX);
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 0:3417ca0a36c0 43 delete _DEBUG;
TonyYI 0:3417ca0a36c0 44 delete _IntelToMbed;
TonyYI 0:3417ca0a36c0 45 delete _MbedToArduino;
TonyYI 0:3417ca0a36c0 46 }
TonyYI 0:3417ca0a36c0 47
TonyYI 0:3417ca0a36c0 48 void Communication::init()
TonyYI 0:3417ca0a36c0 49 {
TonyYI 0:3417ca0a36c0 50 buffer_IntelToMbed = new uint8_t[BUFFER_SIZE];
TonyYI 0:3417ca0a36c0 51 buffer_MbedToArduino = new uint8_t[BUFFER_SIZE];
TonyYI 0:3417ca0a36c0 52 in_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 53 out_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 54 in_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 55 out_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 56 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 57 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 58 check_sum = 0;
TonyYI 0:3417ca0a36c0 59 info_ok_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 60 info_ok_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 61 }
TonyYI 0:3417ca0a36c0 62
TonyYI 0:3417ca0a36c0 63 uint8_t Communication::getByte(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 64 {
TonyYI 0:3417ca0a36c0 65 uint8_t _x = 0;
TonyYI 0:3417ca0a36c0 66 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 67 {
TonyYI 0:3417ca0a36c0 68 _x = buffer_IntelToMbed[out_IntelToMbed++];
TonyYI 0:3417ca0a36c0 69 if(out_IntelToMbed == BUFFER_SIZE)
TonyYI 0:3417ca0a36c0 70 {
TonyYI 0:3417ca0a36c0 71 out_IntelToMbed &= 0x0000;
TonyYI 0:3417ca0a36c0 72 }
TonyYI 0:3417ca0a36c0 73 }
TonyYI 0:3417ca0a36c0 74 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 75 {
TonyYI 0:3417ca0a36c0 76 _x = buffer_MbedToArduino[out_MbedToArduino++];
TonyYI 0:3417ca0a36c0 77 if(out_MbedToArduino == BUFFER_SIZE)
TonyYI 0:3417ca0a36c0 78 {
TonyYI 0:3417ca0a36c0 79 out_MbedToArduino &= 0x0000;
TonyYI 0:3417ca0a36c0 80 }
TonyYI 0:3417ca0a36c0 81 }
TonyYI 0:3417ca0a36c0 82 return _x;
TonyYI 0:3417ca0a36c0 83 }
TonyYI 0:3417ca0a36c0 84
TonyYI 0:3417ca0a36c0 85 uint16_t Communication::get2Bytes(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 86 {
TonyYI 0:3417ca0a36c0 87 uint8_t byte1 = getByte(communication_type);
TonyYI 0:3417ca0a36c0 88 uint8_t byte2 = getByte(communication_type);
TonyYI 0:3417ca0a36c0 89 return uint16_t((byte1 << 8) | byte2);
TonyYI 0:3417ca0a36c0 90 }
TonyYI 0:3417ca0a36c0 91
TonyYI 0:3417ca0a36c0 92 void Communication::putByte(uint8_t _x, uint8_t _i)
TonyYI 0:3417ca0a36c0 93 {
TonyYI 0:3417ca0a36c0 94 //Serial.write(_x);//For Arduino
TonyYI 0:3417ca0a36c0 95 //For Mbed
TonyYI 0:3417ca0a36c0 96 if(_i == 0) //_DEBUG
TonyYI 0:3417ca0a36c0 97 {
TonyYI 0:3417ca0a36c0 98 _DEBUG->putc(_x);
TonyYI 0:3417ca0a36c0 99 }
TonyYI 0:3417ca0a36c0 100 else if(_i == 1) //IntelToMbed
TonyYI 0:3417ca0a36c0 101 {
TonyYI 0:3417ca0a36c0 102 _IntelToMbed->putc(_x);
TonyYI 0:3417ca0a36c0 103 }
TonyYI 0:3417ca0a36c0 104 else if(_i == 2) //MbedToArduino
TonyYI 0:3417ca0a36c0 105 {
TonyYI 0:3417ca0a36c0 106 _MbedToArduino->putc(_x);
TonyYI 0:3417ca0a36c0 107 }
TonyYI 0:3417ca0a36c0 108 }
TonyYI 0:3417ca0a36c0 109
TonyYI 0:3417ca0a36c0 110 void Communication::put2Bytes(uint16_t _x, uint8_t _i)
TonyYI 0:3417ca0a36c0 111 {
TonyYI 0:3417ca0a36c0 112 putByte(uint8_t(_x >> 8), _i);
TonyYI 0:3417ca0a36c0 113 putByte(uint8_t(_x & 0x0f), _i);
TonyYI 0:3417ca0a36c0 114 }
TonyYI 0:3417ca0a36c0 115
TonyYI 0:3417ca0a36c0 116 void Communication::putToBuffer(uint8_t _x, uint8_t communication_type)
TonyYI 0:3417ca0a36c0 117 {
TonyYI 0:3417ca0a36c0 118 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 119 {
TonyYI 0:3417ca0a36c0 120 buffer_IntelToMbed[in_IntelToMbed++] = _x;
TonyYI 0:3417ca0a36c0 121 if(in_IntelToMbed == BUFFER_SIZE)
TonyYI 0:3417ca0a36c0 122 {
TonyYI 0:3417ca0a36c0 123 in_IntelToMbed &= 0x00;
TonyYI 0:3417ca0a36c0 124 }
TonyYI 0:3417ca0a36c0 125 }
TonyYI 0:3417ca0a36c0 126 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 127 {
TonyYI 0:3417ca0a36c0 128 buffer_MbedToArduino[in_MbedToArduino++] = _x;
TonyYI 0:3417ca0a36c0 129 if(in_MbedToArduino == BUFFER_SIZE)
TonyYI 0:3417ca0a36c0 130 {
TonyYI 0:3417ca0a36c0 131 in_MbedToArduino &= 0x00;
TonyYI 0:3417ca0a36c0 132 }
TonyYI 0:3417ca0a36c0 133 }
TonyYI 0:3417ca0a36c0 134 }
TonyYI 0:3417ca0a36c0 135
TonyYI 0:3417ca0a36c0 136 void Communication::parseMessage()
TonyYI 0:3417ca0a36c0 137 {
TonyYI 0:3417ca0a36c0 138 if(in_IntelToMbed != out_IntelToMbed)
TonyYI 0:3417ca0a36c0 139 {
TonyYI 0:3417ca0a36c0 140 uint8_t _x = getByte(0);
TonyYI 0:3417ca0a36c0 141 switch(state_IntelToMbed)
TonyYI 0:3417ca0a36c0 142 {
TonyYI 0:3417ca0a36c0 143 case 0: //checking starter
TonyYI 0:3417ca0a36c0 144 {
TonyYI 0:3417ca0a36c0 145 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 146 {
TonyYI 0:3417ca0a36c0 147 _DEBUG->printf("Communication::parseMessage(). Checking STARTER...\r\n");
TonyYI 0:3417ca0a36c0 148 }
TonyYI 0:3417ca0a36c0 149 check_sum = 0;
TonyYI 0:3417ca0a36c0 150
TonyYI 0:3417ca0a36c0 151 if(_x == STARTER || _x == COMPASS_STARTER)
TonyYI 0:3417ca0a36c0 152 {
TonyYI 0:3417ca0a36c0 153 state_IntelToMbed++;
TonyYI 0:3417ca0a36c0 154 }
TonyYI 0:3417ca0a36c0 155 else
TonyYI 0:3417ca0a36c0 156 {
TonyYI 0:3417ca0a36c0 157 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 158 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 159 {
TonyYI 0:3417ca0a36c0 160 _DEBUG->printf("Communication::parseMessage(). ERROR when checking STARTER: %x.\r\n", _x);
TonyYI 0:3417ca0a36c0 161 }
TonyYI 0:3417ca0a36c0 162 }
TonyYI 0:3417ca0a36c0 163 break;
TonyYI 0:3417ca0a36c0 164 }
TonyYI 0:3417ca0a36c0 165
TonyYI 0:3417ca0a36c0 166 case 1: //checking action_type
TonyYI 0:3417ca0a36c0 167 {
TonyYI 0:3417ca0a36c0 168 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 169 {
TonyYI 0:3417ca0a36c0 170 _DEBUG->printf("Communication::parseMessage(). Checking ACTION_TYPE...\r\n");
TonyYI 0:3417ca0a36c0 171 }
TonyYI 0:3417ca0a36c0 172 check_sum += _x;
TonyYI 0:3417ca0a36c0 173 action_type = _x;
TonyYI 0:3417ca0a36c0 174 if(action_type == 0 || action_type == 1 || action_type == 2 || action_type == 3)
TonyYI 0:3417ca0a36c0 175 {
TonyYI 0:3417ca0a36c0 176 state_IntelToMbed++;
TonyYI 0:3417ca0a36c0 177 }
TonyYI 0:3417ca0a36c0 178 else
TonyYI 0:3417ca0a36c0 179 {
TonyYI 0:3417ca0a36c0 180 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 181 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 182 {
TonyYI 0:3417ca0a36c0 183 _DEBUG->printf("Communication::parseMessage(). ERROR when checking ACTION_TYPE: %x.\r\n", action_type);
TonyYI 0:3417ca0a36c0 184 }
TonyYI 0:3417ca0a36c0 185 }
TonyYI 0:3417ca0a36c0 186 break;
TonyYI 0:3417ca0a36c0 187 }
TonyYI 0:3417ca0a36c0 188
TonyYI 0:3417ca0a36c0 189 case 2: //move_dis upper 4 bits
TonyYI 0:3417ca0a36c0 190 {
TonyYI 0:3417ca0a36c0 191 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 192 {
TonyYI 0:3417ca0a36c0 193 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIS upper 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 194 }
TonyYI 0:3417ca0a36c0 195 check_sum += _x;
TonyYI 0:3417ca0a36c0 196 move_dis = _x << 8;
TonyYI 0:3417ca0a36c0 197 state_IntelToMbed++;
TonyYI 0:3417ca0a36c0 198 break;
TonyYI 0:3417ca0a36c0 199 }
TonyYI 0:3417ca0a36c0 200
TonyYI 0:3417ca0a36c0 201 case 3: //move_dis lower 4 bits
TonyYI 0:3417ca0a36c0 202 {
TonyYI 0:3417ca0a36c0 203 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 204 {
TonyYI 0:3417ca0a36c0 205 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIS lower 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 206 }
TonyYI 0:3417ca0a36c0 207 check_sum += _x;
TonyYI 0:3417ca0a36c0 208 move_dis |= _x;
TonyYI 0:3417ca0a36c0 209 state_IntelToMbed++;
TonyYI 0:3417ca0a36c0 210 break;
TonyYI 0:3417ca0a36c0 211 }
TonyYI 0:3417ca0a36c0 212
TonyYI 0:3417ca0a36c0 213 case 4: //move_dir
TonyYI 0:3417ca0a36c0 214 {
TonyYI 0:3417ca0a36c0 215 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 216 {
TonyYI 0:3417ca0a36c0 217 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIR...\r\n");
TonyYI 0:3417ca0a36c0 218 }
TonyYI 0:3417ca0a36c0 219 check_sum += _x;
TonyYI 0:3417ca0a36c0 220 move_dir = _x;
TonyYI 0:3417ca0a36c0 221 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)
TonyYI 0:3417ca0a36c0 222 {
TonyYI 0:3417ca0a36c0 223 state_IntelToMbed++;
TonyYI 0:3417ca0a36c0 224 }
TonyYI 0:3417ca0a36c0 225 else
TonyYI 0:3417ca0a36c0 226 {
TonyYI 0:3417ca0a36c0 227 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 228 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 229 {
TonyYI 0:3417ca0a36c0 230 _DEBUG->printf("Communication::parseMessage(). ERROR when checking MOVE_DIR: %x.\r\n", move_dir);
TonyYI 0:3417ca0a36c0 231 }
TonyYI 0:3417ca0a36c0 232 }
TonyYI 0:3417ca0a36c0 233 break;
TonyYI 0:3417ca0a36c0 234 }
TonyYI 0:3417ca0a36c0 235
TonyYI 0:3417ca0a36c0 236 case 5: //rotate_dis upper 4 bits
TonyYI 0:3417ca0a36c0 237 {
TonyYI 0:3417ca0a36c0 238 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 239 {
TonyYI 0:3417ca0a36c0 240 _DEBUG->printf("Communication::parseMessage(). Checking ROTATE_DIS upper 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 241 }
TonyYI 0:3417ca0a36c0 242 check_sum += _x;
TonyYI 0:3417ca0a36c0 243 rotate_dis = _x << 8;
TonyYI 0:3417ca0a36c0 244 state_IntelToMbed++;
TonyYI 0:3417ca0a36c0 245 break;
TonyYI 0:3417ca0a36c0 246 }
TonyYI 0:3417ca0a36c0 247
TonyYI 0:3417ca0a36c0 248 case 6: //rotate_dis lower 4 bits
TonyYI 0:3417ca0a36c0 249 {
TonyYI 0:3417ca0a36c0 250 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 251 {
TonyYI 0:3417ca0a36c0 252 _DEBUG->printf("Communication::parseMessage(). Checking ROTATE_DIS lower 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 253 }
TonyYI 0:3417ca0a36c0 254 check_sum += _x;
TonyYI 0:3417ca0a36c0 255 rotate_dis |= _x;
TonyYI 0:3417ca0a36c0 256 state_IntelToMbed++;
TonyYI 0:3417ca0a36c0 257 break;
TonyYI 0:3417ca0a36c0 258 }
TonyYI 0:3417ca0a36c0 259
TonyYI 0:3417ca0a36c0 260 case 7: //rotate_dir
TonyYI 0:3417ca0a36c0 261 {
TonyYI 0:3417ca0a36c0 262 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 263 {
TonyYI 0:3417ca0a36c0 264 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIR...\r\n");
TonyYI 0:3417ca0a36c0 265 }
TonyYI 0:3417ca0a36c0 266 check_sum += _x;
TonyYI 0:3417ca0a36c0 267 rotate_dir = _x;
TonyYI 0:3417ca0a36c0 268 if(action_type == 3 || (action_type == 1 && ((rotate_dir >> 6) == 0)) || ((action_type == 0 || action_type == 2) && ((rotate_dir >> 6) == 3)))
TonyYI 0:3417ca0a36c0 269 {
TonyYI 0:3417ca0a36c0 270 state_IntelToMbed++;
TonyYI 0:3417ca0a36c0 271 }
TonyYI 0:3417ca0a36c0 272 else
TonyYI 0:3417ca0a36c0 273 {
TonyYI 0:3417ca0a36c0 274 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 275 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 276 {
TonyYI 0:3417ca0a36c0 277 _DEBUG->printf("Communication::parseMessage(). ERROR when checking ROTATE_DIR: %x.\r\n", rotate_dir);
TonyYI 0:3417ca0a36c0 278 }
TonyYI 0:3417ca0a36c0 279 }
TonyYI 0:3417ca0a36c0 280 break;
TonyYI 0:3417ca0a36c0 281 }
TonyYI 0:3417ca0a36c0 282
TonyYI 0:3417ca0a36c0 283 case 8: //check_sum
TonyYI 0:3417ca0a36c0 284 {
TonyYI 0:3417ca0a36c0 285 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 286 {
TonyYI 0:3417ca0a36c0 287 _DEBUG->printf("Communication::parseMessage(). Checking CHECK_SUM...\r\n");
TonyYI 0:3417ca0a36c0 288 }
TonyYI 0:3417ca0a36c0 289 if(check_sum == _x)
TonyYI 0:3417ca0a36c0 290 {
TonyYI 0:3417ca0a36c0 291 switch(action_type)
TonyYI 0:3417ca0a36c0 292 {
TonyYI 0:3417ca0a36c0 293 case 0: //car movement
TonyYI 0:3417ca0a36c0 294 info_ok_IntelToMbed = 1;
TonyYI 0:3417ca0a36c0 295 break;
TonyYI 0:3417ca0a36c0 296
TonyYI 0:3417ca0a36c0 297 case 1: //lifter
TonyYI 0:3417ca0a36c0 298 info_ok_IntelToMbed = 2;
TonyYI 0:3417ca0a36c0 299 break;
TonyYI 0:3417ca0a36c0 300
TonyYI 0:3417ca0a36c0 301 case 2: //camera platform
TonyYI 0:3417ca0a36c0 302 info_ok_IntelToMbed = 3;
TonyYI 0:3417ca0a36c0 303 break;
TonyYI 0:3417ca0a36c0 304
TonyYI 0:3417ca0a36c0 305 case 3: //compass
TonyYI 0:3417ca0a36c0 306 info_ok_IntelToMbed = 4;
TonyYI 0:3417ca0a36c0 307 break;
TonyYI 0:3417ca0a36c0 308
TonyYI 0:3417ca0a36c0 309 default:
TonyYI 0:3417ca0a36c0 310 info_ok_IntelToMbed = 0; //not ok
TonyYI 0:3417ca0a36c0 311 break;
TonyYI 0:3417ca0a36c0 312 }
TonyYI 0:3417ca0a36c0 313 }
TonyYI 0:3417ca0a36c0 314 else
TonyYI 0:3417ca0a36c0 315 {
TonyYI 0:3417ca0a36c0 316 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 317 {
TonyYI 0:3417ca0a36c0 318 _DEBUG->printf("Communication::parseMessage(). ERROR when checking CHECK_SUM: %x.\r\n", check_sum);
TonyYI 0:3417ca0a36c0 319 }
TonyYI 0:3417ca0a36c0 320 }
TonyYI 0:3417ca0a36c0 321 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 322 break;
TonyYI 0:3417ca0a36c0 323 }
TonyYI 0:3417ca0a36c0 324
TonyYI 0:3417ca0a36c0 325 default:
TonyYI 0:3417ca0a36c0 326 {
TonyYI 0:3417ca0a36c0 327 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 328 break;
TonyYI 0:3417ca0a36c0 329 }
TonyYI 0:3417ca0a36c0 330 }
TonyYI 0:3417ca0a36c0 331 }
TonyYI 0:3417ca0a36c0 332 }
TonyYI 0:3417ca0a36c0 333
TonyYI 0:3417ca0a36c0 334 void Communication::forwardMessage()
TonyYI 0:3417ca0a36c0 335 {
TonyYI 0:3417ca0a36c0 336 //message structure is defined in source/motion_platform/intel_board/lib/message.h
TonyYI 0:3417ca0a36c0 337 uint8_t i = out_IntelToMbed - 9; //message size is 9 bytes
TonyYI 0:3417ca0a36c0 338 putByte(buffer_IntelToMbed[i++], 2); //starter, 2 means MbedToArduino
TonyYI 0:3417ca0a36c0 339 putByte(buffer_IntelToMbed[i++], 2); //action_type
TonyYI 0:3417ca0a36c0 340 putByte(buffer_IntelToMbed[i++], 2); //move_dis
TonyYI 0:3417ca0a36c0 341 putByte(buffer_IntelToMbed[i++], 2);
TonyYI 0:3417ca0a36c0 342 putByte(buffer_IntelToMbed[i++], 2); //move_dir
TonyYI 0:3417ca0a36c0 343 putByte(buffer_IntelToMbed[i++], 2); //rotate_dis
TonyYI 0:3417ca0a36c0 344 putByte(buffer_IntelToMbed[i++], 2);
TonyYI 0:3417ca0a36c0 345 putByte(buffer_IntelToMbed[i++], 2); //rotate_dir
TonyYI 0:3417ca0a36c0 346 putByte(buffer_IntelToMbed[i++], 2); //checksum
TonyYI 0:3417ca0a36c0 347 }
TonyYI 0:3417ca0a36c0 348
TonyYI 0:3417ca0a36c0 349 void Communication::ACK(Lifter* lifter, Camera_platform* camera_platform)
TonyYI 0:3417ca0a36c0 350 {
TonyYI 0:3417ca0a36c0 351 if(action_type == 0) //car movement
TonyYI 0:3417ca0a36c0 352 {
TonyYI 0:3417ca0a36c0 353 while(info_ok_MbedToArduino != 1)
TonyYI 0:3417ca0a36c0 354 {
TonyYI 0:3417ca0a36c0 355 if(in_MbedToArduino != out_MbedToArduino)
TonyYI 0:3417ca0a36c0 356 {
TonyYI 0:3417ca0a36c0 357 uint8_t _x = getByte(1);
TonyYI 0:3417ca0a36c0 358 switch(state_MbedToArduino)
TonyYI 0:3417ca0a36c0 359 {
TonyYI 0:3417ca0a36c0 360 case 0: //checking starter
TonyYI 0:3417ca0a36c0 361 {
TonyYI 0:3417ca0a36c0 362 //putByte('0', 1);
TonyYI 0:3417ca0a36c0 363 if(_x == STARTER)
TonyYI 0:3417ca0a36c0 364 {
TonyYI 0:3417ca0a36c0 365 state_MbedToArduino++;
TonyYI 0:3417ca0a36c0 366 }
TonyYI 0:3417ca0a36c0 367 else
TonyYI 0:3417ca0a36c0 368 {
TonyYI 0:3417ca0a36c0 369 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 370 }
TonyYI 0:3417ca0a36c0 371 break;
TonyYI 0:3417ca0a36c0 372 }
TonyYI 0:3417ca0a36c0 373
TonyYI 0:3417ca0a36c0 374 case 1: //checking 'O'
TonyYI 0:3417ca0a36c0 375 {
TonyYI 0:3417ca0a36c0 376 //putByte('1', 1);
TonyYI 0:3417ca0a36c0 377 if(_x == 0x4f) //O
TonyYI 0:3417ca0a36c0 378 {
TonyYI 0:3417ca0a36c0 379 state_MbedToArduino++;
TonyYI 0:3417ca0a36c0 380 }
TonyYI 0:3417ca0a36c0 381 else
TonyYI 0:3417ca0a36c0 382 {
TonyYI 0:3417ca0a36c0 383 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 384 }
TonyYI 0:3417ca0a36c0 385 break;
TonyYI 0:3417ca0a36c0 386 }
TonyYI 0:3417ca0a36c0 387
TonyYI 0:3417ca0a36c0 388 case 2: //checking 'K'
TonyYI 0:3417ca0a36c0 389 {
TonyYI 0:3417ca0a36c0 390 //putByte('2', 1);
TonyYI 0:3417ca0a36c0 391 if(_x == 0x4b) //K
TonyYI 0:3417ca0a36c0 392 {
TonyYI 0:3417ca0a36c0 393 state_MbedToArduino++;
TonyYI 0:3417ca0a36c0 394 }
TonyYI 0:3417ca0a36c0 395 else
TonyYI 0:3417ca0a36c0 396 {
TonyYI 0:3417ca0a36c0 397 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 398 }
TonyYI 0:3417ca0a36c0 399 break;
TonyYI 0:3417ca0a36c0 400 }
TonyYI 0:3417ca0a36c0 401
TonyYI 0:3417ca0a36c0 402 case 3: //checking check_sum_MbedToArduino
TonyYI 0:3417ca0a36c0 403 {
TonyYI 0:3417ca0a36c0 404 //putByte('3', 1);
TonyYI 0:3417ca0a36c0 405 if(_x == 0x9a) //checksum
TonyYI 0:3417ca0a36c0 406 {
TonyYI 0:3417ca0a36c0 407 info_ok_MbedToArduino = 1;
TonyYI 0:3417ca0a36c0 408 }
TonyYI 0:3417ca0a36c0 409
TonyYI 0:3417ca0a36c0 410 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 411 break;
TonyYI 0:3417ca0a36c0 412 }
TonyYI 0:3417ca0a36c0 413
TonyYI 0:3417ca0a36c0 414 default:
TonyYI 0:3417ca0a36c0 415 {
TonyYI 0:3417ca0a36c0 416 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 417 info_ok_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 418 break;
TonyYI 0:3417ca0a36c0 419 }
TonyYI 0:3417ca0a36c0 420 }
TonyYI 0:3417ca0a36c0 421 }
TonyYI 0:3417ca0a36c0 422 }
TonyYI 0:3417ca0a36c0 423 }
TonyYI 0:3417ca0a36c0 424 else if(action_type == 1) //lifter
TonyYI 0:3417ca0a36c0 425 {
TonyYI 0:3417ca0a36c0 426 uint32_t pulseCountOld = 0;
TonyYI 0:3417ca0a36c0 427 uint32_t pulseCountNew = 0;
TonyYI 0:3417ca0a36c0 428 while(!lifter->isStopped())
TonyYI 0:3417ca0a36c0 429 {
TonyYI 0:3417ca0a36c0 430 pulseCountOld = lifter->pulseCount;
TonyYI 0:3417ca0a36c0 431 wait_ms(50);
TonyYI 0:3417ca0a36c0 432 pulseCountNew = lifter->pulseCount;
TonyYI 0:3417ca0a36c0 433 if(pulseCountOld == pulseCountNew)
TonyYI 0:3417ca0a36c0 434 {
TonyYI 0:3417ca0a36c0 435 break;
TonyYI 0:3417ca0a36c0 436 }
TonyYI 0:3417ca0a36c0 437 }
TonyYI 0:3417ca0a36c0 438 }
TonyYI 0:3417ca0a36c0 439 else if(action_type == 3)
TonyYI 0:3417ca0a36c0 440 {
terryLAI 1:cbec1283a16a 441
terryLAI 1:cbec1283a16a 442 campass_degree = compass->Read();
TonyYI 0:3417ca0a36c0 443
TonyYI 0:3417ca0a36c0 444 uint8_t temp1,temp2;
TonyYI 0:3417ca0a36c0 445 temp1 = campass_degree;
TonyYI 0:3417ca0a36c0 446 temp2 = campass_degree>>8;
TonyYI 0:3417ca0a36c0 447 putByte(COMPASS_STARTER ,1); //1 means IntelToMbed
TonyYI 0:3417ca0a36c0 448 putByte(temp1 ,1); //O
TonyYI 0:3417ca0a36c0 449 putByte(temp2 ,1); //K
TonyYI 0:3417ca0a36c0 450 putByte(0x9a ,1); //check_sum = 0xaf + 0x4b = 0x9a
TonyYI 0:3417ca0a36c0 451
TonyYI 0:3417ca0a36c0 452 return;
TonyYI 0:3417ca0a36c0 453 }
TonyYI 0:3417ca0a36c0 454
TonyYI 0:3417ca0a36c0 455 putByte(STARTER ,1); //1 means IntelToMbed
TonyYI 0:3417ca0a36c0 456 putByte(0x4f ,1); //O
TonyYI 0:3417ca0a36c0 457 putByte(0x4b ,1); //K
TonyYI 0:3417ca0a36c0 458 putByte(0x9a ,1); //check_sum = 0xaf + 0x4b = 0x9a
TonyYI 0:3417ca0a36c0 459 }
TonyYI 0:3417ca0a36c0 460
TonyYI 0:3417ca0a36c0 461 uint8_t Communication::getInfoOK(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 462 {
TonyYI 0:3417ca0a36c0 463 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 464 {
TonyYI 0:3417ca0a36c0 465 return info_ok_IntelToMbed;
TonyYI 0:3417ca0a36c0 466 }
TonyYI 0:3417ca0a36c0 467 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 468 {
TonyYI 0:3417ca0a36c0 469 return info_ok_MbedToArduino;
TonyYI 0:3417ca0a36c0 470 }
TonyYI 0:3417ca0a36c0 471 return 0; //error
TonyYI 0:3417ca0a36c0 472 }
TonyYI 0:3417ca0a36c0 473
TonyYI 0:3417ca0a36c0 474 void Communication::resetInfoOK(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 475 {
TonyYI 0:3417ca0a36c0 476 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 477 {
TonyYI 0:3417ca0a36c0 478 info_ok_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 479 }
TonyYI 0:3417ca0a36c0 480 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 481 {
TonyYI 0:3417ca0a36c0 482 info_ok_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 483 }
TonyYI 0:3417ca0a36c0 484 }
TonyYI 0:3417ca0a36c0 485
TonyYI 0:3417ca0a36c0 486 uint16_t Communication::getMoveDis()
TonyYI 0:3417ca0a36c0 487 {
TonyYI 0:3417ca0a36c0 488 return move_dis;
TonyYI 0:3417ca0a36c0 489 }
TonyYI 0:3417ca0a36c0 490
TonyYI 0:3417ca0a36c0 491 uint16_t Communication::getRotateDis()
TonyYI 0:3417ca0a36c0 492 {
TonyYI 0:3417ca0a36c0 493 return rotate_dis;
TonyYI 0:3417ca0a36c0 494 }
TonyYI 0:3417ca0a36c0 495
TonyYI 0:3417ca0a36c0 496 uint8_t Communication::getMoveDir()
TonyYI 0:3417ca0a36c0 497 {
TonyYI 0:3417ca0a36c0 498 return move_dir;
TonyYI 0:3417ca0a36c0 499 }
TonyYI 0:3417ca0a36c0 500
TonyYI 0:3417ca0a36c0 501 uint8_t Communication::getRotateDir()
TonyYI 0:3417ca0a36c0 502 {
TonyYI 0:3417ca0a36c0 503 return rotate_dir;
TonyYI 0:3417ca0a36c0 504 }