palm and chin / Communication_Robot

v1

Fork of Fork_Boss_Communication_Robot by BE@R lab

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?

UserRevisionLine numberNew 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 }