Robotis Dynamixel Pro, Quick Start Manual

Page: 104 / 140

Quick Start for Dynamixel Pro v1.00b
Page 104 / 139
if (dxl_initialize(Port, COM_PORT_NUM, BAUD_RATE_NUM) ==
COMM_RXSUCCESS )
printf( "Succeed to open USB2Dynamixel!\n" );
else
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press any key to terminate...\n" );
_getch();
return 0;
}
int Result;
int position_length=4;
unsigned char param[15];
param[0] = ID_1;
param[1] = DXL_LOBYTE(P_PRESENT_POSITION);
param[2] = DXL_HIBYTE(P_PRESENT_POSITION);
param[3] = DXL_LOBYTE(position_length);
param[4] = DXL_HIBYTE(position_length);
param[5] = ID_2;
param[6] = DXL_LOBYTE(P_PRESENT_POSITION);
param[7] = DXL_HIBYTE(P_PRESENT_POSITION);
param[8] = DXL_LOBYTE(position_length);
param[9] = DXL_HIBYTE(position_length);
param[10] = ID_3;
param[11] = DXL_LOBYTE(P_PRESENT_POSITION);
param[12] = DXL_HIBYTE(P_PRESENT_POSITION);
param[13] = DXL_LOBYTE(position_length);
param[14] = DXL_HIBYTE(position_length);
BulkData bd[256];
BulkData *pbd[256];
for ( unsigned int i = 0 ; i <256 ; i++)
{
pbd[i] = &bd[i];
}
dxl_bulk_read(Port, param, 15, pbd);
int present_position1;
int present_position2;
int present_position3;
dxl_get_bulk_dword(pbd, ID_1, P_PRESENT_POSITION, ( unsigned *)&present_position1);
dxl_get_bulk_dword(pbd, ID_2, P_PRESENT_POSITION, ( unsigned *)&present_position2);
dxl_get_bulk_dword(pbd, ID_3, P_PRESENT_POSITION, ( unsigned *)&present_position3);
printf( "Present Position\n" );