Writing 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 write the AX-12+ position :

[sourcecode language="cpp"]
int Dynamixel::getSetAX12PositionCommand(byte id, short goal)
{
int pos = 0;
byte numberOfParameters = 0;
//OXFF 0XFF ID LENGTH INSTRUCTION PARAMETER1 …PARAMETER N CHECK SUM

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

// bodyLength
buffer[pos++] = 0; //place holder

//the instruction, query => 3
buffer[pos++] = 3;

// goal registers 30 and 31
buffer[pos++] = 0x1E;// 30;

//bytes to write
byte hexH = 0;
byte hexL = 0;
toHexHLConversion(goal, &hexH, &hexL);
buffer[pos++] = hexL;
numberOfParameters++;
buffer[pos++] = hexH;
numberOfParameters++;

// bodyLength
buffer[3] = (byte)(numberOfParameters + 3);

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

return pos;
}

int Dynamixel::setPosition(SerialPort *serialPort, int idAX12, int position)
{
int error=0;

int n=getSetAX12PositionCommand(idAX12, position);
long l=serialPort->sendArray(buffer,n);
Sleep(10);

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

if (n>4 && bufferIn[4] == 0)
printf("nid=<%i> set at pos=<%i>n", idAX12, position);
else {
error=-1;
printf("nid=<%i> error: <%i>n", idAX12, bufferIn[4]);
bf(bufferIn, n);
}

return error;
}

[/sourcecode]