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@5:098e6a44bd94, 2014-07-04 (annotated)

Committer:
terryLAI
Date:
Fri Jul 04 16:08:29 2014 +0000
Revision:
5:098e6a44bd94
Parent:
4:a377ecb9364f
Child:
6:7ab57721c0fb
can work

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
terryLAI 5:098e6a44bd94 30 Communication::Communication(MySerial* _DEBUG, MySerial *_IntelToMbed, MySerial *_MbedToArduino, MySerial* CompassData)
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 5:098e6a44bd94 35 this->CompassData = CompassData;
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;
terryLAI 5:098e6a44bd94 43 delete[] buffer_compass;
TonyYI 3:4306d042af6f 44 delete[] forward_msg_buffer;
TonyYI 0:3417ca0a36c0 45 delete _DEBUG;
TonyYI 0:3417ca0a36c0 46 delete _IntelToMbed;
TonyYI 0:3417ca0a36c0 47 delete _MbedToArduino;
terryLAI 5:098e6a44bd94 48 delete CompassData;
terryLAI 5:098e6a44bd94 49
terryLAI 5:098e6a44bd94 50
TonyYI 0:3417ca0a36c0 51 }
TonyYI 0:3417ca0a36c0 52
TonyYI 0:3417ca0a36c0 53 void Communication::init()
TonyYI 0:3417ca0a36c0 54 {
TonyYI 0:3417ca0a36c0 55 buffer_IntelToMbed = new uint8_t[BUFFER_SIZE];
TonyYI 0:3417ca0a36c0 56 buffer_MbedToArduino = new uint8_t[BUFFER_SIZE];
terryLAI 5:098e6a44bd94 57 buffer_compass = new uint8_t [BUFFER_SIZE];
terryLAI 5:098e6a44bd94 58
TonyYI 3:4306d042af6f 59
TonyYI 3:4306d042af6f 60 forward_msg_buffer = new uint8_t[9]; //the message struct is 9 byte
TonyYI 3:4306d042af6f 61
TonyYI 0:3417ca0a36c0 62 in_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 63 out_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 64 in_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 65 out_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 66 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 67 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 68 check_sum = 0;
TonyYI 0:3417ca0a36c0 69 info_ok_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 70 info_ok_MbedToArduino = 0;
terryLAI 5:098e6a44bd94 71
terryLAI 5:098e6a44bd94 72
terryLAI 5:098e6a44bd94 73
terryLAI 5:098e6a44bd94 74 _MSB = 0;
terryLAI 5:098e6a44bd94 75 _LSB = 0;
terryLAI 5:098e6a44bd94 76 _in = _out = 0;
terryLAI 5:098e6a44bd94 77
terryLAI 5:098e6a44bd94 78
TonyYI 0:3417ca0a36c0 79 }
TonyYI 0:3417ca0a36c0 80
TonyYI 0:3417ca0a36c0 81 uint8_t Communication::getByte(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 82 {
TonyYI 0:3417ca0a36c0 83 uint8_t _x = 0;
TonyYI 0:3417ca0a36c0 84 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 85 {
TonyYI 0:3417ca0a36c0 86 _x = buffer_IntelToMbed[out_IntelToMbed++];
terryLAI 4:a377ecb9364f 87 if(out_IntelToMbed == BUFFER_SIZE-1)
TonyYI 0:3417ca0a36c0 88 {
TonyYI 0:3417ca0a36c0 89 out_IntelToMbed &= 0x0000;
TonyYI 0:3417ca0a36c0 90 }
TonyYI 0:3417ca0a36c0 91 }
TonyYI 0:3417ca0a36c0 92 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 93 {
TonyYI 0:3417ca0a36c0 94 _x = buffer_MbedToArduino[out_MbedToArduino++];
terryLAI 4:a377ecb9364f 95 if(out_MbedToArduino == BUFFER_SIZE-1)
TonyYI 0:3417ca0a36c0 96 {
TonyYI 0:3417ca0a36c0 97 out_MbedToArduino &= 0x0000;
TonyYI 0:3417ca0a36c0 98 }
TonyYI 0:3417ca0a36c0 99 }
terryLAI 5:098e6a44bd94 100 else if(communication_type == 2)
terryLAI 5:098e6a44bd94 101 {
terryLAI 5:098e6a44bd94 102 _x = buffer_compass[_out++];
terryLAI 5:098e6a44bd94 103 if(_out == BUFFER_SIZE-1)
terryLAI 5:098e6a44bd94 104 {
terryLAI 5:098e6a44bd94 105 _out &= 0x0000;
terryLAI 5:098e6a44bd94 106 }
terryLAI 5:098e6a44bd94 107 }
terryLAI 5:098e6a44bd94 108
TonyYI 0:3417ca0a36c0 109 return _x;
TonyYI 0:3417ca0a36c0 110 }
TonyYI 0:3417ca0a36c0 111
TonyYI 0:3417ca0a36c0 112 uint16_t Communication::get2Bytes(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 113 {
TonyYI 0:3417ca0a36c0 114 uint8_t byte1 = getByte(communication_type);
TonyYI 0:3417ca0a36c0 115 uint8_t byte2 = getByte(communication_type);
TonyYI 0:3417ca0a36c0 116 return uint16_t((byte1 << 8) | byte2);
TonyYI 0:3417ca0a36c0 117 }
TonyYI 0:3417ca0a36c0 118
TonyYI 0:3417ca0a36c0 119 void Communication::putByte(uint8_t _x, uint8_t _i)
TonyYI 0:3417ca0a36c0 120 {
TonyYI 0:3417ca0a36c0 121 //Serial.write(_x);//For Arduino
TonyYI 0:3417ca0a36c0 122 //For Mbed
TonyYI 0:3417ca0a36c0 123 if(_i == 0) //_DEBUG
TonyYI 0:3417ca0a36c0 124 {
TonyYI 0:3417ca0a36c0 125 _DEBUG->putc(_x);
TonyYI 0:3417ca0a36c0 126 }
TonyYI 0:3417ca0a36c0 127 else if(_i == 1) //IntelToMbed
TonyYI 0:3417ca0a36c0 128 {
TonyYI 0:3417ca0a36c0 129 _IntelToMbed->putc(_x);
TonyYI 0:3417ca0a36c0 130 }
TonyYI 0:3417ca0a36c0 131 else if(_i == 2) //MbedToArduino
TonyYI 0:3417ca0a36c0 132 {
TonyYI 0:3417ca0a36c0 133 _MbedToArduino->putc(_x);
TonyYI 0:3417ca0a36c0 134 }
terryLAI 5:098e6a44bd94 135
TonyYI 0:3417ca0a36c0 136 }
TonyYI 0:3417ca0a36c0 137
TonyYI 0:3417ca0a36c0 138 void Communication::put2Bytes(uint16_t _x, uint8_t _i)
TonyYI 0:3417ca0a36c0 139 {
TonyYI 0:3417ca0a36c0 140 putByte(uint8_t(_x >> 8), _i);
TonyYI 0:3417ca0a36c0 141 putByte(uint8_t(_x & 0x0f), _i);
TonyYI 0:3417ca0a36c0 142 }
TonyYI 0:3417ca0a36c0 143
TonyYI 0:3417ca0a36c0 144 void Communication::putToBuffer(uint8_t _x, uint8_t communication_type)
TonyYI 0:3417ca0a36c0 145 {
TonyYI 0:3417ca0a36c0 146 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 147 {
TonyYI 0:3417ca0a36c0 148 buffer_IntelToMbed[in_IntelToMbed++] = _x;
terryLAI 4:a377ecb9364f 149 if(in_IntelToMbed == BUFFER_SIZE-1)
TonyYI 0:3417ca0a36c0 150 {
TonyYI 3:4306d042af6f 151 in_IntelToMbed &= 0x0000;
TonyYI 0:3417ca0a36c0 152 }
TonyYI 0:3417ca0a36c0 153 }
TonyYI 0:3417ca0a36c0 154 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 155 {
TonyYI 0:3417ca0a36c0 156 buffer_MbedToArduino[in_MbedToArduino++] = _x;
terryLAI 4:a377ecb9364f 157 if(in_MbedToArduino == BUFFER_SIZE-1)
TonyYI 0:3417ca0a36c0 158 {
TonyYI 3:4306d042af6f 159 in_MbedToArduino &= 0x0000;
TonyYI 0:3417ca0a36c0 160 }
TonyYI 0:3417ca0a36c0 161 }
terryLAI 5:098e6a44bd94 162 else if(communication_type == 2)
terryLAI 5:098e6a44bd94 163 {
terryLAI 5:098e6a44bd94 164
terryLAI 5:098e6a44bd94 165 buffer_compass[_in++] = _x;
terryLAI 5:098e6a44bd94 166 if(_in == BUFFER_SIZE-1)
terryLAI 5:098e6a44bd94 167 {
terryLAI 5:098e6a44bd94 168 _in &= 0x0000;
terryLAI 5:098e6a44bd94 169 }
terryLAI 5:098e6a44bd94 170 }
TonyYI 0:3417ca0a36c0 171 }
TonyYI 0:3417ca0a36c0 172
TonyYI 0:3417ca0a36c0 173 void Communication::parseMessage()
TonyYI 0:3417ca0a36c0 174 {
TonyYI 0:3417ca0a36c0 175 if(in_IntelToMbed != out_IntelToMbed)
TonyYI 0:3417ca0a36c0 176 {
TonyYI 0:3417ca0a36c0 177 uint8_t _x = getByte(0);
TonyYI 0:3417ca0a36c0 178 switch(state_IntelToMbed)
TonyYI 0:3417ca0a36c0 179 {
TonyYI 0:3417ca0a36c0 180 case 0: //checking starter
TonyYI 0:3417ca0a36c0 181 {
TonyYI 0:3417ca0a36c0 182 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 183 {
TonyYI 0:3417ca0a36c0 184 _DEBUG->printf("Communication::parseMessage(). Checking STARTER...\r\n");
TonyYI 0:3417ca0a36c0 185 }
TonyYI 0:3417ca0a36c0 186 check_sum = 0;
TonyYI 0:3417ca0a36c0 187
TonyYI 0:3417ca0a36c0 188 if(_x == STARTER || _x == COMPASS_STARTER)
TonyYI 0:3417ca0a36c0 189 {
TonyYI 0:3417ca0a36c0 190 state_IntelToMbed++;
TonyYI 3:4306d042af6f 191 forward_msg_buffer[0] = _x;
TonyYI 0:3417ca0a36c0 192 }
TonyYI 0:3417ca0a36c0 193 else
TonyYI 0:3417ca0a36c0 194 {
TonyYI 0:3417ca0a36c0 195 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 196 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 197 {
TonyYI 0:3417ca0a36c0 198 _DEBUG->printf("Communication::parseMessage(). ERROR when checking STARTER: %x.\r\n", _x);
TonyYI 0:3417ca0a36c0 199 }
TonyYI 0:3417ca0a36c0 200 }
TonyYI 0:3417ca0a36c0 201 break;
TonyYI 0:3417ca0a36c0 202 }
TonyYI 0:3417ca0a36c0 203
TonyYI 0:3417ca0a36c0 204 case 1: //checking action_type
TonyYI 0:3417ca0a36c0 205 {
TonyYI 0:3417ca0a36c0 206 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 207 {
TonyYI 0:3417ca0a36c0 208 _DEBUG->printf("Communication::parseMessage(). Checking ACTION_TYPE...\r\n");
TonyYI 0:3417ca0a36c0 209 }
TonyYI 0:3417ca0a36c0 210 check_sum += _x;
TonyYI 0:3417ca0a36c0 211 action_type = _x;
terryLAI 2:442902ec3aa1 212 if(action_type == 0 || action_type == 1 || action_type == 2 || action_type == 3 || action_type == 4)
TonyYI 0:3417ca0a36c0 213 {
TonyYI 0:3417ca0a36c0 214 state_IntelToMbed++;
TonyYI 3:4306d042af6f 215 forward_msg_buffer[1] = _x;
TonyYI 0:3417ca0a36c0 216 }
TonyYI 0:3417ca0a36c0 217 else
TonyYI 0:3417ca0a36c0 218 {
TonyYI 0:3417ca0a36c0 219 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 220 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 221 {
TonyYI 0:3417ca0a36c0 222 _DEBUG->printf("Communication::parseMessage(). ERROR when checking ACTION_TYPE: %x.\r\n", action_type);
TonyYI 0:3417ca0a36c0 223 }
TonyYI 0:3417ca0a36c0 224 }
TonyYI 0:3417ca0a36c0 225 break;
TonyYI 0:3417ca0a36c0 226 }
TonyYI 0:3417ca0a36c0 227
TonyYI 0:3417ca0a36c0 228 case 2: //move_dis upper 4 bits
TonyYI 0:3417ca0a36c0 229 {
TonyYI 0:3417ca0a36c0 230 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 231 {
TonyYI 0:3417ca0a36c0 232 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIS upper 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 233 }
TonyYI 0:3417ca0a36c0 234 check_sum += _x;
TonyYI 0:3417ca0a36c0 235 move_dis = _x << 8;
TonyYI 0:3417ca0a36c0 236 state_IntelToMbed++;
TonyYI 3:4306d042af6f 237 forward_msg_buffer[2] = _x;
TonyYI 0:3417ca0a36c0 238 break;
TonyYI 0:3417ca0a36c0 239 }
TonyYI 0:3417ca0a36c0 240
TonyYI 0:3417ca0a36c0 241 case 3: //move_dis lower 4 bits
TonyYI 0:3417ca0a36c0 242 {
TonyYI 0:3417ca0a36c0 243 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 244 {
TonyYI 0:3417ca0a36c0 245 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIS lower 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 246 }
TonyYI 0:3417ca0a36c0 247 check_sum += _x;
TonyYI 0:3417ca0a36c0 248 move_dis |= _x;
TonyYI 0:3417ca0a36c0 249 state_IntelToMbed++;
TonyYI 3:4306d042af6f 250 forward_msg_buffer[3] = _x;
TonyYI 0:3417ca0a36c0 251 break;
TonyYI 0:3417ca0a36c0 252 }
TonyYI 0:3417ca0a36c0 253
TonyYI 0:3417ca0a36c0 254 case 4: //move_dir
TonyYI 0:3417ca0a36c0 255 {
TonyYI 0:3417ca0a36c0 256 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 257 {
TonyYI 0:3417ca0a36c0 258 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIR...\r\n");
TonyYI 0:3417ca0a36c0 259 }
TonyYI 0:3417ca0a36c0 260 check_sum += _x;
TonyYI 0:3417ca0a36c0 261 move_dir = _x;
terryLAI 2:442902ec3aa1 262 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 263 {
TonyYI 0:3417ca0a36c0 264 state_IntelToMbed++;
TonyYI 3:4306d042af6f 265 forward_msg_buffer[4] = _x;
TonyYI 0:3417ca0a36c0 266 }
TonyYI 0:3417ca0a36c0 267 else
TonyYI 0:3417ca0a36c0 268 {
TonyYI 0:3417ca0a36c0 269 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 270 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 271 {
TonyYI 0:3417ca0a36c0 272 _DEBUG->printf("Communication::parseMessage(). ERROR when checking MOVE_DIR: %x.\r\n", move_dir);
TonyYI 0:3417ca0a36c0 273 }
TonyYI 0:3417ca0a36c0 274 }
TonyYI 0:3417ca0a36c0 275 break;
TonyYI 0:3417ca0a36c0 276 }
TonyYI 0:3417ca0a36c0 277
TonyYI 0:3417ca0a36c0 278 case 5: //rotate_dis upper 4 bits
TonyYI 0:3417ca0a36c0 279 {
TonyYI 0:3417ca0a36c0 280 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 281 {
TonyYI 0:3417ca0a36c0 282 _DEBUG->printf("Communication::parseMessage(). Checking ROTATE_DIS upper 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 283 }
TonyYI 0:3417ca0a36c0 284 check_sum += _x;
TonyYI 0:3417ca0a36c0 285 rotate_dis = _x << 8;
TonyYI 0:3417ca0a36c0 286 state_IntelToMbed++;
TonyYI 3:4306d042af6f 287 forward_msg_buffer[5] = _x;
TonyYI 0:3417ca0a36c0 288 break;
TonyYI 0:3417ca0a36c0 289 }
TonyYI 0:3417ca0a36c0 290
TonyYI 0:3417ca0a36c0 291 case 6: //rotate_dis lower 4 bits
TonyYI 0:3417ca0a36c0 292 {
TonyYI 0:3417ca0a36c0 293 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 294 {
TonyYI 0:3417ca0a36c0 295 _DEBUG->printf("Communication::parseMessage(). Checking ROTATE_DIS lower 4 bits...\r\n");
TonyYI 0:3417ca0a36c0 296 }
TonyYI 0:3417ca0a36c0 297 check_sum += _x;
TonyYI 0:3417ca0a36c0 298 rotate_dis |= _x;
TonyYI 0:3417ca0a36c0 299 state_IntelToMbed++;
TonyYI 3:4306d042af6f 300 forward_msg_buffer[6] = _x;
TonyYI 0:3417ca0a36c0 301 break;
TonyYI 0:3417ca0a36c0 302 }
TonyYI 0:3417ca0a36c0 303
TonyYI 0:3417ca0a36c0 304 case 7: //rotate_dir
TonyYI 0:3417ca0a36c0 305 {
TonyYI 0:3417ca0a36c0 306 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 307 {
TonyYI 0:3417ca0a36c0 308 _DEBUG->printf("Communication::parseMessage(). Checking MOVE_DIR...\r\n");
TonyYI 0:3417ca0a36c0 309 }
TonyYI 0:3417ca0a36c0 310 check_sum += _x;
TonyYI 0:3417ca0a36c0 311 rotate_dir = _x;
terryLAI 2:442902ec3aa1 312 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 313 {
TonyYI 0:3417ca0a36c0 314 state_IntelToMbed++;
TonyYI 3:4306d042af6f 315 forward_msg_buffer[7] = _x;
TonyYI 0:3417ca0a36c0 316 }
TonyYI 0:3417ca0a36c0 317 else
TonyYI 0:3417ca0a36c0 318 {
TonyYI 0:3417ca0a36c0 319 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 320 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 321 {
TonyYI 0:3417ca0a36c0 322 _DEBUG->printf("Communication::parseMessage(). ERROR when checking ROTATE_DIR: %x.\r\n", rotate_dir);
TonyYI 0:3417ca0a36c0 323 }
TonyYI 0:3417ca0a36c0 324 }
TonyYI 0:3417ca0a36c0 325 break;
TonyYI 0:3417ca0a36c0 326 }
TonyYI 0:3417ca0a36c0 327
TonyYI 0:3417ca0a36c0 328 case 8: //check_sum
TonyYI 0:3417ca0a36c0 329 {
TonyYI 0:3417ca0a36c0 330 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 331 {
TonyYI 0:3417ca0a36c0 332 _DEBUG->printf("Communication::parseMessage(). Checking CHECK_SUM...\r\n");
TonyYI 0:3417ca0a36c0 333 }
TonyYI 0:3417ca0a36c0 334 if(check_sum == _x)
TonyYI 0:3417ca0a36c0 335 {
TonyYI 3:4306d042af6f 336 forward_msg_buffer[8] = _x;
TonyYI 0:3417ca0a36c0 337 switch(action_type)
TonyYI 0:3417ca0a36c0 338 {
TonyYI 0:3417ca0a36c0 339 case 0: //car movement
TonyYI 0:3417ca0a36c0 340 info_ok_IntelToMbed = 1;
TonyYI 0:3417ca0a36c0 341 break;
TonyYI 0:3417ca0a36c0 342
TonyYI 0:3417ca0a36c0 343 case 1: //lifter
TonyYI 0:3417ca0a36c0 344 info_ok_IntelToMbed = 2;
TonyYI 0:3417ca0a36c0 345 break;
TonyYI 0:3417ca0a36c0 346
TonyYI 0:3417ca0a36c0 347 case 2: //camera platform
TonyYI 0:3417ca0a36c0 348 info_ok_IntelToMbed = 3;
TonyYI 0:3417ca0a36c0 349 break;
TonyYI 0:3417ca0a36c0 350
TonyYI 0:3417ca0a36c0 351 case 3: //compass
TonyYI 0:3417ca0a36c0 352 info_ok_IntelToMbed = 4;
TonyYI 0:3417ca0a36c0 353 break;
terryLAI 2:442902ec3aa1 354
terryLAI 2:442902ec3aa1 355 case 4: //buzzer
terryLAI 2:442902ec3aa1 356 info_ok_IntelToMbed = 5;
terryLAI 2:442902ec3aa1 357 break;
TonyYI 0:3417ca0a36c0 358
TonyYI 0:3417ca0a36c0 359 default:
TonyYI 0:3417ca0a36c0 360 info_ok_IntelToMbed = 0; //not ok
TonyYI 0:3417ca0a36c0 361 break;
TonyYI 0:3417ca0a36c0 362 }
TonyYI 0:3417ca0a36c0 363 }
TonyYI 0:3417ca0a36c0 364 else
TonyYI 0:3417ca0a36c0 365 {
TonyYI 0:3417ca0a36c0 366 if(DEBUG_ON)
TonyYI 0:3417ca0a36c0 367 {
TonyYI 0:3417ca0a36c0 368 _DEBUG->printf("Communication::parseMessage(). ERROR when checking CHECK_SUM: %x.\r\n", check_sum);
TonyYI 0:3417ca0a36c0 369 }
TonyYI 0:3417ca0a36c0 370 }
TonyYI 0:3417ca0a36c0 371 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 372 break;
TonyYI 0:3417ca0a36c0 373 }
TonyYI 0:3417ca0a36c0 374
TonyYI 0:3417ca0a36c0 375 default:
TonyYI 0:3417ca0a36c0 376 {
TonyYI 0:3417ca0a36c0 377 state_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 378 break;
TonyYI 0:3417ca0a36c0 379 }
TonyYI 0:3417ca0a36c0 380 }
TonyYI 0:3417ca0a36c0 381 }
TonyYI 0:3417ca0a36c0 382 }
TonyYI 0:3417ca0a36c0 383
TonyYI 0:3417ca0a36c0 384 void Communication::forwardMessage()
TonyYI 0:3417ca0a36c0 385 {
TonyYI 0:3417ca0a36c0 386 //message structure is defined in source/motion_platform/intel_board/lib/message.h
TonyYI 3:4306d042af6f 387 putByte(forward_msg_buffer[0], 2); //starter, 2 means MbedToArduino
TonyYI 3:4306d042af6f 388 putByte(forward_msg_buffer[1], 2); //action_type
TonyYI 3:4306d042af6f 389 putByte(forward_msg_buffer[2], 2); //move_dis
TonyYI 3:4306d042af6f 390 putByte(forward_msg_buffer[3], 2);
TonyYI 3:4306d042af6f 391 putByte(forward_msg_buffer[4], 2); //move_dir
TonyYI 3:4306d042af6f 392 putByte(forward_msg_buffer[5], 2); //rotate_dis
TonyYI 3:4306d042af6f 393 putByte(forward_msg_buffer[6], 2);
TonyYI 3:4306d042af6f 394 putByte(forward_msg_buffer[7], 2); //rotate_dir
TonyYI 3:4306d042af6f 395 putByte(forward_msg_buffer[8], 2); //checksum
TonyYI 0:3417ca0a36c0 396 }
TonyYI 0:3417ca0a36c0 397
TonyYI 0:3417ca0a36c0 398 void Communication::ACK(Lifter* lifter, Camera_platform* camera_platform)
TonyYI 0:3417ca0a36c0 399 {
TonyYI 0:3417ca0a36c0 400 if(action_type == 0) //car movement
TonyYI 0:3417ca0a36c0 401 {
TonyYI 3:4306d042af6f 402 for(int i = 0; i < 9; i++)
TonyYI 3:4306d042af6f 403 {
TonyYI 3:4306d042af6f 404 uint8_t _y = getByte(1);
TonyYI 3:4306d042af6f 405 printf("Communication::ACK(). Get byte: %x\r\n", _y);
TonyYI 3:4306d042af6f 406 }
TonyYI 3:4306d042af6f 407
TonyYI 0:3417ca0a36c0 408 while(info_ok_MbedToArduino != 1)
TonyYI 0:3417ca0a36c0 409 {
TonyYI 0:3417ca0a36c0 410 if(in_MbedToArduino != out_MbedToArduino)
TonyYI 0:3417ca0a36c0 411 {
TonyYI 0:3417ca0a36c0 412 uint8_t _x = getByte(1);
TonyYI 0:3417ca0a36c0 413 switch(state_MbedToArduino)
TonyYI 0:3417ca0a36c0 414 {
TonyYI 0:3417ca0a36c0 415 case 0: //checking starter
TonyYI 0:3417ca0a36c0 416 {
TonyYI 3:4306d042af6f 417 if(DEBUG_ON)
TonyYI 3:4306d042af6f 418 {
TonyYI 3:4306d042af6f 419 _DEBUG->printf("Communication::ACK(). Checking SARTER...\r\n");
TonyYI 3:4306d042af6f 420 }
TonyYI 3:4306d042af6f 421
TonyYI 0:3417ca0a36c0 422 if(_x == STARTER)
TonyYI 0:3417ca0a36c0 423 {
TonyYI 0:3417ca0a36c0 424 state_MbedToArduino++;
TonyYI 0:3417ca0a36c0 425 }
TonyYI 0:3417ca0a36c0 426 else
TonyYI 0:3417ca0a36c0 427 {
TonyYI 3:4306d042af6f 428 if(DEBUG_ON)
TonyYI 3:4306d042af6f 429 {
TonyYI 3:4306d042af6f 430 _DEBUG->printf("Communication::ACK(). ERROR when checking SARTER: %x\r\n", _x);
TonyYI 3:4306d042af6f 431 }
TonyYI 0:3417ca0a36c0 432 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 433 }
TonyYI 0:3417ca0a36c0 434 break;
TonyYI 0:3417ca0a36c0 435 }
TonyYI 0:3417ca0a36c0 436
TonyYI 0:3417ca0a36c0 437 case 1: //checking 'O'
TonyYI 0:3417ca0a36c0 438 {
TonyYI 3:4306d042af6f 439 if(DEBUG_ON)
TonyYI 3:4306d042af6f 440 {
TonyYI 3:4306d042af6f 441 _DEBUG->printf("Communication::ACK(). Checking O...\r\n");
TonyYI 3:4306d042af6f 442 }
TonyYI 3:4306d042af6f 443
TonyYI 0:3417ca0a36c0 444 if(_x == 0x4f) //O
TonyYI 0:3417ca0a36c0 445 {
TonyYI 0:3417ca0a36c0 446 state_MbedToArduino++;
TonyYI 0:3417ca0a36c0 447 }
TonyYI 0:3417ca0a36c0 448 else
TonyYI 0:3417ca0a36c0 449 {
TonyYI 3:4306d042af6f 450 if(DEBUG_ON)
TonyYI 3:4306d042af6f 451 {
TonyYI 3:4306d042af6f 452 _DEBUG->printf("Communication::ACK(). ERROR when checking O: %x\r\n", _x);
TonyYI 3:4306d042af6f 453 }
TonyYI 0:3417ca0a36c0 454 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 455 }
TonyYI 0:3417ca0a36c0 456 break;
TonyYI 0:3417ca0a36c0 457 }
TonyYI 0:3417ca0a36c0 458
TonyYI 0:3417ca0a36c0 459 case 2: //checking 'K'
TonyYI 0:3417ca0a36c0 460 {
TonyYI 3:4306d042af6f 461 if(DEBUG_ON)
TonyYI 3:4306d042af6f 462 {
TonyYI 3:4306d042af6f 463 _DEBUG->printf("Communication::ACK(). Checking K...\r\n");
TonyYI 3:4306d042af6f 464 }
TonyYI 3:4306d042af6f 465
TonyYI 0:3417ca0a36c0 466 if(_x == 0x4b) //K
TonyYI 0:3417ca0a36c0 467 {
TonyYI 0:3417ca0a36c0 468 state_MbedToArduino++;
TonyYI 0:3417ca0a36c0 469 }
TonyYI 0:3417ca0a36c0 470 else
TonyYI 0:3417ca0a36c0 471 {
TonyYI 3:4306d042af6f 472 if(DEBUG_ON)
TonyYI 3:4306d042af6f 473 {
TonyYI 3:4306d042af6f 474 _DEBUG->printf("Communication::ACK(). ERROR when checking K: %x\r\n", _x);
TonyYI 3:4306d042af6f 475 }
TonyYI 0:3417ca0a36c0 476 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 477 }
TonyYI 0:3417ca0a36c0 478 break;
TonyYI 0:3417ca0a36c0 479 }
TonyYI 0:3417ca0a36c0 480
TonyYI 0:3417ca0a36c0 481 case 3: //checking check_sum_MbedToArduino
TonyYI 0:3417ca0a36c0 482 {
TonyYI 3:4306d042af6f 483 if(DEBUG_ON)
TonyYI 3:4306d042af6f 484 {
TonyYI 3:4306d042af6f 485 _DEBUG->printf("Communication::ACK(). Checking CHECK_SUM...\r\n");
TonyYI 3:4306d042af6f 486 }
TonyYI 3:4306d042af6f 487
TonyYI 0:3417ca0a36c0 488 if(_x == 0x9a) //checksum
TonyYI 0:3417ca0a36c0 489 {
TonyYI 0:3417ca0a36c0 490 info_ok_MbedToArduino = 1;
TonyYI 0:3417ca0a36c0 491 }
TonyYI 3:4306d042af6f 492 else
TonyYI 3:4306d042af6f 493 {
TonyYI 3:4306d042af6f 494 if(DEBUG_ON)
TonyYI 3:4306d042af6f 495 {
TonyYI 3:4306d042af6f 496 _DEBUG->printf("Communication::ACK(). ERROR when checking CHECK_SUM: %x\r\n", _x);
TonyYI 3:4306d042af6f 497 }
TonyYI 3:4306d042af6f 498 }
TonyYI 0:3417ca0a36c0 499
TonyYI 0:3417ca0a36c0 500 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 501 break;
TonyYI 0:3417ca0a36c0 502 }
TonyYI 0:3417ca0a36c0 503
TonyYI 0:3417ca0a36c0 504 default:
TonyYI 0:3417ca0a36c0 505 {
TonyYI 0:3417ca0a36c0 506 state_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 507 info_ok_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 508 break;
TonyYI 0:3417ca0a36c0 509 }
TonyYI 0:3417ca0a36c0 510 }
TonyYI 0:3417ca0a36c0 511 }
TonyYI 0:3417ca0a36c0 512 }
TonyYI 0:3417ca0a36c0 513 }
TonyYI 0:3417ca0a36c0 514 else if(action_type == 1) //lifter
TonyYI 0:3417ca0a36c0 515 {
TonyYI 0:3417ca0a36c0 516 uint32_t pulseCountOld = 0;
TonyYI 0:3417ca0a36c0 517 uint32_t pulseCountNew = 0;
TonyYI 0:3417ca0a36c0 518 while(!lifter->isStopped())
TonyYI 0:3417ca0a36c0 519 {
TonyYI 0:3417ca0a36c0 520 pulseCountOld = lifter->pulseCount;
TonyYI 0:3417ca0a36c0 521 wait_ms(50);
TonyYI 0:3417ca0a36c0 522 pulseCountNew = lifter->pulseCount;
TonyYI 0:3417ca0a36c0 523 if(pulseCountOld == pulseCountNew)
TonyYI 0:3417ca0a36c0 524 {
TonyYI 0:3417ca0a36c0 525 break;
TonyYI 0:3417ca0a36c0 526 }
TonyYI 0:3417ca0a36c0 527 }
TonyYI 0:3417ca0a36c0 528 }
TonyYI 0:3417ca0a36c0 529 else if(action_type == 3)
TonyYI 0:3417ca0a36c0 530 {
terryLAI 1:cbec1283a16a 531
terryLAI 5:098e6a44bd94 532 printf("Entering Compass::read()...\r\n");
terryLAI 5:098e6a44bd94 533
terryLAI 5:098e6a44bd94 534 stop();
terryLAI 5:098e6a44bd94 535 clearBuffer();
terryLAI 5:098e6a44bd94 536
terryLAI 5:098e6a44bd94 537
terryLAI 5:098e6a44bd94 538 printf("0\r\n");
terryLAI 5:098e6a44bd94 539 resume();
terryLAI 5:098e6a44bd94 540 if(buffer_compass[_out++] == ACK_RESUME_MSB && buffer_compass[_out++] == ACK_RESUME_LSB)
terryLAI 5:098e6a44bd94 541 {
terryLAI 5:098e6a44bd94 542 }
terryLAI 5:098e6a44bd94 543 else
terryLAI 5:098e6a44bd94 544 {
terryLAI 5:098e6a44bd94 545 clearBuffer();
terryLAI 5:098e6a44bd94 546 }
terryLAI 5:098e6a44bd94 547
terryLAI 5:098e6a44bd94 548 printf("1\r\n");
terryLAI 5:098e6a44bd94 549 run();
terryLAI 5:098e6a44bd94 550
terryLAI 5:098e6a44bd94 551 if(buffer_compass[_out++] == ACK_RUN_MSB && buffer_compass[_out++] == ACK_RUN_LSB)
terryLAI 5:098e6a44bd94 552 {
terryLAI 5:098e6a44bd94 553 }
terryLAI 5:098e6a44bd94 554 else
terryLAI 5:098e6a44bd94 555 {
terryLAI 5:098e6a44bd94 556 clearBuffer();
terryLAI 5:098e6a44bd94 557 }
terryLAI 5:098e6a44bd94 558
terryLAI 5:098e6a44bd94 559 printf("2\r\n");
terryLAI 5:098e6a44bd94 560 _MSB = buffer_compass[_out++];
terryLAI 5:098e6a44bd94 561 _LSB = buffer_compass[_out++];
terryLAI 5:098e6a44bd94 562
terryLAI 5:098e6a44bd94 563 if( 0xa0==( (_MSB) & 0xe0))
terryLAI 5:098e6a44bd94 564 {
terryLAI 5:098e6a44bd94 565 printf("match !!!!!\r\n");
terryLAI 5:098e6a44bd94 566 uint8_t digits= _LSB & 0x0f;
terryLAI 5:098e6a44bd94 567 uint8_t tens= (_LSB>>4) & 0x0f;
terryLAI 5:098e6a44bd94 568 uint8_t hundreds= _MSB & 0x07;
terryLAI 5:098e6a44bd94 569 campass_degree=100*hundreds+10*tens+digits;
terryLAI 5:098e6a44bd94 570 }
terryLAI 5:098e6a44bd94 571
terryLAI 5:098e6a44bd94 572 else
terryLAI 5:098e6a44bd94 573 {
terryLAI 5:098e6a44bd94 574 printf("Error data !!!!!!!!!!!\r\n");
terryLAI 5:098e6a44bd94 575 campass_degree=0;
terryLAI 5:098e6a44bd94 576 }
TonyYI 0:3417ca0a36c0 577
terryLAI 5:098e6a44bd94 578 printf("_MSB: %x, _LSB: %x\r\n", _MSB, _LSB);
terryLAI 5:098e6a44bd94 579
terryLAI 5:098e6a44bd94 580 printf("3\r\n");
terryLAI 5:098e6a44bd94 581 stop();
terryLAI 5:098e6a44bd94 582
terryLAI 5:098e6a44bd94 583 uint16_t index1=_in - 1;
terryLAI 5:098e6a44bd94 584 uint16_t index2=_in;
terryLAI 5:098e6a44bd94 585
terryLAI 5:098e6a44bd94 586 if(_in == 0)
terryLAI 5:098e6a44bd94 587 {
terryLAI 5:098e6a44bd94 588 index1=BUFFER_SIZE - 1;
terryLAI 5:098e6a44bd94 589 index2=0;
terryLAI 5:098e6a44bd94 590 }
terryLAI 5:098e6a44bd94 591 else
terryLAI 5:098e6a44bd94 592 {
terryLAI 5:098e6a44bd94 593 index1=_in - 1;
terryLAI 5:098e6a44bd94 594 index2=_in;
terryLAI 5:098e6a44bd94 595 }
terryLAI 5:098e6a44bd94 596
terryLAI 5:098e6a44bd94 597 if(buffer_compass[index1] == ACK_STOP_MSB && buffer_compass[index2] == ACK_STOP_LSB)
terryLAI 5:098e6a44bd94 598 {
terryLAI 5:098e6a44bd94 599 }
terryLAI 5:098e6a44bd94 600 else
terryLAI 5:098e6a44bd94 601 {
terryLAI 5:098e6a44bd94 602 }
terryLAI 5:098e6a44bd94 603 _out = _in;
terryLAI 5:098e6a44bd94 604 clearBuffer();
terryLAI 5:098e6a44bd94 605
terryLAI 5:098e6a44bd94 606 printf("Exiting Compass::read()...\r\n");
terryLAI 5:098e6a44bd94 607
terryLAI 5:098e6a44bd94 608 campass_degree+=90;
terryLAI 5:098e6a44bd94 609
terryLAI 5:098e6a44bd94 610 if(campass_degree>=360)
terryLAI 5:098e6a44bd94 611 campass_degree-=360;
terryLAI 5:098e6a44bd94 612
terryLAI 5:098e6a44bd94 613 printf("campass_degree: %d\r\n",campass_degree);
terryLAI 5:098e6a44bd94 614 printf("----------------------\r\n");
terryLAI 5:098e6a44bd94 615
terryLAI 5:098e6a44bd94 616
terryLAI 5:098e6a44bd94 617
TonyYI 0:3417ca0a36c0 618 uint8_t temp1,temp2;
TonyYI 0:3417ca0a36c0 619 temp1 = campass_degree;
TonyYI 0:3417ca0a36c0 620 temp2 = campass_degree>>8;
TonyYI 0:3417ca0a36c0 621 putByte(COMPASS_STARTER ,1); //1 means IntelToMbed
TonyYI 0:3417ca0a36c0 622 putByte(temp1 ,1); //O
TonyYI 0:3417ca0a36c0 623 putByte(temp2 ,1); //K
TonyYI 0:3417ca0a36c0 624 putByte(0x9a ,1); //check_sum = 0xaf + 0x4b = 0x9a
TonyYI 0:3417ca0a36c0 625
TonyYI 0:3417ca0a36c0 626 return;
TonyYI 0:3417ca0a36c0 627 }
terryLAI 2:442902ec3aa1 628 else if(action_type == 4)
terryLAI 2:442902ec3aa1 629 {
terryLAI 2:442902ec3aa1 630
terryLAI 2:442902ec3aa1 631 putByte(BUZZER_STARTER ,1); //1 means IntelToMbed
terryLAI 2:442902ec3aa1 632 putByte(0,1); //O
terryLAI 2:442902ec3aa1 633 putByte(0,1); //K
terryLAI 2:442902ec3aa1 634 putByte(0,1); //check_sum = 0xaf + 0x4b = 0x9a
terryLAI 2:442902ec3aa1 635
terryLAI 2:442902ec3aa1 636 return;
terryLAI 2:442902ec3aa1 637 }
TonyYI 0:3417ca0a36c0 638
TonyYI 0:3417ca0a36c0 639 putByte(STARTER ,1); //1 means IntelToMbed
TonyYI 0:3417ca0a36c0 640 putByte(0x4f ,1); //O
TonyYI 0:3417ca0a36c0 641 putByte(0x4b ,1); //K
TonyYI 0:3417ca0a36c0 642 putByte(0x9a ,1); //check_sum = 0xaf + 0x4b = 0x9a
TonyYI 0:3417ca0a36c0 643 }
TonyYI 0:3417ca0a36c0 644
TonyYI 0:3417ca0a36c0 645 uint8_t Communication::getInfoOK(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 646 {
TonyYI 0:3417ca0a36c0 647 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 648 {
TonyYI 0:3417ca0a36c0 649 return info_ok_IntelToMbed;
TonyYI 0:3417ca0a36c0 650 }
TonyYI 0:3417ca0a36c0 651 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 652 {
TonyYI 0:3417ca0a36c0 653 return info_ok_MbedToArduino;
TonyYI 0:3417ca0a36c0 654 }
TonyYI 0:3417ca0a36c0 655 return 0; //error
TonyYI 0:3417ca0a36c0 656 }
TonyYI 0:3417ca0a36c0 657
TonyYI 0:3417ca0a36c0 658 void Communication::resetInfoOK(uint8_t communication_type)
TonyYI 0:3417ca0a36c0 659 {
TonyYI 0:3417ca0a36c0 660 if(communication_type == 0)
TonyYI 0:3417ca0a36c0 661 {
TonyYI 0:3417ca0a36c0 662 info_ok_IntelToMbed = 0;
TonyYI 0:3417ca0a36c0 663 }
TonyYI 0:3417ca0a36c0 664 else if(communication_type == 1)
TonyYI 0:3417ca0a36c0 665 {
TonyYI 0:3417ca0a36c0 666 info_ok_MbedToArduino = 0;
TonyYI 0:3417ca0a36c0 667 }
TonyYI 0:3417ca0a36c0 668 }
TonyYI 0:3417ca0a36c0 669
TonyYI 0:3417ca0a36c0 670 uint16_t Communication::getMoveDis()
TonyYI 0:3417ca0a36c0 671 {
TonyYI 0:3417ca0a36c0 672 return move_dis;
TonyYI 0:3417ca0a36c0 673 }
TonyYI 0:3417ca0a36c0 674
TonyYI 0:3417ca0a36c0 675 uint16_t Communication::getRotateDis()
TonyYI 0:3417ca0a36c0 676 {
TonyYI 0:3417ca0a36c0 677 return rotate_dis;
TonyYI 0:3417ca0a36c0 678 }
TonyYI 0:3417ca0a36c0 679
TonyYI 0:3417ca0a36c0 680 uint8_t Communication::getMoveDir()
TonyYI 0:3417ca0a36c0 681 {
TonyYI 0:3417ca0a36c0 682 return move_dir;
TonyYI 0:3417ca0a36c0 683 }
TonyYI 0:3417ca0a36c0 684
TonyYI 0:3417ca0a36c0 685 uint8_t Communication::getRotateDir()
TonyYI 0:3417ca0a36c0 686 {
TonyYI 0:3417ca0a36c0 687 return rotate_dir;
terryLAI 5:098e6a44bd94 688 }
terryLAI 5:098e6a44bd94 689
terryLAI 5:098e6a44bd94 690
terryLAI 5:098e6a44bd94 691
terryLAI 5:098e6a44bd94 692 void Communication::run()
terryLAI 5:098e6a44bd94 693 {
terryLAI 5:098e6a44bd94 694 printf("Entering Compass::run()...\r\n");
terryLAI 5:098e6a44bd94 695 write2Bytes(RUN_MSB, RUN_LSB);
terryLAI 5:098e6a44bd94 696 }
terryLAI 5:098e6a44bd94 697
terryLAI 5:098e6a44bd94 698 void Communication::stop()
terryLAI 5:098e6a44bd94 699 {
terryLAI 5:098e6a44bd94 700 printf("Entering Compass::stop()...\r\n");
terryLAI 5:098e6a44bd94 701 write2Bytes(STOP_MSB, STOP_LSB);
terryLAI 5:098e6a44bd94 702 }
terryLAI 5:098e6a44bd94 703
terryLAI 5:098e6a44bd94 704 void Communication::resume()
terryLAI 5:098e6a44bd94 705 {
terryLAI 5:098e6a44bd94 706 printf("Entering Compass::resume()...\r\n");
terryLAI 5:098e6a44bd94 707 write2Bytes(RESUME_MSB, RESUME_LSB);
terryLAI 5:098e6a44bd94 708 }
terryLAI 5:098e6a44bd94 709
terryLAI 5:098e6a44bd94 710 void Communication::reset()
terryLAI 5:098e6a44bd94 711 {
terryLAI 5:098e6a44bd94 712 printf("Entering Compass::reset()...\r\n");
terryLAI 5:098e6a44bd94 713 write2Bytes(RST_MSB, RST_LSB);
terryLAI 5:098e6a44bd94 714 }
terryLAI 5:098e6a44bd94 715
terryLAI 5:098e6a44bd94 716 void Communication::write2Bytes(char msb, char lsb)
terryLAI 5:098e6a44bd94 717 {
terryLAI 5:098e6a44bd94 718 printf("before writable...\r\n");
terryLAI 5:098e6a44bd94 719 if(CompassData->writeable())
terryLAI 5:098e6a44bd94 720 {
terryLAI 5:098e6a44bd94 721 CompassData->putc(msb);
terryLAI 5:098e6a44bd94 722 }
terryLAI 5:098e6a44bd94 723 printf("_MSB = %x wrote...\r\n", msb);
terryLAI 5:098e6a44bd94 724 if(CompassData->writeable())
terryLAI 5:098e6a44bd94 725 {
terryLAI 5:098e6a44bd94 726 CompassData->putc(lsb);
terryLAI 5:098e6a44bd94 727 }
terryLAI 5:098e6a44bd94 728 printf("_LSB = %x wrote...\r\n", lsb);
terryLAI 5:098e6a44bd94 729 wait(0.1);
terryLAI 5:098e6a44bd94 730 }
terryLAI 5:098e6a44bd94 731
terryLAI 5:098e6a44bd94 732 void Communication::clearBuffer()
terryLAI 5:098e6a44bd94 733 {
terryLAI 5:098e6a44bd94 734 _in = _out = 0;
TonyYI 0:3417ca0a36c0 735 }