Announcement

Collapse
No announcement yet.

Using the COM Port directly

Collapse
X
 
  • Time
  • Show
Clear All
new posts

  • Using the COM Port directly

    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.

  • #2
    Your gyro is a R6093U.



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

    Code:
    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 );
    }
    
    ‚Äč

    Comment

    Working...
    X
    😀
    🥰
    🤢
    😎
    😡
    👍
    👎