Ejemplo n.º 1
0
void Read(int id, int addr, int len)
{
	int result = COMM_TXFAIL, error = 0;
	int ivalue = 0;
	long lvalue = 0;

	if(len == 1)
		result = DXL.ReadByte(id, addr, &ivalue, &error);
	else if(len == 2)
		result = DXL.ReadWord(id, addr, &ivalue, &error);
	else if(len == 4)
		result = DXL.ReadDWord(id, addr, &lvalue, &error);

	if(result != COMM_RXSUCCESS)
	{
		fprintf(stderr, "\n Fail to read! (result : %d) \n\n", result);
		return;
	}

	if(error != 0)
		PrintErrorCode(error);

	if(len == 1)
		fprintf(stderr, "\n READ VALUE : %d \n\n", ivalue);
	else if(len == 2)
		fprintf(stderr, "\n READ VALUE : (UNSIGNED) %u , (SIGNED) %d \n\n", ivalue, ivalue);
	else
		fprintf(stderr, "\n READ VALUE : (UNSIGNED) %lu , (SIGNED) %d \n\n", lvalue, lvalue);
}
Ejemplo n.º 2
0
void xlMotor::jointMove(int position, int speed)
{
    Dxl.jointMode(myID);
    delayMicroseconds(100);
    Dxl.setPosition(myID,position,speed);
    delayMicroseconds(1);
}
Ejemplo n.º 3
0
void xlMotor::jointMove(int position)
{
    Dxl.jointMode(myID);
    delayMicroseconds(1);
    Dxl.setPosition(myID,position,512);
    delayMicroseconds(1);
}
Ejemplo n.º 4
0
void xlMotor::wheelMove(char direction,int speed)
{
    if (direction=='F') {
        Dxl.goalSpeed(myID, speed);
    }
    else if(direction=='R')
    {
        Dxl.goalSpeed(myID, speed| 0x400);
    }
   
    
}
Ejemplo n.º 5
0
void xlMotor::wheelMove(char direction)
{
    Dxl.wheelMode(myID);
    if (direction=='F')
	{
        Dxl.goalSpeed(myID, 512);
    }
    else if(direction=='R')
	{
        Dxl.goalSpeed(myID, 512| 0x400);
    }
    else if(direction=='S')
    {
        Dxl.goalSpeed(myID, 0);
    }
    
}
Ejemplo n.º 6
0
void xlMotor::resetSettings(void){
	xlMotor::setBaud(3);
	xlMotor::setReturnDelay(250);
    Dxl.jointMode(myID);
	xlMotor::setTempLimit(70);
	xlMotor::setStatusReturn(2);
    
}
Ejemplo n.º 7
0
int main()
{
    Dynamixel test;
    double feed;
    if(!test.Initialize("/dev/ttyUSB0",false,10.0))
    {
        return -1;
    }
    
    
    //do a sine wave
    
    double t=0;
    double val;
    while(1)
    {
        double top=(564-512)/512.0*100.0;
        double bot=(440-512)/512.0*100.0;
        
        
        
        //val=(top-bot)/2.0*sin(t/(2*M_PI))+(top+bot)/2.0;
        //test.SetServo(1,val);
        //feed=test.GetServo(1);
        //printf("%lf %lf\n",val,feed);
        //t+=0.05;
        for(double val=bot;val<=top;val+=1.0)
        {
            printf("%lf\n",val);
            test.SetServo(1,val);
            CxUtils::SleepMs(50);
        }
        for(double val=top;val>=bot;val-=1.0)
        {
            printf("%lf\n",val);
            test.SetServo(1,val);
            CxUtils::SleepMs(50);
        }
        
    }
    
    return 0;
}
int main(int argc,char** argv)
{
  ros::init(argc, argv, "c8_dynamixel");
  Dynamixel motors;

  float counter = -180;
  ros::Rate loop_rate(100);
  while(ros::ok())
  {
    if(counter < 180)
    {
      motors.moveMotor(counter*3.14/180);
      counter++;
    }else{
      counter = -180;
    }
    loop_rate.sleep();
  }
}
Ejemplo n.º 9
0
void Refresh(int id, int addr, int len)
{
	int result = COMM_TXFAIL, error = 0;
	int ivalue = 0;
	long lvalue = 0;

	fprintf(stderr, "\n [ESC] : Quit monitoring \n\n");
	while(1)
	{
		if(len == 1)
			result = DXL.ReadByte(id, addr, &ivalue, &error);
		else if(len == 2)
			result = DXL.ReadWord(id, addr, &ivalue, &error);
		else if(len == 4)
			result = DXL.ReadDWord(id, addr, &lvalue, &error);

		if(result != COMM_RXSUCCESS)
		{
			//fprintf(stderr, "\n Fail to read! (result : %d) \n\n", result);
			continue;
		}

		if(error != 0)
			PrintErrorCode(error);

		if(len == 1)
			fprintf(stderr, " READ VALUE : %.3d [0x%.2X] \r", ivalue, ivalue);
		else if(len == 2)
			fprintf(stderr, " READ VALUE : %.5d [0x%.2X 0x%.2X] \r", ivalue, DXL_LOBYTE(ivalue), DXL_HIBYTE(ivalue));
		else
			fprintf(stderr, " READ VALUE : %.10ld [0x%.2X 0x%.2X 0x%.2X 0x%.2X] \r",
					lvalue, DXL_LOBYTE(DXL_LOWORD(lvalue)), DXL_HIBYTE(DXL_LOWORD(lvalue)), DXL_LOBYTE(DXL_HIWORD(lvalue)), DXL_HIBYTE(DXL_HIWORD(lvalue)));

		if(kbhit())
		{
			char c = _getch();
			if(c == 0x1b)
				break;
		}
	}
	fprintf(stderr, "\n\n");
}
Ejemplo n.º 10
0
int Instruction::moveWithSpeed(std::vector<instruction_set> instruction_list){
	SerialPortF comPort;
	Dynamixel communication;
	hr = comPort.Open(L"COM1", baudRate);
	if(FAILED(hr))
	{
		printf("Cannot open COM1 port\n");
		return -1;
	}else{
		//printf("Starting communication\n");
	}

	communication.SendTossModeCommand(&comPort);
	for(int indx = 0;  indx < instruction_list.size(); indx++){
		communication.SetDynamixelMovingSpeed(&comPort, instruction_list[indx].id_motor, instruction_list[indx].value, instruction_list[indx].speed);	
	}

	comPort.Close();
	return 0;
}
Ejemplo n.º 11
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);
    }
}
Ejemplo n.º 12
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");
}
Ejemplo n.º 13
0
void BeltTest()
{
	int result;
	long lvalue1 , lvalue2;

	while(true)
	{
		if(kbhit())
			break;

		result = DXL.ReadDWord(17, 611, &lvalue1, 0);
		result = DXL.ReadDWord(18, 611, &lvalue2, 0);
		fprintf(stderr, "\n READ VALUE : %d , %d \n\n", lvalue1, lvalue2);
	}
}
Ejemplo n.º 14
0
int main()
{
    
    int i;

    Dynamixel DXL("/dev/ttyUSB0");
    //BulkRead bulkread(&DXL);


    // Open device
    if( DXL.Connect() == 0 )
    {
        printf( "Failed to open USB2Dynamixel!\n" );
        return 0;
    }
    else
        printf( "Succeed to open USB2Dynamixel!\n" );

	//Close port of USB2DXL
    DXL.Disconnect();
   

    return 0;
}
Ejemplo n.º 15
0
void BroadcastPing()
{
	std::vector<PingInfo> vec_info = std::vector<PingInfo>();
	DXL.BroadcastPing(vec_info);

	if(vec_info.size() > 0)
	{
		Amount=vec_info.size();
		fprintf(stderr, "\n");
		for(int i = 0; i < (int)vec_info.size(); i++)
		{
			fprintf(stderr, "                                          ... SUCCESS \r");
			fprintf(stderr, " [ID:%.3d] Model No : %.5d (0x%.2X 0x%.2X) \n",
					vec_info.at(i).ID, vec_info.at(i).ModelNumber, DXL_LOBYTE(vec_info.at(i).ModelNumber), DXL_HIBYTE(vec_info.at(i).ModelNumber));
			Id[i]=vec_info.at(i).ID;
		}
		fprintf(stderr, "\n");
	}
}
Ejemplo n.º 16
0
void Dump(int id, int addr, int len)
{
	int result = COMM_TXFAIL, error = 0;
	unsigned char* data = (unsigned char*)calloc(len, sizeof(unsigned char));

	result = DXL.Read(id, addr, len, data, &error);
	if(result != COMM_RXSUCCESS)
	{
		fprintf(stderr, "\n Fail to read! (result : %d) \n\n", result);
		return;
	}

	if(error != 0)
		PrintErrorCode(error);

	if(id != BROADCAST_ID)
	{
		fprintf(stderr, "\n");
		for(int i = addr; i < addr+len; i++)
			fprintf(stderr, "ADDR %.3d [0x%.4X] :     %.3d [0x%.2X] \n", i, i, data[i-addr], data[i-addr]);
		fprintf(stderr, "\n");
	}
}
Ejemplo n.º 17
0
void Scan()
{
	fprintf(stderr, "\n");
	PingInfo *data = new PingInfo();
	for(int i = 1; i < 253; i++)
	{
		if(DXL.Ping(i, data, 0) == COMM_RXSUCCESS)
		{
			fprintf(stderr, "\n                                          ... SUCCESS \r");
			fprintf(stderr, " [ID:%.3d] Model No : %.5d (0x%.2X 0x%.2X) \n",
					data->ID, data->ModelNumber, DXL_LOBYTE(data->ModelNumber), DXL_HIBYTE(data->ModelNumber));
		}
		else
			fprintf(stderr, ".");

		if(kbhit())
		{
			char c = _getch();
			if(c == 0x1b)
				break;
		}
	}
	fprintf(stderr, "\n\n");
}
Ejemplo n.º 18
0
int main(int argc, char *argv[])
{
    fprintf(stderr, "\n***********************************************************************\n");
    fprintf(stderr,   "*                     DXL Protocol 2.0 Monitor                        *\n");
    fprintf(stderr,   "***********************************************************************\n\n");

    char *dev = (char*)"/dev/ttyUSB0";

    /* parameter parsing */
    while(1)
    {
    	int option_index = 0, c = 0;
    	static struct option long_options[] = {
                {"h", no_argument, 0, 0},
                {"help", no_argument, 0, 0},
                {"d", required_argument, 0, 0},
                {"device", required_argument, 0, 0},
                {0, 0, 0, 0}
    	};

        /* parsing all parameters according to the list above is sufficent */
        c = getopt_long_only(argc, argv, "", long_options, &option_index);

        /* no more options to parse */
        if(c == -1) break;

        /* unrecognized option */
        if(c == '?') {
        	Usage(argv[0]);
            return 0;
        }

        /* dispatch the given options */
        switch(option_index) {
        /* h, help */
        case 0:
        case 1:
        	Usage(argv[0]);
            return 0;
            break;

            /* d, device */
        case 2:
        case 3:
        	Usage(argv[0]);
            dev = strdup(optarg);
            DXL.ComPort->SetPortName(dev);
            break;

        default:
        	Usage(argv[0]);
            return 0;
        }
    }

    if(DXL.Connect() == false)
    {
    	fprintf(stderr, " Fail to open USB2Dyanmixel! [%s] \n\n", dev);
    	return 0;
    }

    char input[128];
    char cmd[80];
    char param[20][30];
    int num_param;
    char* token;
    while(1)
    {
    	printf("[CMD] ");
    	gets(input);
    	fflush(stdin);

    	if(strlen(input) == 0)
    		continue;

    	token = strtok(input, " ");
    	if(token == 0)
    		continue;

    	strcpy(cmd, token);
    	token = strtok(0, " ");
    	num_param = 0;
    	while(token != 0)
    	{
    		strcpy(param[num_param++], token);
    		token = strtok(0, " ");
    	}

    	if(strcmp(cmd, "help") == 0 || strcmp(cmd, "h") == 0 || strcmp(cmd, "?") == 0)
    	{
    		Help();
    	}
    	else if(strcmp(cmd, "ping") == 0)
    	{
    		if(num_param == 0)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		fprintf(stderr, "\n");
			PingInfo *data = new PingInfo();
    		for(int i = 0; i < num_param; i++)
    		{
    			if(DXL.Ping(atoi(param[i]), data, 0) == COMM_RXSUCCESS)
    			{
        			fprintf(stderr, "                                          ... SUCCESS \r");
        			fprintf(stderr, " [ID:%.3d] Model No : %.5d (0x%.2X 0x%.2X) \n",
        					data->ID, data->ModelNumber, DXL_LOBYTE(data->ModelNumber), DXL_HIBYTE(data->ModelNumber));
    			}
    			else
    			{
        			fprintf(stderr, "                                          ... FAIL \r");
        			fprintf(stderr, " [ID:%.3d] \n", atoi(param[i]));
    			}
    		}
    		fprintf(stderr, "\n");
    	}
    	else if(strcmp(cmd, "scan") == 0)
    	{
    		Scan();
    	}
	else if(strcmp(cmd, "broadcast") == 0)
	{
		Broadcast();
	}
    	else if(strcmp(cmd, "baud") == 0)
    	{
    		if(num_param != 1)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		if(DXL.SetBaudrate(atoi(param[0])) == false)
    			fprintf(stderr, " Failed to change baudrate! \n");
    		else
    			fprintf(stderr, " Success to change baudrate! [ BAUD NUM: %d ]\n", atoi(param[0]));
    	}
    	else if(strcmp(cmd, "wrb") == 0 || strcmp(cmd, "w") == 0)
    	{
    		if(num_param != 3)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		Write(atoi(param[0]), atoi(param[1]), atoi(param[2]), 1);
    	}
    	else if(strcmp(cmd, "wrw") == 0)
    	{
    		if(num_param != 3)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		Write(atoi(param[0]), atoi(param[1]), atoi(param[2]), 2);
    	}
    	else if(strcmp(cmd, "wrd") == 0)
    	{
    		if(num_param != 3)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		Write(atoi(param[0]), atoi(param[1]), atol(param[2]), 4);
    	}
    	else if(strcmp(cmd, "rdb") == 0)
    	{
    		if(num_param != 2)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		Read(atoi(param[0]), atoi(param[1]), 1);
    	}
    	else if(strcmp(cmd, "rdw") == 0)
    	{
    		if(num_param != 2)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		Read(atoi(param[0]), atoi(param[1]), 2);
    	}
    	else if(strcmp(cmd, "rdd") == 0)
    	{
    		if(num_param != 2)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		Read(atoi(param[0]), atoi(param[1]), 4);
    	}
    	else if(strcmp(cmd, "r") == 0)
    	{
    		if(num_param != 3)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		Dump(atoi(param[0]), atoi(param[1]), atoi(param[2]));
    	}
    	else if(strcmp(cmd, "mon") == 0)
    	{
    		int len = 0;

    		if(num_param > 2 && strcmp(param[2],"b") == 0) len = 1;
    		else if(num_param > 2 && strcmp(param[2],"w") == 0) len = 2;
    		else if(num_param > 2 && strcmp(param[2],"d") == 0) len = 4;

    		if(num_param != 3 || len == 0)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		Refresh(atoi(param[0]), atoi(param[1]), len);
    	}
    	else if(strcmp(cmd, "bp") == 0)
    	{
    		if(num_param != 0)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		BroadcastPing();
    	}
    	else if(strcmp(cmd, "reboot") == 0)
    	{
    		if(num_param != 1)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		int result = DXL.Reboot(atoi(param[0]), 0);
    		if(result != COMM_RXSUCCESS)
    			fprintf(stderr, "\n Fail to reboot! \n\n");
    		else
    			fprintf(stderr, "\n Success to reboot! \n\n");
    	}
    	else if(strcmp(cmd, "reset") == 0)
    	{
    		if(num_param != 2)
    		{
    			fprintf(stderr, " Invalid parameters! \n");
    			continue;
    		}

    		int result = DXL.FactoryReset(atoi(param[0]), atoi(param[1]), 0);
    		if(result != COMM_RXSUCCESS)
    			fprintf(stderr, "\n Fail to reset! \n\n");
    		else
    			fprintf(stderr, "\n Success to reset! \n\n");
    	}
    	else if(strcmp(cmd, "exit") == 0)
    	{
    		DXL.Disconnect();
    		return 0;
    	}
    }
}
Ejemplo n.º 19
0
int xlMotor::readVoltage(void){
	return Dxl.readByte(myID, 45);
}
Ejemplo n.º 20
0
void xlMotor::unlock(void){
	Dxl.writeByte(myID, 47, 0);
}
Ejemplo n.º 21
0
void xlMotor::lock(void){
	Dxl.writeByte(myID, 47, 1);
}
Ejemplo n.º 22
0
void xlMotor::setPunch(int newPunch){
	Dxl.writeWord(myID, 48, newPunch);
}
Ejemplo n.º 23
0
int xlMotor::isError(void){
	return Dxl.readByte(myID, 18);
}
Ejemplo n.º 24
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;
}
Ejemplo n.º 25
0
int xlMotor::isItMoving(void){
	return Dxl.readByte(myID, 49);
}
Ejemplo n.º 26
0
int xlMotor::readTemperature(void){
	return Dxl.readByte(myID, 46);
}
Ejemplo n.º 27
0
int xlMotor::isItWorking(void){
	return Dxl.readByte(myID, 47);
}
Ejemplo n.º 28
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;
}
Ejemplo n.º 29
-1
void setup() {
	pinMode(LED_PIN, OUTPUT);

	Serial.begin(9600);

	dyn.begin(config::dynamixel::baud_rate);
	wrist_left.init();
	wrist_right.init();
}
Ejemplo n.º 30
-1
void loop() {
	static long p_time = 0;

	while (Serial.available()) {
		Serial.print((char) Serial.read());
		Serial.flush();
	}

	long c_time = millis();
	if (c_time - p_time > 500) {
		p_time = c_time;
		digitalWrite(LED_PIN, !digitalRead(LED_PIN));
		Serial.print("Dynamixel 1: ");
		Serial.print(wrist_left.get_voltage());
		Serial.print(" Dynamixel 2: ");
		Serial.print(wrist_right.get_voltage());
		Serial.println();
		Serial.flush();
	}
}