void Write(int id, int addr, long value, int len)
{
int result = COMM_TXFAIL, error = 0;
if(len == 1)
result = DXL.WriteByte(id, addr, (int)value, &error);
else if(len == 2)
result = DXL.WriteWord(id, addr, (int)value, &error);
else if(len == 4)
result = DXL.WriteDWord(id, addr, value, &error);
if(result != COMM_RXSUCCESS)
{
fprintf(stderr, "\n Fail to write! \n\n");
return;
}
if(error != 0)
PrintErrorCode(error);
fprintf(stderr, "\n Success to write! \n\n");
}
int main()
{
int PresentPos = 0;
int PresentVel = 0;
int index = 0, result = COMM_TXFAIL, error = 0, Moving = 1;
int GoalPos[2] = {-125700, 125700};
int GoalVel[2] = {-8000, 8000};
Dynamixel DXL("/dev/ttyUSB0");
// Open device
if( DXL.Connect() == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press any key to terminate...\n" );
_getch();
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
if(DXL.SetBaudrate(DEFAULT_BAUDNUM) == true)
{
printf( "Succeed to change the baudrate!\n" );
}
else
{
printf( "Failed to change the baudrate!\n" );
printf( "Press any key to terminate...\n" );
_getch();
return 0;
}
result = DXL.WriteByte(DEFAULT_ID, P_TORQUE_ENABLE, 1, &error);
if(result == COMM_RXSUCCESS)
{
PrintErrorCode(error);
}
else
PrintCommStatus(result);
while(1)
{
printf("Definir Velocidad (-8000,8000) \r\n");
cin>>GoalVel[0];
printf( "Presione Enter para continuar!(presione ESC y Enter para salir)\n" );
if(_getch() == 0x1b)
break;
// Write goal position
//DXL.WriteDWord( DEFAULT_ID, P_GOAL_POSITION_LL, GoalPos[index], &error);
// Write goal Speed
result = DXL.WriteDWord( DEFAULT_ID, P_GOAL_VELOCITY_LL, GoalVel[index], &error);
if(result == COMM_RXSUCCESS)
{
PrintErrorCode(error);
}
else
PrintCommStatus(result);
// Change goal position
/* for(int i=0;i<1;i++)
{
// Read present position
//result = DXL.ReadDWord(DEFAULT_ID, P_PRESENT_POSITION_LL, (long*) &PresentPos, &error);
// Read present Velocity
result = DXL.ReadDWord(DEFAULT_ID, P_PRESENT_VELOCITY_LL, (long*) &PresentVel, &error);
if( result == COMM_RXSUCCESS )
{
printf( "%d %d\n", GoalVel[index], PresentVel );
PrintErrorCode(error);
}
else
{
PrintCommStatus(result);
break;
}
}//while(Moving == 1);*/
// Change goal position
}
// Close device
DXL.Disconnect();
printf( "Press Enter key to terminate...\n" );
_getch();
return 0;
}