21 void swap(int32_t *array);
23 int main(
int argc,
char *argv[])
25 const char* port_name =
"/dev/ttyUSB0";
26 int baud_rate = 57600;
28 uint16_t model_number = 0;
29 uint8_t dxl_id[2] = {0, 0};
33 printf(
"Please set '-port_name', '-baud_rate', '-dynamixel_id_1', '-dynamixel_id_2' arguments for connected Dynamixels\n");
39 baud_rate = atoi(argv[2]);
40 dxl_id[0] = atoi(argv[3]);
41 dxl_id[1] = atoi(argv[4]);
49 result =
dxl_wb.
init(port_name, baud_rate, &log);
53 printf(
"Failed to init\n");
58 printf(
"Succeed to init(%d)\n", baud_rate);
60 for (
int cnt = 0; cnt < 2; cnt++)
62 result =
dxl_wb.
ping(dxl_id[cnt], &model_number, &log);
66 printf(
"Failed to ping\n");
70 printf(
"Succeeded to ping\n");
71 printf(
"id : %d, model_number : %d\n", dxl_id[cnt], model_number);
78 printf(
"Failed to change joint mode\n");
82 printf(
"Succeed to change joint mode\n");
110 printf(
"Failed to add bulk read position param\n");
121 printf(
"Failed to add bulk read led param\n");
128 int32_t goal_position[2] = {0, 1023};
129 int32_t led[2] = {0, 1};
131 int32_t get_data[2] = {0, 0};
133 const uint8_t handler_index = 0;
141 printf(
"Failed to add bulk write position param\n");
152 printf(
"Failed to add bulk write led param\n");
163 printf(
"Failed to bulk write\n");
172 printf(
"Failed to bulk read\n");
182 printf(
"[ID %d]\tGoal Position : %d\tPresent Position : %d, [ID %d]\tLED : %d\n"
183 ,dxl_id[0], goal_position[0], get_data[0], dxl_id[1], get_data[1]);
186 }
while(abs(goal_position[0] - get_data[0]) > 15);
197 int32_t tmp = array[0];