Controlling Bioloid from PC

Finally, I decided to do some inverse kinematics and control for my assembled Bioloid humanoid “type A” from PC:

Bioloid Humanoid Type A

Unfortunately, it turned out to be not so easy as I thought initially…

I read some online stuff from Robotis (their site lacks good documentation by the way) and expected that I can use USB2Dynamixel connector to connect my Bioloid to PC and then use Dynamixel SDK from Robotis to control dynamixels programmatically from a C++ program. After some trial and error and looking for answers online I found to my huge surprise that you cannot use Dynamixel SDK when the kit is connected to PC via CM-510 or CM-5 controller.

This  setup (which I think should be fairly popular): PC<->USB2Dynamixel<->CM-510<->Dynamixels  is NOT supported by Dynamixel SDK 😦   You can only use this SDK if you hook Dynamixels directly to USB2Dynamixel and connect them to the battery/power. I know that many “advanced” people do it this way, but I don’t like it when the most obvious setup is not supported out of the box.

Fortunately, after some searching I found how to do it yourself (no soldering required, only programming). It is a non-documented solution and it is sort of easy to do once you know how, but it is not easy to find good explanation about it and working code. So I decided to document it here step by step and provide a working C++ code sample for Windows (see below).

BIG thanks to Aprendiendo whos site contains the solution, but it is not complete for C++ if you run on Windows. So I re-wrote some of that code to make it a complete C++ code sample that you can download and run on Windows. This could save you some time if you want to do the same 🙂

So, to control your Bioloid from PC you need to:

1) Use USB2Dynamixel or other USB to Serial connector to connect CM-510 (or CM-5) controller via a serial cable to PC’s USB port. If you have USB2Dynamixel, then you have to switch it to RS232 mode (position #3)

2) Usually, USB2Dynamixel appears as COM3 port on your PC. On Windows, you have to open this port’s properties (in devices) and go to “Advanced” settings and set “Latency Timer” to 1 (msec) as described here

3) Power up your CM-510 (or CM-5). The “manage” mode indicator should start blinking. Press the “Start” button to start the “managed mode”. The indicator next to “manage” title should light up. If you skip this step, Bioloid’s controller is not going to be in the right state for controlling from PC.

4) Now, the most important part – the Bioloid controller (CM-510 or CM-5) needs to be switched to the “Toss mode“. In this mode, CM-510 firmware works as a bridge and forwards all commands it receives from PC to Dynamixels connected to it and also forwards all Dynamixel responses back to PC. This mode is not documented by Robotis by some reason. To switch to it you need to send “t\r” (‘t’ + 0x0D sequence) from your C/C++ program via a serial port to CM-510. See SendTossModeCommand() function that does it in the C++ code below.

5) After that, you can create Dynamixel protocol commands and send them to CM-510 via the serial port. Make sure that the program initializes the COM port communication speed at 57600, since this is the max speed that CM-5 and CM-510 support. See C++ code below for details.

NOTE 1: I found that on some PCs this process was not enough. After powering your Bioloid you need to run RoboPlus Manager from Robotis and connect the app to your Bioloid (press “connect” button) before controlling it from your PC program. The RoboPlus Manager does tons of obfuscated non documented communication and by some reason the “Toss mode” can be initialized only after that!

NOTE 2: “Toss mode” cannot be turned OFF (at least I don’t know how at the moment) and to turn it off you need to turn your Bioloid power OFF.

NOTE 3: I am still not sure if I can read sensors connected to CM-510 without custom firmware. See Aprendiendo website for more information about custom firmware to do this.

This BioloidAPI.ZIP file contains the C++ project for Windows (VS 2010 project) that demonstrates how to switch to the “toss mode” and how to send Dynamixel commands to move servos. I was trying to make it easy to understand. Here is most of the C++ code:

#include
#include
#include

class SerialPort
{
public:
    SerialPort();
    virtual ~SerialPort();

    HRESULT Open(const wchar_t* szPortName, DWORD baudRate);
    void Close();
    void Clear();

    HRESULT SendData(BYTE* pBuffer, unsigned long* pSize);
    HRESULT ReceiveData(BYTE* pBuffer, unsigned long* pSize);

private:
    HANDLE serialPortHandle;
};

SerialPort::SerialPort() :
serialPortHandle(INVALID_HANDLE_VALUE)
{
}

SerialPort::~SerialPort()
{
    Close();
}

HRESULT SerialPort::Open(const wchar_t* szPortName, DWORD baudRate)
{
    HRESULT hrResult = S_OK;
    DCB dcb;

    memset( &dcb, 0, sizeof(dcb) );

    dcb.DCBlength = sizeof(dcb);
    dcb.BaudRate = baudRate;
    dcb.Parity = NOPARITY;
    dcb.fParity = 0;
    dcb.StopBits = ONESTOPBIT;
    dcb.ByteSize = 8;

    serialPortHandle = CreateFile(szPortName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, NULL, NULL);
    if ( serialPortHandle!=INVALID_HANDLE_VALUE )
    {
        if( !SetCommState(serialPortHandle, &dcb) )
        {
            hrResult = E_INVALIDARG;
            Close();
        }
    }
    else
    {
        hrResult = ERROR_OPEN_FAILED;
    }

    return hrResult;
}

void SerialPort::Close()
{
    if (serialPortHandle!=INVALID_HANDLE_VALUE || serialPortHandle!=NULL)
    {
        PurgeComm(serialPortHandle, PURGE_RXCLEAR | PURGE_TXCLEAR);
        CloseHandle(serialPortHandle);
    }
    serialPortHandle = INVALID_HANDLE_VALUE;
}

void SerialPort::Clear()
{
    if (serialPortHandle!=INVALID_HANDLE_VALUE || serialPortHandle!=NULL)
    {
        PurgeComm(serialPortHandle, PURGE_RXCLEAR | PURGE_TXCLEAR);
    }
}

HRESULT SerialPort::SendData(BYTE* pBuffer, unsigned long* pSize)
{
    HRESULT hrResult = ERROR_WRITE_FAULT;

    if (serialPortHandle!=INVALID_HANDLE_VALUE && serialPortHandle!=NULL)
    {
        if( WriteFile(serialPortHandle, pBuffer, *pSize, pSize, NULL) &&
            FlushFileBuffers(serialPortHandle)
            )
        {
            hrResult = S_OK;
        }
    }

    return hrResult;
}

HRESULT SerialPort::ReceiveData(BYTE* pBuffer, unsigned long* pSize)
{
    HRESULT hrResult = ERROR_READ_FAULT;

    if (serialPortHandle!=INVALID_HANDLE_VALUE && serialPortHandle!=NULL)
    {
        if( ReadFile(serialPortHandle, pBuffer, *pSize, pSize, NULL) )
        {
            hrResult = S_OK;
        }
    }

    return hrResult;
}

bool CreateAX12SetPositionCommand(BYTE id, short goal, BYTE* pBuffer, DWORD* pSize)
{
    const unsigned int packetSize = 9;

    if(*pSize >8);			// goal high byte (to address 31)

    // Checksum
    DWORD packetSum = 0;
    for(size_t i=2; iSendData(buffer, &size);
    if(FAILED(hr))
    {
        printf("Failed to send set dynamixel position command\n");
        return false;
    }
    Sleep(10);

    memset(buffer, 0, sizeof(buffer));
    size = sizeof(buffer);
    pSerialPort->ReceiveData(buffer, &size);

    if (size>4 && buffer[4] == 0)
    {
        printf("id=%d set to position=%d\n", id, position);
    }
    else
    {
        printf("Error while setting id=%d position=%d, error:%d\n", id, position, buffer[4]);
        return false;
    }

    return true;
}

bool SendTossModeCommand(SerialPort* pSerialPort)
{
    BYTE buffer[1024];
    buffer[0]='t';
    buffer[1]='\r';
    DWORD size = 2;

    HRESULT hr = pSerialPort->SendData(buffer, &size);
    if(FAILED(hr))
    {
        printf("Failed to send TOSS model command\n");
        return false;
    }
    Sleep(100);

    size = sizeof(buffer);
    pSerialPort->ReceiveData(buffer, &size);

    return true;
}

int _tmain(int argc, _TCHAR* argv[])
{
    DWORD baudRate = 57600;
    SerialPort comPort;

    HRESULT hr = comPort.Open(L"COM3", baudRate);
    if(FAILED(hr))
    {
        printf("Cannot open COM3 port\n");
        return 0;
    }

    SendTossModeCommand(&comPort);

    while(1)
    {
        printf( "Enter dynamixel ID and goal position:\n" );

        int id = 0;
        int position = 0;
        scanf("%d %d", &id, &position);

        SetDynamixelPosition(&comPort, id, position);

        printf("Press ESC to terminate, otherwise press any other key to continue\n");
        if(getch() == 0x1b)
        {
            break;
        }
    }

    comPort.Close();

    return 0;
}