Dynamixel Synchronization Control

Multiple Dynamixels can be controlled by synchronization.

 

Things to Prepare

The controller and Dynamixel are connected.

This example is operated when the Dynamixel ID is designated from 1 to 3 in order.

 

Theory

Dynamixel can be controlled by transmitting designated packet.  The location of Dynamixel can be controlled using provided library.

 

Source

int AmpPos = 512;

//int AmpPos = 2048; // for EX series

Like EX 106+, if an actuator belongs to the location range between 0 and 4095, notes are applied to the upper line but not applied to the lower line.

 

serial_initialize(57600);

dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index

sei(); // Interrupt Enable

This part is for initialization to use serial communication.  The serial initialization function is included in serial library, and the serial port is initialized if communication speed is transmitted by data.

In the case of sei(), it is an internal command makes users possible to use "Interrupt."

In the case of dxl_initialize() function, communication environment of the controller is initialized if device index and communication speed are transmitted by data.

DEFAULT_BAUDNUM is 1.

If there are no specific reasons, device index is 0.

 

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);

This part is for calculation of the initial location and initialization of the location of each Dynamixel.

The speed of all Dynamixels is set to the maximum level, and the location is set to the center using dxl_write_word() function.

 

// 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);

This part is for packet creation.  Please refer to Dynamixel Packet Structure.

All connected actuator packets are created and transmitted.

 

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);

After receiving result packets, if there are errors, the error codes are printed.

If a value exceeds the calculated location boundary value, the increase/decrease direction is changed to the opposite.

 

Result

Multiple Dynamixels are moved back and forth in the designated location, and the current location is printed through terminal.