int dxl_read_word( int id, int address )
{
while(giBusUsing);
gbInstructionPacket[ID] = (unsigned char)id;
gbInstructionPacket[INSTRUCTION] = INST_READ;
gbInstructionPacket[PARAMETER] = (unsigned char)address;
gbInstructionPacket[PARAMETER+1] = 2;
gbInstructionPacket[LENGTH] = 4;
dxl_txrx_packet();
return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]);
}
int dxl_sync_read_pop_word()
{
int b0, b1;
while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX.
if ( gbSyncNbParam >= gbStatusPacket[LENGTH] - 3 )
{
return -1;
}
b0 = gbStatusPacket[PARAMETER + gbSyncNbParam++];
b1 = gbStatusPacket[PARAMETER + gbSyncNbParam++];
return dxl_makeword( b0, b1 );
}
// Read data from the control table of a Dynamixel device
// Length 0x04, Instruction 0x02
// Parameter1 Starting address of the location where the data is to be read
// Parameter2 Length of the data to be read (2 bytes in this case)
int dxl_read_word( int id, int address )
{
// wait for the bus to be free
while(giBusUsing);
// create a READ instruction packet and send
gbInstructionPacket[ID] = (unsigned char)id;
gbInstructionPacket[INSTRUCTION] = INST_READ;
gbInstructionPacket[PARAMETER] = (unsigned char)address;
gbInstructionPacket[PARAMETER+1] = 2;
gbInstructionPacket[LENGTH] = 4;
dxl_txrx_packet();
// combine the 2 bytes into a word and return
return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]);
}
////////// utility methods for value ////////////
int Dynamixel::makeWord( int lowbyte, int highbyte ){
return dxl_makeword( lowbyte, highbyte );
}
