The intent of this code is to be clear and easy to understand, it not pretend to be the best code to do the job.

This method create the command to read the AX-12+ position :

int Dynamixel::getReadAX12PositionCommand(byte id)
{
    //OXFF 0XFF ID LENGTH INSTRUCTION PARAMETER1 …PARAMETER N CHECK SUM
    int pos = 0;

    buffer[pos++] = 0xff;
    buffer[pos++] = 0xff;
    buffer[pos++] = id;

    // length = 4
    buffer[pos++] = 4; //placeholder

    //the instruction, read => 2
    buffer[pos++] = 2;

    // pos registers 36 and 37
    buffer[pos++] = 36;

    //bytes to read
    buffer[pos++] = 2;

    byte checksum = checkSumatory(buffer, pos);
    buffer[pos++] = checksum;

    return pos;
}

int Dynamixel::getPosition(SerialPort *serialPort, int idAX12)
{
	int ret=0;

	int n=getReadAX12PositionCommand(idAX12);
	long l=serialPort->sendArray(buffer,n);
	Sleep(2);

	memset(bufferIn,0,BufferSize);
	n=serialPort->getArray(bufferIn, 8);

	short pos = -1;
	if (n>7)
	{
		pos = fromHexHLConversion(bufferIn[5], bufferIn[6]);
	}

	printf("nid=<%i> pos=<%i> length=<%i>n", idAX12, pos, n);
	if (pos<0 || pos > 1023)
		ret=-2;
	else
		ret=pos;

	return ret;
}