BE@R lab / Fork_Boss_Communication_Robot

ใช้กับขาหมา

Dependents:   BEAR_Protocol BEAR_Reciever BETAGO

Fork of Communication_Robot by BE@R lab

communication.cpp@10:dfcb16b99b85, 2016-02-02 (annotated)

Committer:
b0ssiz
Date:
Tue Feb 02 01:29:23 2016 +0000
Revision:
10:dfcb16b99b85
Parent:
6:2ed7945594db
Child:
8:49a5e7d12c82
Turn off debug

Who changed what in which revision?

UserRevisionLine numberNew 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 6:2ed7945594db 171 if(mx28ProtocolHeader0 == 0xFF)
soulx 6:2ed7945594db 172 decoderState = WAIT_ON_HEADER_1;
soulx 6:2ed7945594db 173 else
soulx 6:2ed7945594db 174 isWholePacket = false;
soulx 0:0c88691e3904 175
soulx 0:0c88691e3904 176 break;
soulx 0:0c88691e3904 177 }
soulx 0:0c88691e3904 178 case WAIT_ON_HEADER_1: {
soulx 0:0c88691e3904 179
soulx 0:0c88691e3904 180 uint8_t mx28ProtocolHeader1 = serialCom->getc();
soulx 0:0c88691e3904 181
soulx 0:0c88691e3904 182 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 183 pc->printf("0x%02X ", mx28ProtocolHeader1);
soulx 0:0c88691e3904 184 #endif
soulx 0:0c88691e3904 185
soulx 0:0c88691e3904 186 decoderState = WAIT_ON_ROBOT_ID;
soulx 0:0c88691e3904 187
soulx 0:0c88691e3904 188 break;
soulx 0:0c88691e3904 189 }
soulx 0:0c88691e3904 190 case WAIT_ON_ROBOT_ID: {
soulx 0:0c88691e3904 191
soulx 0:0c88691e3904 192
soulx 0:0c88691e3904 193 packet->robotId = serialCom->getc();
soulx 0:0c88691e3904 194
soulx 0:0c88691e3904 195 #ifdef ANDANTE_DEBUG
soulx 2:56c1de1612dd 196 pc->printf("0x%02X ", packet->robotId);
soulx 0:0c88691e3904 197 #endif
soulx 0:0c88691e3904 198
soulx 0:0c88691e3904 199 decoderState = WAIT_ON_LENGTH;
soulx 0:0c88691e3904 200
soulx 0:0c88691e3904 201 break;
soulx 0:0c88691e3904 202 }
soulx 0:0c88691e3904 203 case WAIT_ON_LENGTH: {
soulx 0:0c88691e3904 204
soulx 0:0c88691e3904 205 packet->length = serialCom->getc();
soulx 0:0c88691e3904 206
soulx 0:0c88691e3904 207 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 208 pc->printf("0x%02X ", packet->length);
soulx 0:0c88691e3904 209 #endif
soulx 0:0c88691e3904 210
soulx 0:0c88691e3904 211 decoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
soulx 0:0c88691e3904 212
soulx 0:0c88691e3904 213 break;
soulx 0:0c88691e3904 214 }
soulx 0:0c88691e3904 215 case WAIT_ON_INSTRUCTION_ERROR_ID: {
soulx 0:0c88691e3904 216
soulx 0:0c88691e3904 217 packet->instructionErrorId = serialCom->getc();
soulx 0:0c88691e3904 218
soulx 0:0c88691e3904 219 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 220 pc->printf("0x%02X ", packet->instructionErrorId);
soulx 0:0c88691e3904 221 #endif
soulx 0:0c88691e3904 222
soulx 0:0c88691e3904 223 if(packet->length > 2)
soulx 0:0c88691e3904 224 decoderState = WAIT_ON_PARAMETER;
soulx 0:0c88691e3904 225 else
soulx 0:0c88691e3904 226 decoderState = WAIT_ON_CHECK_SUM;
soulx 0:0c88691e3904 227
soulx 0:0c88691e3904 228 break;
soulx 0:0c88691e3904 229 }
soulx 0:0c88691e3904 230 case WAIT_ON_PARAMETER: {
soulx 0:0c88691e3904 231
soulx 0:0c88691e3904 232 uint8_t parameter = serialCom->getc();
soulx 0:0c88691e3904 233 packet->parameter[currentParameter] = parameter;
soulx 0:0c88691e3904 234
soulx 0:0c88691e3904 235 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 236 pc->printf("0x%02X ", parameter);
soulx 0:0c88691e3904 237 #endif
soulx 0:0c88691e3904 238
soulx 0:0c88691e3904 239 if(++currentParameter == packet->length - 2)
soulx 0:0c88691e3904 240 decoderState = WAIT_ON_CHECK_SUM;
soulx 0:0c88691e3904 241
soulx 0:0c88691e3904 242 break;
soulx 0:0c88691e3904 243 }
soulx 0:0c88691e3904 244 case WAIT_ON_CHECK_SUM: {
soulx 0:0c88691e3904 245
soulx 0:0c88691e3904 246 packet->checkSum = serialCom->getc();
soulx 0:0c88691e3904 247
soulx 0:0c88691e3904 248 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 249 pc->printf("sum =0x%02X\r\n", packet->checkSum);
soulx 0:0c88691e3904 250 #endif
soulx 0:0c88691e3904 251
soulx 0:0c88691e3904 252 decoderState = WAIT_ON_HEADER_0;
soulx 0:0c88691e3904 253 isWholePacket = true;
soulx 0:0c88691e3904 254
soulx 0:0c88691e3904 255 break;
soulx 0:0c88691e3904 256 }
soulx 0:0c88691e3904 257 }
soulx 0:0c88691e3904 258 }
soulx 0:0c88691e3904 259 }
soulx 0:0c88691e3904 260
soulx 0:0c88691e3904 261 timer.stop();
soulx 0:0c88691e3904 262
soulx 0:0c88691e3904 263 if(!isWholePacket) {
soulx 0:0c88691e3904 264 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 265 pc->printf("Error: Read response timeout\r\n");
soulx 1:1f6864549b92 266 pc->printf("Timer: %d ms\r\n", timer.read_ms());
soulx 0:0c88691e3904 267 #endif
soulx 0:0c88691e3904 268
soulx 0:0c88691e3904 269 return ANDANTE_ERRBIT_READ_TIMEOUT;
soulx 0:0c88691e3904 270 }
soulx 0:0c88691e3904 271
soulx 1:1f6864549b92 272 return ANDANTE_ERRBIT_NONE;
soulx 0:0c88691e3904 273 }
soulx 0:0c88691e3904 274
soulx 0:0c88691e3904 275 COMMUNICATION::COMMUNICATION(PinName tx, PinName rx, uint32_t baudRate, uint16_t tx_buff, uint16_t rx_buff )
soulx 0:0c88691e3904 276 {
soulx 0:0c88691e3904 277
soulx 0:0c88691e3904 278
soulx 0:0c88691e3904 279 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 280 pc = new Serial(USBTX, USBRX);
soulx 0:0c88691e3904 281 pc->baud(115200);
soulx 0:0c88691e3904 282 pc->printf("\033[2J");
soulx 0:0c88691e3904 283 #endif
soulx 0:0c88691e3904 284
soulx 0:0c88691e3904 285 serialCom = new iSerial(tx, rx,NULL,tx_buff,rx_buff);
soulx 0:0c88691e3904 286 serialCom->baud(baudRate);
soulx 0:0c88691e3904 287
soulx 0:0c88691e3904 288 }
soulx 0:0c88691e3904 289
soulx 0:0c88691e3904 290 COMMUNICATION::~COMMUNICATION()
soulx 0:0c88691e3904 291 {
soulx 0:0c88691e3904 292 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 293 if(pc != NULL)
soulx 0:0c88691e3904 294 delete pc;
soulx 0:0c88691e3904 295 #endif
soulx 0:0c88691e3904 296
soulx 0:0c88691e3904 297 if(serialCom != NULL)
soulx 0:0c88691e3904 298 delete serialCom;
soulx 0:0c88691e3904 299 }