示例#1
0
文件: monitor.cpp 项目: Yunaik/thu
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);
    }
}
示例#2
0
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;
}