terry LAI / Mbed 2 deprecated ESDC2014-pwm

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of ESDC2014 by terry LAI

communication.cpp@0:3417ca0a36c0, 2014-07-01 (annotated)

Committer:
TonyYI
Date:
Tue Jul 01 10:59:33 2014 +0000
Revision:
0:3417ca0a36c0
Child:
1:cbec1283a16a
2014

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