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