v1
Fork of Fork_Boss_Communication_Robot by
Diff: communication.cpp
- Revision:
- 0:0c88691e3904
- Child:
- 1:1f6864549b92
diff -r 000000000000 -r 0c88691e3904 communication.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/communication.cpp Sun Mar 01 06:28:01 2015 +0000 @@ -0,0 +1,157 @@ +#include "mbed.h" +#include "communication.h" + +uint8_t COMMUNICATION::CommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet) +{ + uint8_t currentParameter = 0; + bool isWholePacket = false; + + currentParameter = 0; + isWholePacket = false; + uint8_t decoderState = WAIT_ON_HEADER_0; + + Timer timer; + timer.start(); + + while((timer.read_ms() < ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket)) { + if(serialCom->readable()) { + switch(decoderState) { + case WAIT_ON_HEADER_0: { + + uint8_t mx28ProtocolHeader0 = serialCom->getc(); + +#ifdef ANDANTE_DEBUG + pc->printf("Read: 0x%02X ", mx28ProtocolHeader0); +#endif + + decoderState = WAIT_ON_HEADER_1; + + break; + } + case WAIT_ON_HEADER_1: { + + uint8_t mx28ProtocolHeader1 = serialCom->getc(); + +#ifdef ANDANTE_DEBUG + pc->printf("0x%02X ", mx28ProtocolHeader1); +#endif + + decoderState = WAIT_ON_ROBOT_ID; + + break; + } + case WAIT_ON_ROBOT_ID: { + + + packet->robotId = serialCom->getc(); + +#ifdef ANDANTE_DEBUG + pc->printf("0x%02X ", packet->servoId); +#endif + + decoderState = WAIT_ON_LENGTH; + + break; + } + case WAIT_ON_LENGTH: { + + packet->length = serialCom->getc(); + +#ifdef ANDANTE_DEBUG + pc->printf("0x%02X ", packet->length); +#endif + + decoderState = WAIT_ON_INSTRUCTION_ERROR_ID; + + break; + } + case WAIT_ON_INSTRUCTION_ERROR_ID: { + + packet->instructionErrorId = serialCom->getc(); + +#ifdef ANDANTE_DEBUG + pc->printf("0x%02X ", packet->instructionErrorId); +#endif + + if(packet->length > 2) + decoderState = WAIT_ON_PARAMETER; + else + decoderState = WAIT_ON_CHECK_SUM; + + break; + } + case WAIT_ON_PARAMETER: { + + uint8_t parameter = serialCom->getc(); + packet->parameter[currentParameter] = parameter; + +#ifdef ANDANTE_DEBUG + pc->printf("0x%02X ", parameter); +#endif + + if(++currentParameter == packet->length - 2) + decoderState = WAIT_ON_CHECK_SUM; + + break; + } + case WAIT_ON_CHECK_SUM: { + + packet->checkSum = serialCom->getc(); + +#ifdef ANDANTE_DEBUG + pc->printf("sum =0x%02X\r\n", packet->checkSum); +#endif + + decoderState = 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: Read response timeout\r\n"); +#endif + + return ANDANTE_ERRBIT_READ_TIMEOUT; + } + + timer.reset(); + timer.start(); + +} + +COMMUNICATION::COMMUNICATION(PinName tx, PinName rx, uint32_t baudRate, uint16_t tx_buff, uint16_t rx_buff ) +{ + + +#ifdef ANDANTE_DEBUG + pc = new Serial(USBTX, USBRX); + pc->baud(115200); + pc->printf("\033[2J"); +#endif + + serialCom = new iSerial(tx, rx,NULL,tx_buff,rx_buff); + serialCom->baud(baudRate); + +} + +COMMUNICATION::~COMMUNICATION() +{ +#ifdef ANDANTE_DEBUG + if(pc != NULL) + delete pc; +#endif + + if(serialCom != NULL) + delete serialCom; +} \ No newline at end of file