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