Using the COM Port directly

[techtasie]

Hello,

I am a member of the carologistics team at the rcll. Our current software stack is based on Fawkes. Fawkes uses direct communication over the Sriall/COM port with the robotino. My job is to add support for the internal gyro and accelerometer to fawkes. We are using the Robotino 4. But the person that developed the Robotino Plugin is no longer a team member.

In order to add that support without rewriting the entire plugin to OpenRobotinoAPI2 i need either the source code of OpenRobotinoAPI2 or some internal documentation for the COM port.

[verbeek]

Your gyro is a R6093U.

https://ip.festo-didactic.com/infoportal/Robotino/document/gyro.pdf

And here is my implementation to parse the data from the gyro

void ComImpl::reset()
{
    if (isOpen())
    {
        _reset = true;
    }
}

void ComImpl::run()
{
    _sequence = 0;

    int errorCounter = 0;
    unsigned char buffer[26];
    memset(buffer, 0, sizeof(buffer));
    int bufferIndex = 0;

    QTime dataTimer;
    dataTimer.start();
    
    unsigned char startCh = 0xA6;
    int numChars = 26;
    
    int state=NOTSYNCED;
    
    switch(_com->type())
    {
        case R1071T:
            startCh = 0xFF;
            numChars = 8;
            break;
            
        default:
            startCh = 0xA6;
            numChars = 26;
            break;
    }
    
    while( _run )
    {    
        if( errorCounter > 2 )
        {
            if(R1071T == _com->type() )
            {
                setParameters();
            }
            
            errorCounter = 0;
            _serialPort.flush();
        }

        if (_reset)
        {
            const char* resetCommand = "$MIB,RESET*87";
            _serialPort.write((const unsigned char*)resetCommand, strlen(resetCommand));
            _reset = false;
        }

        switch(state)
        {
            case NOTSYNCED:
            {
                unsigned char ch;
                int bytes_read = _serialPort.read( &ch, 1 );
                
                if (1 == bytes_read)
                {
                    if (bufferIndex<2)
                    {
                        if (startCh == ch)
                        {
                            buffer[bufferIndex++] = ch;
                        }
                    }
                    else
                    {
                        buffer[bufferIndex++] = ch;
                    }

                    if (numChars == bufferIndex)
                    {
                        Measurement mes;
                        
                        bool ret = false;
                        
                        switch(_com->type())
                        {
                            case R1071T:
                                ret = parse_R1071T_data(buffer, &mes);
                                break;
                                
                            default:
                                ret = parse_R6093U_data(buffer, &mes);
                                break;
                        }

                        if (ret)
                        {
                            errorCounter = 0;

                            if (0 == _sequence)
                            {
                                _comActiveSemaphore.release();
                            }
                            ++_sequence;
                            dataTimer.restart();

                            _com->dataReceivedEvent(mes);
                            
                            //qDebug() << "synced";
                            state = SYNCED;
                        }
                        else
                        {
                            _com->errorReport("CruizCore: Error parsing data");
                            ++errorCounter;
                        }

                        bufferIndex = 0;
                    }
                }
            }
            break;
            
            case SYNCED:
            {
                int bytes_read = _serialPort.read( &buffer[0], numChars );
                if(bytes_read == numChars)
                {
                    Measurement mes;
                    
                    bool ret = false;
                    
                    switch(_com->type())
                    {
                        case R1071T:
                            ret = parse_R1071T_data(buffer, &mes);
                            break;
                            
                        default:
                            ret = parse_R6093U_data(buffer, &mes);
                            break;
                    }

                    if (ret)
                    {
                        errorCounter = 0;

                        if (0 == _sequence)
                        {
                            _comActiveSemaphore.release();
                        }
                        ++_sequence;
                        dataTimer.restart();

                        _com->dataReceivedEvent(mes);
                    }
                    else
                    {
                        _com->errorReport("CruizCore: Error parsing data");
                        ++errorCounter;
                        
                        _com->errorReport("CruizCore: lost synchronization");
                        state = NOTSYNCED;

                        //for (int i = 0; i < numChars; ++i) qDebug() << i << "  " << buffer[i] << "  " << (char)buffer[i];
                    }
                }
                else
                {
                    _com->errorReport("CruizCore: lost synchronization");
                    state = NOTSYNCED;
                }
            }
            break;
            
            default:
            break;
        }
        
        if(dataTimer.elapsed() > 1000 )
        {
            _com->errorReport( "Could not read vaild data package for mor than 1000ms." );
            _run = false;
        }
    }

    _com->errorReport( "CruizCore receive thread stopped" );
    _com->stopped();
}

bool ComImpl::parse_R6093U_data( const unsigned char* data, Measurement* result) const
{
    short int temp;
    
    //Verify packet heading information
    if( data[0] != 0xA6 || data[1] != 0xA6 )
    {
        _com->errorReport( "Data header error" );
        return false;
    }
    
    //Verify checksum
    unsigned char check_sum = 0;
    for(int i=2; i<25; ++i)
    {
        check_sum += data[i];
    }

    if(data[25] != check_sum)
    {
        _com->errorReport( "Checksum mismatch error" );
        return false;
    }

    temp = (data[3] & 0xFF) | ((data[4] << 8) & 0xFF00);
    result->roll = -PI * temp / 18000.0f;

    temp = (data[5] & 0xFF) | ((data[6] << 8) & 0xFF00);
    result->pitch = -PI * temp / 18000.0f;

    temp = (data[7] & 0xFF) | ((data[8] << 8) & 0xFF00);
    result->yaw = -PI * temp / 18000.0f;

    temp = (data[9] & 0xFF) | ((data[10] << 8) & 0XFF00);
    result->rollrate = -PI * temp / 18000.0f;

    temp = (data[11] & 0xFF) | ((data[12] << 8) & 0XFF00);
    result->pitchrate = -PI * temp / 18000.0f;

    temp = (data[13] & 0xFF) | ((data[14] << 8) & 0XFF00);
    result->yawrate = - PI * temp / 18000.0f;
    
    temp = (data[15] & 0xFF) | ((data[16] << 8) & 0XFF00);
    result->xacc = 0.01f * temp;

    temp = (data[17] & 0xFF) | ((data[18] << 8) & 0XFF00);
    result->yacc = 0.01f * temp;

    temp = (data[19] & 0xFF) | ((data[20] << 8) & 0XFF00);
    result->zacc = 0.01f * temp;

    return true;
}

void ComImpl::setParameters()
{
    unsigned char buf[30];

    buf[0] = '$';
    buf[1] = 'M';
    buf[2] = 'I';
    buf[3] = 'A';
    buf[4] = ',';
    buf[5] = 'I';
    buf[6] = ',';
    buf[7] = 'B';
    buf[8] = ',';
    buf[9] = '1';
    buf[10] = '1';
    buf[11] = '5';
    buf[12] = '2';
    buf[13] = '0';
    buf[14] = '0';
    buf[15] = ',';
    buf[16] = 'R';
    buf[17] = ',';
    buf[18] = '1';
    buf[19] = '0';
    buf[20] = '0';
    buf[21] = ',';
    buf[22] = 'D';
    buf[23] = ',';
    buf[24] = 'Y';
    buf[25] = ',';
    buf[26] = 'N';
    buf[27] = '*';
    buf[28] = 0;
    buf[29] = 0;

    unsigned int checksum = 0;
    for( int i=1; i<27; ++i )
    {
        checksum += buf[i];
    }

    checksum &= 0xFF;

    std::ostringstream os;
    os << std::hex << std::uppercase << checksum;

    std::string str = os.str();

    buf[28] = str[0];
    buf[29] = str[1];

    _serialPort.write( buf, 30 );
}

​