void Broadcast()
{
unsigned char param[Amount*(1+4)];
for(int i = 0; i < Amount; i++)
{
DXL.WriteByte(Id[i], P_TORQUE_ENABLE, 1, 0);
}
int goal=0;
int GoalPos;
while(1)
{
printf( "Press any key to continue!(press ESC to quit)\n" );
if(_getch() == 0x1b)
break;
do
{
// Make syncwrite packet
for(int i = 0; i < Amount; i++)
{
printf("for id=%d,input goalpos\n",Id[i]);
scanf("%d",&goal);
GoalPos = (int)(goal);
param[i*(1+4)+0] = (unsigned char)Id[i];
param[i*(1+4)+1] = DXL_LOBYTE(DXL_LOWORD(GoalPos));
param[i*(1+4)+2] = DXL_HIBYTE(DXL_LOWORD(GoalPos));
param[i*(1+4)+3] = DXL_LOBYTE(DXL_HIWORD(GoalPos));
param[i*(1+4)+4] = DXL_HIBYTE(DXL_HIWORD(GoalPos));
}
printf("%d\n", GoalPos);
int result = DXL.SyncWrite(P_GOAL_POSITION_LL, 4, param, Amount*(1+4));
if( result != COMM_RXSUCCESS )
{
printf("fail to open serial port!\n");//PrintCommStatus(result);
break;
}
usleep(CONTROL_PERIOD*1000);
}while(1);
}
}
int main()
{
int id[NUM_ACTUATOR];
float phase[NUM_ACTUATOR];
float theta = 0;
int AmpPos = 150000;
int GoalPos;
int i;
int result;
// Initialize id and phase
for( i=0; i < NUM_ACTUATOR; i++ )
{
id[i] = i+1;
phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR;
}
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;
}
unsigned char param[NUM_ACTUATOR*(1+4)];
DXL.WriteByte(1, P_TORQUE_ENABLE, 1, 0);
DXL.WriteByte(2, P_TORQUE_ENABLE, 1, 0);
while(1)
{
printf( "Press any key to continue!(press ESC to quit)\n" );
if(_getch() == 0x1b)
break;
theta = 0;
do
{
// Make syncwrite packet
for(i = 0; i < NUM_ACTUATOR; i++)
{
GoalPos = (int)((sin(theta+phase[i])) * (double)AmpPos);
param[i*(1+4)+0] = (unsigned char)id[i];
param[i*(1+4)+1] = DXL_LOBYTE(DXL_LOWORD(GoalPos));
param[i*(1+4)+2] = DXL_HIBYTE(DXL_LOWORD(GoalPos));
param[i*(1+4)+3] = DXL_LOBYTE(DXL_HIWORD(GoalPos));
param[i*(1+4)+4] = DXL_HIBYTE(DXL_HIWORD(GoalPos));
}
printf("%d\n", GoalPos);
result = DXL.SyncWrite(P_GOAL_POSITION_LL, 4, param, NUM_ACTUATOR*(1+4));
if( result != COMM_RXSUCCESS )
{
PrintCommStatus(result);
break;
}
theta += STEP_THETA;
usleep(CONTROL_PERIOD*1000);
}while(theta < 2*PI);
}
DXL.Disconnect();
printf( "Press any key to terminate...\n" );
_getch();
return 0;
}
