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; }