File content as of revision 0:0c88691e3904:

#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;
}