similar robotis communication v1.0 in this lib use for protocol to control and diagnotic robot
Fork of Communication_Robot by
communication.cpp
- Committer:
- soulx
- Date:
- 2015-03-02
- Revision:
- 3:2f334f0cee75
- Parent:
- 2:56c1de1612dd
File content as of revision 3:2f334f0cee75:
#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) { 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->robotId); #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; }