Example #1
0
//------------------------------------------------------------------------------
// 
//  Data Format for SYNC write:
//  			ID		0xFE
//			Length		((L+1)*N )+4  	L: Data Length per RX-28, N: Number of motors
//			Instruction	0x83
//			Start address to write data	parameter 1
//			Length of data to write		parameter 2 = Number of motors
//			
//			First ID of RX-28		parameter 3
//			First data of the 1st RX-28
//			..
//			Lth data of the 1st RX-28 
//
//     #define ID				(2)
//     #define LENGTH				(3)
//     #define INSTRUCTION			(4)
//     #define ERRBIT				(4)
//     #define PARAMETER			(5)
//     #define DEFAULT_BAUDNUMBER		(1)
//
int16_t MOTOR_SendPositionCommandsToMotors(void)
{
  int16_t i;
  int16_t numJointsSent = 0;
  
  dxl_set_txpacket_id(BROADCAST_ID);
  dxl_set_txpacket_instruction(INST_SYNC_WRITE);
  dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
  dxl_set_txpacket_parameter(1, 4); // Write 4 bytes
  
  for( i=0; i<MOTOR_NJOINTS; i++ )
  {
      if ( !joint[i].bPositionSet )
      {
          // Don't send a joint value if we haven't got a command
          continue;
      }
      
	  dxl_set_txpacket_parameter(2+(5*numJointsSent), i);
	  dxl_set_txpacket_parameter(2+(5*numJointsSent)+1, dxl_get_lowbyte(joint[i].pos));
	  dxl_set_txpacket_parameter(2+(5*numJointsSent)+2, dxl_get_highbyte(joint[i].pos));
	  dxl_set_txpacket_parameter(2+(5*numJointsSent)+3, dxl_get_lowbyte(joint[i].speed));
	  dxl_set_txpacket_parameter(2+(5*numJointsSent)+4, dxl_get_highbyte(joint[i].speed));
      
      numJointsSent++;
  }
  
  dxl_set_txpacket_length((5*numJointsSent)+4);
  
  dxl_txrx_packet();
  return MOTOR_SUCCESS;
}
Example #2
0
bool ControlUtils::syncWriteWord(int8_t addr, const std::vector<int> &joints, const std::vector<int16_t> &val)
{
  if (joints.size() != val.size())
    return false;
  
  if (joints.empty())
    return true;

  // Make syncwrite packet
  dxl_set_txpacket_id(BROADCAST_ID);
  dxl_set_txpacket_instruction(INST_SYNC_WRITE);
  dxl_set_txpacket_parameter(0, addr);
  dxl_set_txpacket_parameter(1, 2);
  for (size_t i = 0; i < joints.size(); i++) {
    dxl_set_txpacket_parameter(2+3*i, _id[joints[i]]);
    
    dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(val[i]));
    dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(val[i]));
  }
  dxl_set_txpacket_length((2+1)*joints.size()+4);

  dxl_txrx_packet();
  if(dxl_get_result() == COMM_RXSUCCESS)
    return true;
  else
    return false; 
}
Example #3
0
void motor_sync_move(const uint8_t size, const uint8_t * id, const uint16_t * position, const char blocking) {
	volatile int i, CommStatus;
	dxl_set_txpacket_id(MOTOR_BROADCAST_ID);		//set broadcast id 
	dxl_set_txpacket_instruction(INST_SYNC_WRITE);	//set instruction type
	dxl_set_txpacket_parameter(0, GOAL_POSITION_L); //memory area to write
	dxl_set_txpacket_parameter(1, 2); //length of the data
	
	for(i=0;i<size;i++){
		dxl_set_txpacket_parameter(2+(3*i), id[i]);  //id
		dxl_set_txpacket_parameter(2+(3*i)+1, dxl_get_lowbyte(position[i]));//low byte
		dxl_set_txpacket_parameter(2+(3*i)+2, dxl_get_highbyte(position[i]));//high byte
	}
	
	dxl_set_txpacket_length((2+1)*size+4);		//set packet length
	dxl_txrx_packet();			//transmit packet
	CommStatus = dxl_get_result();	//get transmission state
	if( CommStatus == COMM_RXSUCCESS ){	//transmission succeded
		PrintErrorCode();					//show potentiol motors error (overload, overheat,etc....)
		if (blocking == MOTOR_MOVE_BLOCKING) //blocking function requested
		{
			// Wait for finish of all motors
			for (i = 0; i < size; i++)
				motor_wait_finish(id[i], position[i]);
		}			
	}		
	else							//communication failed
		PrintCommStatus(CommStatus);	//show communication error
}
Example #4
0
void dxl_SetPosition(int ServoID, int Position, int Speed)
    //    Sets the target position and speed of the specified servo
	////Made by Martin S. Mason(Professor @Mt. San Antonio College)
{
    while(giBusUsing);

    gbInstructionPacket[ID] = (unsigned char)ServoID;
    gbInstructionPacket[INSTRUCTION] = INST_WRITE;
    gbInstructionPacket[PARAMETER] = (unsigned char)30;
    gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(Position);
    gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(Position);
    gbInstructionPacket[PARAMETER+3] = (unsigned char)dxl_get_lowbyte(Speed);
    gbInstructionPacket[PARAMETER+4] = (unsigned char)dxl_get_highbyte(Speed);
    gbInstructionPacket[LENGTH] = 7;

    dxl_txrx_packet();
}
void getVoltage(void){
	int tmp[SERVO_MAX] = {0};
	for(int i=0; i<SERVO_MAX; i++ ){
		tmp[i] = dxl_get_lowbyte(dxl_read_word( servoId[i], P_PRESENT_VOLTAGE ));
	}
	
	printf( "%d:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
	EVT_GET_VOLTAGE, tmp[0],tmp[1],tmp[2],tmp[3],tmp[4],tmp[5],tmp[6],tmp[7],tmp[8],tmp[9],tmp[10],tmp[11] );
}
Example #6
0
// Function setting goal and speed for all Dynamixel actuators at the same time  
// Uses the Sync Write instruction (also see dxl_sync_write_word) 
// Inputs:	NUM_ACTUATOR - number of Dynamixel servos
//			ids - array of Dynamixel ids to write to
//			goal - array of goal positions
//			speed - array of moving speeds
//Returns:	commStatus
int dxl_set_goal_speed( int NUM_ACTUATOR, const uint8 ids[], uint16 goal[], uint16 speed[] )
{
	int i = 0;

	// wait for the bus to be free
	while(giBusUsing);

	// check how many actuators are to be broadcast to
	if (NUM_ACTUATOR == 0) {
		// nothing to do, return
		return 0;
	} 
	
	// Multiple values, create sync write packet
	// ID is broadcast id
	dxl_set_txpacket_id(BROADCAST_ID);
	// Instruction is sync write
	dxl_set_txpacket_instruction(INST_SYNC_WRITE);
	// Starting address where to write to
	dxl_set_txpacket_parameter(0, DXL_GOAL_POSITION_L);
	// Length of data to be written (2 words = 4 bytes)
	dxl_set_txpacket_parameter(1, 4);
	// Loop over the active Dynamixel id's  
	for( i=0; i<NUM_ACTUATOR; i++ )
	{
		// retrieve the id and value for each actuator and add to packet
		dxl_set_txpacket_parameter(2+5*i, ids[i]);
		dxl_set_txpacket_parameter(2+5*i+1, dxl_get_lowbyte(goal[i]));
		dxl_set_txpacket_parameter(2+5*i+2, dxl_get_highbyte(goal[i]));
		dxl_set_txpacket_parameter(2+5*i+3, dxl_get_lowbyte(speed[i]));
		dxl_set_txpacket_parameter(2+5*i+4, dxl_get_highbyte(speed[i]));
	}
	
	// total length is as per formula above with L=4
	dxl_set_txpacket_length((4+1)*NUM_ACTUATOR + 4);
	
	// all done, send the packet
	dxl_txrx_packet();
	
	// there is no status packet return, so return the CommStatus
	return gbCommStatus;
}
void dxl_sync_write_push_word( int value )
{
    while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX.	
	
    if ( gbSyncNbParam > MAXNUM_TXPARAM )
    {
        return;
    }
	
    gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)dxl_get_lowbyte(value);
	gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)dxl_get_highbyte(value);
}
void dxl_write_word( int id, int address, int value )
{
	while(giBusUsing);

	gbInstructionPacket[ID] = (unsigned char)id;
	gbInstructionPacket[INSTRUCTION] = INST_WRITE;
	gbInstructionPacket[PARAMETER] = (unsigned char)address;
	gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value);
	gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value);
	gbInstructionPacket[LENGTH] = 5;
	
	dxl_txrx_packet();
}
Example #9
0
// Function to write data into the control table of the Dynamixel actuator
// Length N+3 (N is the number of data to be written)
// Instruction 0x03
// Parameter1 Starting address of the location where the data is to be written
// Parameter2 1st data to be written
// Parameter3 2nd data to be written, etc.
// In this case we have a two 1-byte parameters
int dxl_write_word( int id, int address, int value )
{
	// wait for the bus to be free
	while(giBusUsing);

	// create a WRITE instruction packet and send
	gbInstructionPacket[ID] = (unsigned char)id;
	gbInstructionPacket[INSTRUCTION] = INST_WRITE;
	gbInstructionPacket[PARAMETER] = (unsigned char)address;
	gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value);
	gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value);
	gbInstructionPacket[LENGTH] = 5;
	
	dxl_txrx_packet();
	
	return gbCommStatus;
}
Example #10
0
// Function for controlling several Dynamixel actuators at the same time. 
// The communication time decreases by using the Sync Write instruction 
// since many instructions can be transmitted by a single instruction. 
// However, you can use this instruction only when the lengths and addresses 
// of the control table to be written to are the same. 
// The broadcast ID (0xFE) needs to be used for transmitting.
// ID: 0xFE
// Length: (L + 1) * N + 4 (L: Data length for each Dynamixel actuator, N: The number of Dynamixel actuators)
// Instruction: 0x83
// Parameter1 Starting address of the location where the data is to be written
// Parameter2 The length of the data to be written (L)
// Parameter3 The ID of the 1st Dynamixel actuator
// Parameter4 The 1st data for the 1st Dynamixel actuator
// Parameter5 The 2nd data for the 1st Dynamixel actuator
// ParameterL+4 The ID of the 2nd Dynamixel actuator
// ParameterL+5 The 1st data for the 2nd Dynamixel actuator
// ParameterL+6 The 2nd data for the 2nd Dynamixel actuator
// …
// NOTE: this function only allows 2 bytes of data per actuator
int dxl_sync_write_word( int NUM_ACTUATOR, int address, const uint8 ids[], int16 values[] )
{
	int i = 0;

	// wait for the bus to be free
	while(giBusUsing);

	// check how many actuators are to be broadcast to
	if (NUM_ACTUATOR == 0) {
		// nothing to do, return
		return 0;
	} else if (NUM_ACTUATOR == 1) {
		// easy, we can use dxl_write_word for a single actuator
		dxl_write_word( ids[0], address, values[0] );
		return 0;
	}
	
	// Multiple values, create sync write packet
	// ID is broadcast id
	dxl_set_txpacket_id(BROADCAST_ID);
	// Instruction is sync write
	dxl_set_txpacket_instruction(INST_SYNC_WRITE);
	// Starting address where to write to
	dxl_set_txpacket_parameter(0, address);
	// Length of data to be written (each word = 2 bytes)
	dxl_set_txpacket_parameter(1, 2);
	// Loop over the active Dynamixel id's  
	for( i=0; i<NUM_ACTUATOR; i++ )
	{
		// retrieve the id and value for each actuator and add to packet
		dxl_set_txpacket_parameter(2+3*i, ids[i]);
		dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(values[i]));
		dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(values[i]));
	}
	
	// total length is as per formula above with L=2
	dxl_set_txpacket_length((2+1)*NUM_ACTUATOR + 4);
	
	// all done, send the packet
	dxl_txrx_packet();
	
	// there is no status packet return, so return the CommStatus
	return gbCommStatus;
}
void MotorControl( int id, int power ){
	int CommStatus;
//	printf( "%d %d\n", id, power );
//	dxl_write_word( id, P_GOAL_SPEED_L, power );
	if(1){
		dxl_set_txpacket_id(BROADCAST_ID);
		dxl_set_txpacket_instruction(INST_SYNC_WRITE);
		dxl_set_txpacket_parameter(0, P_GOAL_SPEED_L);
		dxl_set_txpacket_parameter(1, 2);
		dxl_set_txpacket_parameter(2, id);
		dxl_set_txpacket_parameter(3, dxl_get_lowbyte(power));
		dxl_set_txpacket_parameter(4, dxl_get_highbyte(power));
		dxl_set_txpacket_length(4+3*1);
		dxl_txrx_packet();
		CommStatus = dxl_get_result();
		if( CommStatus == COMM_RXSUCCESS )
			PrintErrorCode();
		else
			PrintCommStatus(CommStatus);
	}
}
Example #12
0
int main(void)
{
	int id[NUM_ACTUATOR];
	float phase[NUM_ACTUATOR];
	float theta = 0;
	int AmpPos = 512;
	//int AmpPos = 2048; // for EX series
	int GoalPos;
	int i;
	int CommStatus;

	serial_initialize(57600);
	dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
	sei();	// Interrupt Enable	
	
	printf( "\n\nSyncWrite example for CM-700\n\n" );
		
	for( i=0; i<NUM_ACTUATOR; i++ )
	{
		id[i] = i+1;
		phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR;
	}
	
	// Set goal speed
	dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
	// Set goal position
	dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos );
	_delay_ms(1000);
	
	while(1)
	{
		// Make syncwrite packet
		dxl_set_txpacket_id(BROADCAST_ID);
		dxl_set_txpacket_instruction(INST_SYNC_WRITE);
		dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
		dxl_set_txpacket_parameter(1, 2);
		for( i=0; i<NUM_ACTUATOR; i++ )
		{
			dxl_set_txpacket_parameter(2+3*i, id[i]);
			GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (float)AmpPos);
			printf( "%d  ", GoalPos );
			dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos));
			dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos));
		}
		dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4);
		
		printf( "\n" );
		
		dxl_txrx_packet();
		CommStatus = dxl_get_result();
		if( CommStatus == COMM_RXSUCCESS )
			PrintErrorCode();
		else
			PrintCommStatus(CommStatus);
			
		theta += STEP_THETA;

		if( theta > 2*PI )
			theta -= 2*PI;
		_delay_ms(CONTROL_PERIOD);
	}

	return 0;
}
int main(void)
{
#if 0
	int id[NUM_ACTUATOR];
	float phase[NUM_ACTUATOR];
	float theta = 0;
	int AmpPos = 512;
	//int AmpPos = 2048; // for EX series
	int GoalPos;
	int i;
	int CommStatus;
	int isPress = 0;
	int isOn = 0;
	unsigned char ReceivedData;
	int Value;
	mServoList[0] = (stServo *)malloc(sizeof(stServo));
	memset((void *)mServoList[0], 0x00, sizeof(stServo) );
	mServoList[0]->id = 4;

	serial_initialize(57600);
	dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
	sei();	// Interrupt Enable
	
	printf( "\n\nSyncWrite example for CM-700\n\n" );
	
#ifdef MODE_SYNC
	for( i=0; i<NUM_ACTUATOR; i++ )
	{
		id[i] = i+2;
		phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR;
	}
#else
	int wPresentPos = 512;
#endif	
	
	//Set EEP Lock
	dxl_write_word( BROADCAST_ID, P_EEP_LOCK, 1 );
	// Set goal speed
	dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
	// Set goal position
	dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos );
	dxl_write_word( 4, P_TORQUE_LIMIT_L, 0);
	_delay_ms(1000);
	
	while(1)
	{
		if(~PIND & SW_START){
			isPress = 1;
		}else{
 		    if( isPress == 1 ){
				if( isOn == 0 ){
					isOn = 1;
				}else{
					isOn = 0;
				}
			}
			isPress = 0;
		}
		
//		while( ReceivedData = getchar() != NULL ){
			if(ReceivedData == 'u')
			Value++;
			else if(ReceivedData == 'd')
			Value--;
			printf("%d, %d\r\n", Value, ReceivedData);
//		}
		
		if( isOn ){
#ifdef MODE_SYNC
		// Make syncwrite packet
		dxl_set_txpacket_id(BROADCAST_ID);
		dxl_set_txpacket_instruction(INST_SYNC_WRITE);
		dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
		dxl_set_txpacket_parameter(1, 2);
		for( i=0; i<NUM_ACTUATOR; i++ )
		{
			dxl_set_txpacket_parameter(2+3*i, id[i]);
			GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (float)AmpPos);
			printf( "%d  ", GoalPos );
			dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos));
			dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos));
		}
		dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4);
		
		printf( "\n" );
		
		dxl_txrx_packet();
		CommStatus = dxl_get_result();
		if( CommStatus == COMM_RXSUCCESS )
		PrintErrorCode();
		else
		PrintCommStatus(CommStatus);
		
		theta += STEP_THETA;

		if( theta > 2*PI )
		theta -= 2*PI;
#else
	    wPresentPos = dxl_read_word( 4, P_PRESENT_POSITION_L );
        printf( "%d\n", wPresentPos );

		dxl_write_word( 2, P_GOAL_POSITION_L, wPresentPos );
		dxl_write_word( 3, P_GOAL_POSITION_L, wPresentPos );
		PrintErrorCode();
#endif
		}
		getServoStatus();
		_delay_ms(CONTROL_PERIOD);
	}
	return 0;
#endif

#if 0
	DDRC  = 0x7F;
	PORTC = 0x7E;
	
	DDRD  = 0x70;
	PORTD = 0x11;

	while (1)
	{
		if(~PIND & SW_START)
		PORTC = ~(LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY);
		else PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY;
	}
	return 1;
#endif


	while(isFinish == 0){
		_delay_ms(500);
		getSerialData();
//		ReceivedData = getchar();

		//if(ReceivedData == 'u'){
			//printf("%d\r\n", Value);
			//Value++;
		//}else if(ReceivedData == 'd'){
			//printf("%d\r\n", Value);
			//Value--;
		//}else if(ReceivedData == 10 || ReceivedData == 13 ){
			//printf("%s\r\n", "end");
			//break;
		//}
		printf("%s\r\n", "Loop");
	}

	printf("%s\r\n", "finish");

	return 0;
}
void ServoControl( int act ){
	int i;
	int CommStatus = 0;
	if( act >= ACT_MAX ){
//		printf( "act error: %d / %d\n", act, SERVO_MAX );
		return;
	}
	
	//GetAngle
	int angle = 0;
	int diffMax = 0;
	int angleDiff[SERVO_MAX] = {0};
	for(int i=0; i<SERVO_MAX; i++ ){
//		if( motionFirst < 0 ){
			angle = dxl_read_word( servoId[i], P_PRESENT_POSITION_L );
//		}else{
//			angle = angleList[motionFirst][i];
//		}
		angleDiff[i] = angleList[act][i] - angle;
		if( angleDiff[i] < 0 ){
			angleDiff[i] = angleDiff[i] * -1;
		}
		if( diffMax < angleDiff[i] ){
			diffMax = angleDiff[i];
		}
	}
//	motionFirst = act;
	
	int speed[SERVO_MAX] = {100};
	for(int i=0; i<SERVO_MAX; i++ ){
		speed[i] = (int)((float)(angleList[act][SERVO_MAX]) * ((float)angleDiff[i] / diffMax));
		if( speed[i] == 0 ){
			speed[i] = 1;
		}
	}

//    diffmaxTest[motionCount-1] = diffMax;
//	movingTime = ((float)CYCLE_TIME/VALUE_MAX) * ((float)VALUE_MAX / angleList[act][SERVO_MAX]) * diffMax;
    movingTime = diffMax * (float)(((VALUE_MAX*10)/angleList[act][SERVO_MAX])/2);
	if( movingTime < MAIN_DELAY ){
		movingTime = MAIN_DELAY;
	}
	
	//Speed
	dxl_set_txpacket_id(BROADCAST_ID);
	dxl_set_txpacket_instruction(INST_SYNC_WRITE);
	dxl_set_txpacket_parameter(0, P_GOAL_SPEED_L);
	dxl_set_txpacket_parameter(1, 2);
	for( i=0; i<SERVO_MAX; i++ ){
		dxl_set_txpacket_parameter(2+(3*i), servoId[i]);
		dxl_set_txpacket_parameter(3+(3*i), dxl_get_lowbyte(speed[i]));
		dxl_set_txpacket_parameter(4+(3*i), dxl_get_highbyte(speed[i]));
	}
	dxl_set_txpacket_length(4+3*SERVO_MAX);
	dxl_txrx_packet();
	CommStatus = dxl_get_result();
	if( CommStatus == COMM_RXSUCCESS ){
		PrintErrorCode();
		
		//Angle
		dxl_set_txpacket_id(BROADCAST_ID);
		dxl_set_txpacket_instruction(INST_SYNC_WRITE);
		dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
		dxl_set_txpacket_parameter(1, 2);
		for( i=0; i<SERVO_MAX; i++ ){
			dxl_set_txpacket_parameter(2+(3*i), servoId[i]);
			dxl_set_txpacket_parameter(3+(3*i), dxl_get_lowbyte(angleList[act][i]));
			dxl_set_txpacket_parameter(4+(3*i), dxl_get_highbyte(angleList[act][i]));
		}
		dxl_set_txpacket_length(4+3*SERVO_MAX);
		dxl_txrx_packet();
		CommStatus = dxl_get_result();
		if( CommStatus == COMM_RXSUCCESS ){
			PrintErrorCode();
		}else{
			PrintCommStatus(CommStatus);
		}
	}else{
		PrintCommStatus(CommStatus);
	}
}
Example #15
0
int Dynamixel::getLowByte( int word ){
	return dxl_get_lowbyte( word );
}
Example #16
0
int main()
{
	int id[NUM_ACTUATOR];
	int baudnum = 1;
	int deviceIndex = 0;
	float phase[NUM_ACTUATOR];
	float theta = 0;
	int AmpPos = 512;
	//int AmpPos = 2048; // for EX series
	int GoalPos;
	int i;
	int CommStatus;
	printf( "\n\nSyncWrite example for Linux\n\n" );

	// Initialize id and phase
	for( i=0; i<NUM_ACTUATOR; i++ )
	{
		id[i] = i+1;
		phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR;
	}

	///////// Open USB2Dynamixel ////////////
	if( dxl_initialize(deviceIndex, baudnum) == 0 )
	{
		printf( "Failed to open USB2Dynamixel!\n" );
		printf( "Press Enter key to terminate...\n" );
		getchar();
		return 0;
	}
	else
		printf( "Succeed to open USB2Dynamixel!\n" );
	
	// Set goal speed
	dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
	// Set goal position
	dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos );

	while(1)
	{
		printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" );
		if(getchar() == 0x1b)
			break;

		theta = 0;
		do
		{
			// Make syncwrite packet
			dxl_set_txpacket_id(BROADCAST_ID);
			dxl_set_txpacket_instruction(INST_SYNC_WRITE);
			dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
			dxl_set_txpacket_parameter(1, 2);
			for( i=0; i<NUM_ACTUATOR; i++ )
			{
				dxl_set_txpacket_parameter(2+3*i, id[i]);
				GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (double)AmpPos);
				printf( "%d:%d  ", id[i], GoalPos );
				dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos));
				dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos));
			}
			dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4);
			

			printf( "\n" );
			
			dxl_txrx_packet();
			CommStatus = dxl_get_result();
			if( CommStatus == COMM_RXSUCCESS )
			{
				PrintErrorCode();
			}
			else
			{
				PrintCommStatus(CommStatus);
				break;
			}
			
			theta += STEP_THETA;
			usleep(CONTROL_PERIOD);

		}while(theta < 2*PI);
	}

	dxl_terminate();
	printf( "Press Enter key to terminate...\n" );
	getchar();

	return 0;
}