Reading Dynamixel AX-12+ position

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 :

[sourcecode language="cpp"]
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;
}
[/sourcecode]