phirawat rattanachote / Communication_Robot

v2

Fork of Communication_Robot by Betago

communication.cpp@14:cf43df0ddb93, 2016-06-07 (annotated)

Committer:
palmdotax
Date:
Tue Jun 07 17:22:37 2016 +0000
Revision:
14:cf43df0ddb93
Parent:
13:bc19774be4df 
v2

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 {
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 }