Robotis Dynamixel Pro, Quick Start Manual

Page: 96 / 140

Quick Start for Dynamixel Pro v1.00b
Page 96 / 139
Result = dxl_write_byte(Port, ID, P_TORQUE_ENABLE, 1, &ErrorStatus);
if ( Result != COMM_RXSUCCESS )
{
printf( "Failed to write!\n" );
printf( "Press any key to terminate...\n" );
_getch();
return 0;
}
//Rotating Start
printf( "Rotating Start\n" );
Result = dxl_write_dword(Port, ID, 600, 5000, &ErrorStatus);
if ( Result != COMM_RXSUCCESS )
{
printf( "Failed to write!\n" );
printf( "Press any key to terminate...\n" );
_getch();
return 0;
}
printf( "Press any key to terminate...\n" );
printf( "\n" );
while ( true )
{
if (_kbhit())
break ;
unsigned char data[5];
dxl_read(Port, ID, P_INDIRECT_DATA_0, 5, data, &ErrorStatus);
int temp, present_position;
temp = data[0];
present_position = DXL_MAKEDWORD( DXL_MAKEWORD(data[1], data[2]),
DXL_MAKEWORD(data[3], data[4]) );
printf( "\r" );
printf( "present temperature : %d, presen position : %d" , temp, present_position);
}
printf( "\n" );
//Close the port of USB2DXL
dxl_terminate(Port);
return 0;
}