File content as of revision 11:3c11a0355a3e:

#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
                    if(mx28ProtocolHeader0 == 0xFF)
                        decoderState = WAIT_ON_HEADER_1;
                    else
                        isWholePacket = false;

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