ใช้กับขาหมา
Dependents: BEAR_Protocol BEAR_Reciever BETAGO
Fork of Communication_Robot by
communication.cpp@2:56c1de1612dd, 2015-03-01 (annotated)
- Committer:
- soulx
- Date:
- Sun Mar 01 15:40:51 2015 +0000
- Revision:
- 2:56c1de1612dd
- Parent:
- 1:1f6864549b92
- Child:
- 3:2f334f0cee75
edit; -error serialCom because function outside class; ; after test 1; -send and receive =PASS; ; //in must function check data in buffer rx
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
soulx | 0:0c88691e3904 | 1 | #include "mbed.h" |
soulx | 0:0c88691e3904 | 2 | #include "communication.h" |
soulx | 2:56c1de1612dd | 3 | //#include "protocol.h" |
soulx | 2:56c1de1612dd | 4 | //#include "Utilities.h" |
soulx | 2:56c1de1612dd | 5 | //#include "iSerial.h" |
soulx | 2:56c1de1612dd | 6 | |
soulx | 2:56c1de1612dd | 7 | |
soulx | 2:56c1de1612dd | 8 | uint8_t COMMUNICATION::sendCommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet) |
soulx | 2:56c1de1612dd | 9 | { |
soulx | 2:56c1de1612dd | 10 | uint8_t currentParameter = 0; |
soulx | 2:56c1de1612dd | 11 | bool isWholePacket = false; |
soulx | 2:56c1de1612dd | 12 | uint8_t encoderState = WAIT_ON_HEADER_0; |
soulx | 2:56c1de1612dd | 13 | |
soulx | 2:56c1de1612dd | 14 | packet->checkSum = Utilities::GetCheckSum((uint8_t*)&(packet->robotId), packet->length + 1); |
soulx | 2:56c1de1612dd | 15 | |
soulx | 2:56c1de1612dd | 16 | Timer timer; |
soulx | 2:56c1de1612dd | 17 | timer.start(); |
soulx | 2:56c1de1612dd | 18 | |
soulx | 2:56c1de1612dd | 19 | while((timer.read_ms() < ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket)) { |
soulx | 2:56c1de1612dd | 20 | |
soulx | 2:56c1de1612dd | 21 | if( serialCom->writeable()) { |
soulx | 2:56c1de1612dd | 22 | |
soulx | 2:56c1de1612dd | 23 | switch(encoderState) { |
soulx | 2:56c1de1612dd | 24 | case WAIT_ON_HEADER_0: { |
soulx | 2:56c1de1612dd | 25 | #ifdef ANDANTE_DEBUG |
soulx | 2:56c1de1612dd | 26 | pc->printf("Write: 0x%02X ", ANDANTE_PROTOCOL_HEADER_0); |
soulx | 2:56c1de1612dd | 27 | #endif |
soulx | 2:56c1de1612dd | 28 | |
soulx | 2:56c1de1612dd | 29 | serialCom->putc(ANDANTE_PROTOCOL_HEADER_0); |
soulx | 2:56c1de1612dd | 30 | // dirc_rs485=0; |
soulx | 2:56c1de1612dd | 31 | // wait_us(DELAY_DIR); |
soulx | 2:56c1de1612dd | 32 | |
soulx | 2:56c1de1612dd | 33 | |
soulx | 2:56c1de1612dd | 34 | encoderState = WAIT_ON_HEADER_1; |
soulx | 2:56c1de1612dd | 35 | |
soulx | 2:56c1de1612dd | 36 | break; |
soulx | 2:56c1de1612dd | 37 | } |
soulx | 2:56c1de1612dd | 38 | case WAIT_ON_HEADER_1: { |
soulx | 2:56c1de1612dd | 39 | #ifdef ANDANTE_DEBUG |
soulx | 2:56c1de1612dd | 40 | pc->printf("0x%02X ", ANDANTE_PROTOCOL_HEADER_1); |
soulx | 2:56c1de1612dd | 41 | #endif |
soulx | 2:56c1de1612dd | 42 | |
soulx | 2:56c1de1612dd | 43 | |
soulx | 2:56c1de1612dd | 44 | |
soulx | 2:56c1de1612dd | 45 | serialCom->putc(ANDANTE_PROTOCOL_HEADER_1); |
soulx | 2:56c1de1612dd | 46 | // dirc_rs485=0; |
soulx | 2:56c1de1612dd | 47 | // wait_us(DELAY_DIR); |
soulx | 2:56c1de1612dd | 48 | |
soulx | 2:56c1de1612dd | 49 | encoderState = WAIT_ON_ROBOT_ID; |
soulx | 2:56c1de1612dd | 50 | |
soulx | 2:56c1de1612dd | 51 | break; |
soulx | 2:56c1de1612dd | 52 | } |
soulx | 2:56c1de1612dd | 53 | case WAIT_ON_ROBOT_ID: { |
soulx | 2:56c1de1612dd | 54 | #ifdef ANDANTE_DEBUG |
soulx | 2:56c1de1612dd | 55 | pc->printf("0x%02X ", packet->robotId); |
soulx | 2:56c1de1612dd | 56 | #endif |
soulx | 2:56c1de1612dd | 57 | |
soulx | 2:56c1de1612dd | 58 | |
soulx | 2:56c1de1612dd | 59 | serialCom->putc(packet->robotId); |
soulx | 2:56c1de1612dd | 60 | |
soulx | 2:56c1de1612dd | 61 | |
soulx | 2:56c1de1612dd | 62 | encoderState = WAIT_ON_LENGTH; |
soulx | 2:56c1de1612dd | 63 | |
soulx | 2:56c1de1612dd | 64 | break; |
soulx | 2:56c1de1612dd | 65 | } |
soulx | 2:56c1de1612dd | 66 | case WAIT_ON_LENGTH: { |
soulx | 2:56c1de1612dd | 67 | #ifdef ANDANTE_DEBUG |
soulx | 2:56c1de1612dd | 68 | pc->printf("0x%02X ", packet->length); |
soulx | 2:56c1de1612dd | 69 | #endif |
soulx | 2:56c1de1612dd | 70 | |
soulx | 2:56c1de1612dd | 71 | serialCom->putc(packet->length); |
soulx | 2:56c1de1612dd | 72 | |
soulx | 2:56c1de1612dd | 73 | |
soulx | 2:56c1de1612dd | 74 | encoderState = WAIT_ON_INSTRUCTION_ERROR_ID; |
soulx | 2:56c1de1612dd | 75 | |
soulx | 2:56c1de1612dd | 76 | break; |
soulx | 2:56c1de1612dd | 77 | } |
soulx | 2:56c1de1612dd | 78 | case WAIT_ON_INSTRUCTION_ERROR_ID: { |
soulx | 2:56c1de1612dd | 79 | #ifdef ANDANTE_DEBUG |
soulx | 2:56c1de1612dd | 80 | pc->printf("0x%02X ", packet->instructionErrorId); |
soulx | 2:56c1de1612dd | 81 | #endif |
soulx | 2:56c1de1612dd | 82 | |
soulx | 2:56c1de1612dd | 83 | |
soulx | 2:56c1de1612dd | 84 | serialCom->putc(packet->instructionErrorId); |
soulx | 2:56c1de1612dd | 85 | |
soulx | 2:56c1de1612dd | 86 | if(packet->length > 2) |
soulx | 2:56c1de1612dd | 87 | encoderState = WAIT_ON_PARAMETER; |
soulx | 2:56c1de1612dd | 88 | else |
soulx | 2:56c1de1612dd | 89 | encoderState = WAIT_ON_CHECK_SUM; |
soulx | 2:56c1de1612dd | 90 | |
soulx | 2:56c1de1612dd | 91 | break; |
soulx | 2:56c1de1612dd | 92 | } |
soulx | 2:56c1de1612dd | 93 | case WAIT_ON_PARAMETER: { |
soulx | 2:56c1de1612dd | 94 | #ifdef ANDANTE_DEBUG |
soulx | 2:56c1de1612dd | 95 | pc->printf("0x%02X ", packet->parameter[currentParameter]); |
soulx | 2:56c1de1612dd | 96 | #endif |
soulx | 2:56c1de1612dd | 97 | |
soulx | 2:56c1de1612dd | 98 | serialCom->putc(packet->parameter[currentParameter]); |
soulx | 2:56c1de1612dd | 99 | |
soulx | 2:56c1de1612dd | 100 | if(++currentParameter == packet->length - 2) |
soulx | 2:56c1de1612dd | 101 | encoderState = WAIT_ON_CHECK_SUM; |
soulx | 2:56c1de1612dd | 102 | |
soulx | 2:56c1de1612dd | 103 | break; |
soulx | 2:56c1de1612dd | 104 | } |
soulx | 2:56c1de1612dd | 105 | case WAIT_ON_CHECK_SUM: { |
soulx | 2:56c1de1612dd | 106 | #ifdef ANDANTE_DEBUG |
soulx | 2:56c1de1612dd | 107 | pc->printf("0x%02X\r\n", packet->checkSum); |
soulx | 2:56c1de1612dd | 108 | #endif |
soulx | 2:56c1de1612dd | 109 | |
soulx | 2:56c1de1612dd | 110 | |
soulx | 2:56c1de1612dd | 111 | serialCom->putc(packet->checkSum); |
soulx | 2:56c1de1612dd | 112 | |
soulx | 2:56c1de1612dd | 113 | |
soulx | 2:56c1de1612dd | 114 | encoderState = WAIT_ON_HEADER_0; |
soulx | 2:56c1de1612dd | 115 | isWholePacket = true; |
soulx | 2:56c1de1612dd | 116 | |
soulx | 2:56c1de1612dd | 117 | break; |
soulx | 2:56c1de1612dd | 118 | } |
soulx | 2:56c1de1612dd | 119 | } |
soulx | 2:56c1de1612dd | 120 | } |
soulx | 2:56c1de1612dd | 121 | } |
soulx | 2:56c1de1612dd | 122 | |
soulx | 2:56c1de1612dd | 123 | |
soulx | 2:56c1de1612dd | 124 | #ifdef ANDANTE_DEBUG |
soulx | 2:56c1de1612dd | 125 | pc->printf("Timer: %d ms\r\n", timer.read_ms()); |
soulx | 2:56c1de1612dd | 126 | #endif |
soulx | 2:56c1de1612dd | 127 | |
soulx | 2:56c1de1612dd | 128 | timer.stop(); |
soulx | 2:56c1de1612dd | 129 | |
soulx | 2:56c1de1612dd | 130 | |
soulx | 2:56c1de1612dd | 131 | |
soulx | 2:56c1de1612dd | 132 | if(!isWholePacket) { |
soulx | 2:56c1de1612dd | 133 | #ifdef ANDANTE_DEBUG |
soulx | 2:56c1de1612dd | 134 | pc->printf("Error: Write response timeout.\r\n"); |
soulx | 2:56c1de1612dd | 135 | #endif |
soulx | 2:56c1de1612dd | 136 | |
soulx | 2:56c1de1612dd | 137 | return ANDANTE_ERRBIT_WRITE_TIMEOUT; |
soulx | 2:56c1de1612dd | 138 | } |
soulx | 2:56c1de1612dd | 139 | /* |
soulx | 2:56c1de1612dd | 140 | if( packet->servoId == ANDANTE_PROTOCOL_BROADCAST_ID || |
soulx | 2:56c1de1612dd | 141 | packet->instructionErrorId == ANDANTE_ACTION || |
soulx | 2:56c1de1612dd | 142 | packet->instructionErrorId == ANDANTE_SYNC_WRITE) |
soulx | 2:56c1de1612dd | 143 | */ |
soulx | 2:56c1de1612dd | 144 | return ANDANTE_ERRBIT_NONE; |
soulx | 2:56c1de1612dd | 145 | |
soulx | 2:56c1de1612dd | 146 | } |
soulx | 0:0c88691e3904 | 147 | |
soulx | 1:1f6864549b92 | 148 | uint8_t COMMUNICATION::receiveCommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet) |
soulx | 0:0c88691e3904 | 149 | { |
soulx | 0:0c88691e3904 | 150 | uint8_t currentParameter = 0; |
soulx | 0:0c88691e3904 | 151 | bool isWholePacket = false; |
soulx | 0:0c88691e3904 | 152 | |
soulx | 0:0c88691e3904 | 153 | currentParameter = 0; |
soulx | 0:0c88691e3904 | 154 | isWholePacket = false; |
soulx | 0:0c88691e3904 | 155 | uint8_t decoderState = WAIT_ON_HEADER_0; |
soulx | 0:0c88691e3904 | 156 | |
soulx | 0:0c88691e3904 | 157 | Timer timer; |
soulx | 0:0c88691e3904 | 158 | timer.start(); |
soulx | 0:0c88691e3904 | 159 | |
soulx | 0:0c88691e3904 | 160 | while((timer.read_ms() < ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket)) { |
soulx | 0:0c88691e3904 | 161 | if(serialCom->readable()) { |
soulx | 0:0c88691e3904 | 162 | switch(decoderState) { |
soulx | 0:0c88691e3904 | 163 | case WAIT_ON_HEADER_0: { |
soulx | 0:0c88691e3904 | 164 | |
soulx | 0:0c88691e3904 | 165 | uint8_t mx28ProtocolHeader0 = serialCom->getc(); |
soulx | 0:0c88691e3904 | 166 | |
soulx | 0:0c88691e3904 | 167 | #ifdef ANDANTE_DEBUG |
soulx | 0:0c88691e3904 | 168 | pc->printf("Read: 0x%02X ", mx28ProtocolHeader0); |
soulx | 0:0c88691e3904 | 169 | #endif |
soulx | 0:0c88691e3904 | 170 | |
soulx | 0:0c88691e3904 | 171 | decoderState = WAIT_ON_HEADER_1; |
soulx | 0:0c88691e3904 | 172 | |
soulx | 0:0c88691e3904 | 173 | break; |
soulx | 0:0c88691e3904 | 174 | } |
soulx | 0:0c88691e3904 | 175 | case WAIT_ON_HEADER_1: { |
soulx | 0:0c88691e3904 | 176 | |
soulx | 0:0c88691e3904 | 177 | uint8_t mx28ProtocolHeader1 = serialCom->getc(); |
soulx | 0:0c88691e3904 | 178 | |
soulx | 0:0c88691e3904 | 179 | #ifdef ANDANTE_DEBUG |
soulx | 0:0c88691e3904 | 180 | pc->printf("0x%02X ", mx28ProtocolHeader1); |
soulx | 0:0c88691e3904 | 181 | #endif |
soulx | 0:0c88691e3904 | 182 | |
soulx | 0:0c88691e3904 | 183 | decoderState = WAIT_ON_ROBOT_ID; |
soulx | 0:0c88691e3904 | 184 | |
soulx | 0:0c88691e3904 | 185 | break; |
soulx | 0:0c88691e3904 | 186 | } |
soulx | 0:0c88691e3904 | 187 | case WAIT_ON_ROBOT_ID: { |
soulx | 0:0c88691e3904 | 188 | |
soulx | 0:0c88691e3904 | 189 | |
soulx | 0:0c88691e3904 | 190 | packet->robotId = serialCom->getc(); |
soulx | 0:0c88691e3904 | 191 | |
soulx | 0:0c88691e3904 | 192 | #ifdef ANDANTE_DEBUG |
soulx | 2:56c1de1612dd | 193 | pc->printf("0x%02X ", packet->robotId); |
soulx | 0:0c88691e3904 | 194 | #endif |
soulx | 0:0c88691e3904 | 195 | |
soulx | 0:0c88691e3904 | 196 | decoderState = WAIT_ON_LENGTH; |
soulx | 0:0c88691e3904 | 197 | |
soulx | 0:0c88691e3904 | 198 | break; |
soulx | 0:0c88691e3904 | 199 | } |
soulx | 0:0c88691e3904 | 200 | case WAIT_ON_LENGTH: { |
soulx | 0:0c88691e3904 | 201 | |
soulx | 0:0c88691e3904 | 202 | packet->length = serialCom->getc(); |
soulx | 0:0c88691e3904 | 203 | |
soulx | 0:0c88691e3904 | 204 | #ifdef ANDANTE_DEBUG |
soulx | 0:0c88691e3904 | 205 | pc->printf("0x%02X ", packet->length); |
soulx | 0:0c88691e3904 | 206 | #endif |
soulx | 0:0c88691e3904 | 207 | |
soulx | 0:0c88691e3904 | 208 | decoderState = WAIT_ON_INSTRUCTION_ERROR_ID; |
soulx | 0:0c88691e3904 | 209 | |
soulx | 0:0c88691e3904 | 210 | break; |
soulx | 0:0c88691e3904 | 211 | } |
soulx | 0:0c88691e3904 | 212 | case WAIT_ON_INSTRUCTION_ERROR_ID: { |
soulx | 0:0c88691e3904 | 213 | |
soulx | 0:0c88691e3904 | 214 | packet->instructionErrorId = serialCom->getc(); |
soulx | 0:0c88691e3904 | 215 | |
soulx | 0:0c88691e3904 | 216 | #ifdef ANDANTE_DEBUG |
soulx | 0:0c88691e3904 | 217 | pc->printf("0x%02X ", packet->instructionErrorId); |
soulx | 0:0c88691e3904 | 218 | #endif |
soulx | 0:0c88691e3904 | 219 | |
soulx | 0:0c88691e3904 | 220 | if(packet->length > 2) |
soulx | 0:0c88691e3904 | 221 | decoderState = WAIT_ON_PARAMETER; |
soulx | 0:0c88691e3904 | 222 | else |
soulx | 0:0c88691e3904 | 223 | decoderState = WAIT_ON_CHECK_SUM; |
soulx | 0:0c88691e3904 | 224 | |
soulx | 0:0c88691e3904 | 225 | break; |
soulx | 0:0c88691e3904 | 226 | } |
soulx | 0:0c88691e3904 | 227 | case WAIT_ON_PARAMETER: { |
soulx | 0:0c88691e3904 | 228 | |
soulx | 0:0c88691e3904 | 229 | uint8_t parameter = serialCom->getc(); |
soulx | 0:0c88691e3904 | 230 | packet->parameter[currentParameter] = parameter; |
soulx | 0:0c88691e3904 | 231 | |
soulx | 0:0c88691e3904 | 232 | #ifdef ANDANTE_DEBUG |
soulx | 0:0c88691e3904 | 233 | pc->printf("0x%02X ", parameter); |
soulx | 0:0c88691e3904 | 234 | #endif |
soulx | 0:0c88691e3904 | 235 | |
soulx | 0:0c88691e3904 | 236 | if(++currentParameter == packet->length - 2) |
soulx | 0:0c88691e3904 | 237 | decoderState = WAIT_ON_CHECK_SUM; |
soulx | 0:0c88691e3904 | 238 | |
soulx | 0:0c88691e3904 | 239 | break; |
soulx | 0:0c88691e3904 | 240 | } |
soulx | 0:0c88691e3904 | 241 | case WAIT_ON_CHECK_SUM: { |
soulx | 0:0c88691e3904 | 242 | |
soulx | 0:0c88691e3904 | 243 | packet->checkSum = serialCom->getc(); |
soulx | 0:0c88691e3904 | 244 | |
soulx | 0:0c88691e3904 | 245 | #ifdef ANDANTE_DEBUG |
soulx | 0:0c88691e3904 | 246 | pc->printf("sum =0x%02X\r\n", packet->checkSum); |
soulx | 0:0c88691e3904 | 247 | #endif |
soulx | 0:0c88691e3904 | 248 | |
soulx | 0:0c88691e3904 | 249 | decoderState = WAIT_ON_HEADER_0; |
soulx | 0:0c88691e3904 | 250 | isWholePacket = true; |
soulx | 0:0c88691e3904 | 251 | |
soulx | 0:0c88691e3904 | 252 | break; |
soulx | 0:0c88691e3904 | 253 | } |
soulx | 0:0c88691e3904 | 254 | } |
soulx | 0:0c88691e3904 | 255 | } |
soulx | 0:0c88691e3904 | 256 | } |
soulx | 0:0c88691e3904 | 257 | |
soulx | 0:0c88691e3904 | 258 | timer.stop(); |
soulx | 0:0c88691e3904 | 259 | |
soulx | 0:0c88691e3904 | 260 | if(!isWholePacket) { |
soulx | 0:0c88691e3904 | 261 | #ifdef ANDANTE_DEBUG |
soulx | 0:0c88691e3904 | 262 | pc->printf("Error: Read response timeout\r\n"); |
soulx | 1:1f6864549b92 | 263 | pc->printf("Timer: %d ms\r\n", timer.read_ms()); |
soulx | 0:0c88691e3904 | 264 | #endif |
soulx | 0:0c88691e3904 | 265 | |
soulx | 0:0c88691e3904 | 266 | return ANDANTE_ERRBIT_READ_TIMEOUT; |
soulx | 0:0c88691e3904 | 267 | } |
soulx | 0:0c88691e3904 | 268 | |
soulx | 1:1f6864549b92 | 269 | return ANDANTE_ERRBIT_NONE; |
soulx | 0:0c88691e3904 | 270 | } |
soulx | 0:0c88691e3904 | 271 | |
soulx | 0:0c88691e3904 | 272 | COMMUNICATION::COMMUNICATION(PinName tx, PinName rx, uint32_t baudRate, uint16_t tx_buff, uint16_t rx_buff ) |
soulx | 0:0c88691e3904 | 273 | { |
soulx | 0:0c88691e3904 | 274 | |
soulx | 0:0c88691e3904 | 275 | |
soulx | 0:0c88691e3904 | 276 | #ifdef ANDANTE_DEBUG |
soulx | 0:0c88691e3904 | 277 | pc = new Serial(USBTX, USBRX); |
soulx | 0:0c88691e3904 | 278 | pc->baud(115200); |
soulx | 0:0c88691e3904 | 279 | pc->printf("\033[2J"); |
soulx | 0:0c88691e3904 | 280 | #endif |
soulx | 0:0c88691e3904 | 281 | |
soulx | 0:0c88691e3904 | 282 | serialCom = new iSerial(tx, rx,NULL,tx_buff,rx_buff); |
soulx | 0:0c88691e3904 | 283 | serialCom->baud(baudRate); |
soulx | 0:0c88691e3904 | 284 | |
soulx | 0:0c88691e3904 | 285 | } |
soulx | 0:0c88691e3904 | 286 | |
soulx | 0:0c88691e3904 | 287 | COMMUNICATION::~COMMUNICATION() |
soulx | 0:0c88691e3904 | 288 | { |
soulx | 0:0c88691e3904 | 289 | #ifdef ANDANTE_DEBUG |
soulx | 0:0c88691e3904 | 290 | if(pc != NULL) |
soulx | 0:0c88691e3904 | 291 | delete pc; |
soulx | 0:0c88691e3904 | 292 | #endif |
soulx | 0:0c88691e3904 | 293 | |
soulx | 0:0c88691e3904 | 294 | if(serialCom != NULL) |
soulx | 0:0c88691e3904 | 295 | delete serialCom; |
soulx | 0:0c88691e3904 | 296 | } |