v1
Fork of Fork_Boss_Communication_Robot by
communication.cpp
- Committer:
- soulx
- Date:
- 2015-03-01
- Revision:
- 1:1f6864549b92
- Parent:
- 0:0c88691e3904
- Child:
- 2:56c1de1612dd
File content as of revision 1:1f6864549b92:
#include "mbed.h" #include "communication.h" uint8_t COMMUNICATION::receiveCommunicatePacket(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; } } } } timer.stop(); if(!isWholePacket) { #ifdef ANDANTE_DEBUG pc->printf("Error: Read response timeout\r\n"); pc->printf("Timer: %d ms\r\n", timer.read_ms()); #endif return ANDANTE_ERRBIT_READ_TIMEOUT; } return ANDANTE_ERRBIT_NONE; } 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; }