Пример #1
0
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
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");
}
Пример #3
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;
}
Пример #4
0
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;
}