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@6:7ab57721c0fb, 2014-07-07 (annotated)

Committer:
terryLAI
Date:
Mon Jul 07 06:10:11 2014 +0000
Revision:
6:7ab57721c0fb
Parent:
5:098e6a44bd94 
wait pwm;

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