v1
Fork of Fork_Boss_Communication_Robot by
Diff: communication.cpp
- Revision:
- 2:56c1de1612dd
- Parent:
- 1:1f6864549b92
- Child:
- 3:2f334f0cee75
--- a/communication.cpp Sun Mar 01 08:48:23 2015 +0000 +++ b/communication.cpp Sun Mar 01 15:40:51 2015 +0000 @@ -1,5 +1,149 @@ #include "mbed.h" #include "communication.h" +//#include "protocol.h" +//#include "Utilities.h" +//#include "iSerial.h" + + +uint8_t COMMUNICATION::sendCommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet) +{ + uint8_t currentParameter = 0; + bool isWholePacket = false; + uint8_t encoderState = WAIT_ON_HEADER_0; + + packet->checkSum = Utilities::GetCheckSum((uint8_t*)&(packet->robotId), packet->length + 1); + + Timer timer; + timer.start(); + + while((timer.read_ms() < ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket)) { + + if( serialCom->writeable()) { + + switch(encoderState) { + case WAIT_ON_HEADER_0: { +#ifdef ANDANTE_DEBUG + pc->printf("Write: 0x%02X ", ANDANTE_PROTOCOL_HEADER_0); +#endif + + serialCom->putc(ANDANTE_PROTOCOL_HEADER_0); + // dirc_rs485=0; + // wait_us(DELAY_DIR); + + + encoderState = WAIT_ON_HEADER_1; + + break; + } + case WAIT_ON_HEADER_1: { +#ifdef ANDANTE_DEBUG + pc->printf("0x%02X ", ANDANTE_PROTOCOL_HEADER_1); +#endif + + + + serialCom->putc(ANDANTE_PROTOCOL_HEADER_1); + // dirc_rs485=0; + // wait_us(DELAY_DIR); + + encoderState = WAIT_ON_ROBOT_ID; + + break; + } + case WAIT_ON_ROBOT_ID: { +#ifdef ANDANTE_DEBUG + pc->printf("0x%02X ", packet->robotId); +#endif + + + serialCom->putc(packet->robotId); + + + encoderState = WAIT_ON_LENGTH; + + break; + } + case WAIT_ON_LENGTH: { +#ifdef ANDANTE_DEBUG + pc->printf("0x%02X ", packet->length); +#endif + + serialCom->putc(packet->length); + + + encoderState = WAIT_ON_INSTRUCTION_ERROR_ID; + + break; + } + case WAIT_ON_INSTRUCTION_ERROR_ID: { +#ifdef ANDANTE_DEBUG + pc->printf("0x%02X ", packet->instructionErrorId); +#endif + + + serialCom->putc(packet->instructionErrorId); + + if(packet->length > 2) + encoderState = WAIT_ON_PARAMETER; + else + encoderState = WAIT_ON_CHECK_SUM; + + break; + } + case WAIT_ON_PARAMETER: { +#ifdef ANDANTE_DEBUG + pc->printf("0x%02X ", packet->parameter[currentParameter]); +#endif + + serialCom->putc(packet->parameter[currentParameter]); + + if(++currentParameter == packet->length - 2) + encoderState = WAIT_ON_CHECK_SUM; + + break; + } + case WAIT_ON_CHECK_SUM: { +#ifdef ANDANTE_DEBUG + pc->printf("0x%02X\r\n", packet->checkSum); +#endif + + + serialCom->putc(packet->checkSum); + + + encoderState = WAIT_ON_HEADER_0; + isWholePacket = true; + + break; + } + } + } + } + + +#ifdef ANDANTE_DEBUG + pc->printf("Timer: %d ms\r\n", timer.read_ms()); +#endif + + timer.stop(); + + + + if(!isWholePacket) { +#ifdef ANDANTE_DEBUG + pc->printf("Error: Write response timeout.\r\n"); +#endif + + return ANDANTE_ERRBIT_WRITE_TIMEOUT; + } +/* + if( packet->servoId == ANDANTE_PROTOCOL_BROADCAST_ID || + packet->instructionErrorId == ANDANTE_ACTION || + packet->instructionErrorId == ANDANTE_SYNC_WRITE) +*/ + return ANDANTE_ERRBIT_NONE; + +} uint8_t COMMUNICATION::receiveCommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet) { @@ -46,7 +190,7 @@ packet->robotId = serialCom->getc(); #ifdef ANDANTE_DEBUG - pc->printf("0x%02X ", packet->servoId); + pc->printf("0x%02X ", packet->robotId); #endif decoderState = WAIT_ON_LENGTH;