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