26 #define ESC_ASCII_VALUE 0x1b
27 #define SPACEBAR_ASCII_VALUE 0x20
28 #define ENTER_ASCII_VALUE 0x0a
42 int main(
int argc,
char *argv[])
44 const char* port_name =
"/dev/ttyUSB0";
48 printf(
"Please set '-port_name' arguments for connected Dynamixels\n");
73 bool valid_cmd =
false;
75 bool wb_result =
false;
83 fgets(input,
sizeof(input), stdin);
86 if ((p = strchr(input,
'\n'))!= NULL) *p =
'\0';
89 if (strlen(input) == 0)
return false;
91 token = strtok(input,
" ");
93 if (token == 0)
return false;
96 token = strtok(0,
" ");
101 strcpy(
param[num_param++], token);
102 token = strtok(0,
" ");
105 if (strcmp(cmd,
"help") == 0 || strcmp(cmd,
"h") == 0 || strcmp(cmd,
"?") == 0)
109 else if (strcmp(cmd,
"begin") == 0)
111 uint32_t baud = 57600;
113 baud = atoi(
param[0]);
114 wb_result =
dxl_wb.
init(port_name, baud, &log);
115 if (wb_result ==
false)
118 printf(
"Failed to init\n");
121 printf(
"Succeed to init(%d)\n", baud);
123 else if (strcmp(cmd,
"end") == 0)
127 else if (strcmp(cmd,
"scan") == 0)
131 range = atoi(
param[0]);
133 if (wb_result ==
false)
136 printf(
"Failed to scan\n");
140 printf(
"Find %d Dynamixels\n",
scan_cnt);
142 for (
int cnt = 0; cnt <
scan_cnt; cnt++)
143 printf(
"id : %d\n",
get_id[cnt]);
146 else if (strcmp(cmd,
"ping") == 0)
152 uint16_t model_number = 0;
155 if (wb_result ==
false)
158 printf(
"Failed to ping\n");
162 printf(
"id : %d, model_number : %d\n",
get_id[
ping_cnt], model_number);
168 if (strcmp(cmd,
"control_table") == 0)
170 uint8_t
id = atoi(
param[0]);
175 uint16_t last_register_addr = control_item[the_number_of_control_item-1].
address;
176 uint16_t last_register_addr_length = control_item[the_number_of_control_item-1].
data_length;
178 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length];
180 if (control_item != NULL)
182 wb_result =
dxl_wb.
readRegister(
id, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData, &log);
183 if (wb_result ==
false)
190 for (
int index = 0; index < the_number_of_control_item; index++)
196 data = getAllRegisteredData[control_item[index].
address];
197 printf(
"\t%s : %d\n", control_item[index].item_name, data);
201 switch (control_item[index].data_length)
204 data = getAllRegisteredData[control_item[index].
address];
205 printf(
"\t%s : %d\n", control_item[index].item_name, data);
209 data =
DXL_MAKEWORD(getAllRegisteredData[control_item[index].address], getAllRegisteredData[control_item[index].address+1]);
210 printf(
"\t%s : %d\n", control_item[index].item_name, data);
214 data =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[control_item[index].address], getAllRegisteredData[control_item[index].address+1]),
215 DXL_MAKEWORD(getAllRegisteredData[control_item[index].address+2], getAllRegisteredData[control_item[index].address+3]));
216 printf(
"\t%s : %d\n", control_item[index].item_name, data);
220 data = getAllRegisteredData[control_item[index].address];
228 else if (strcmp(cmd,
"sync_write_handler") == 0)
230 static uint8_t sync_write_handler_index = 0;
231 uint8_t
id = atoi(
param[0]);
233 if (wb_result ==
false)
236 printf(
"Failed to add sync write handler\n");
240 printf(
"%s, sync_write_handler_index = %d\n", log, sync_write_handler_index++);
242 else if (strcmp(cmd,
"sync_read_handler") == 0)
244 static uint8_t sync_read_handler_index = 0;
245 uint8_t
id = atoi(
param[0]);
247 if (wb_result ==
false)
250 printf(
"Failed to add sync read handler\n");
254 printf(
"%s, sync_read_handler_index = %d\n", log, sync_read_handler_index++);
256 else if (strcmp(cmd,
"bulk_write_handler") == 0)
259 if (wb_result ==
false)
262 printf(
"Failed to init bulk write handler\n");
268 else if (strcmp(cmd,
"bulk_write_param") == 0)
270 uint8_t
id = atoi(
param[0]);
272 if (wb_result ==
false)
275 printf(
"Failed to add param for bulk write\n");
281 else if (strcmp(cmd,
"bulk_write") == 0)
284 if (wb_result ==
false)
287 printf(
"Failed to bulk write\n");
293 else if (strcmp(cmd,
"bulk_read_handler") == 0)
296 if (wb_result ==
false)
299 printf(
"Failed to init bulk read handler\n");
305 else if (strcmp(cmd,
"bulk_read_param") == 0)
307 uint8_t
id = atoi(
param[0]);
309 if (wb_result ==
false)
312 printf(
"Failed to add param for bulk read\n");
318 else if (strcmp(cmd,
"bulk_read") == 0)
321 if (wb_result ==
false)
324 printf(
"Failed to bulk read\n");
332 if (wb_result ==
false)
335 printf(
"Failed to get bulk read data\n");
342 printf(
"data[%d] : %d ", index, get_data[index]);
351 if (strcmp(cmd,
"sync_write") == 0)
353 uint8_t id_1 = atoi(
param[0]);
354 uint8_t id_2 = atoi(
param[1]);
355 uint8_t
id[2] = {id_1, id_2};
358 int32_t data[2] = {0, 0};
359 data[0] = atoi(
param[3]);
360 data[1] = atoi(
param[4]);
362 uint8_t handler_index = atoi(
param[2]);
364 wb_result =
dxl_wb.
syncWrite(handler_index,
id, id_num, (int32_t *)data, 1, &log);
365 if (wb_result ==
false)
373 else if (strcmp(cmd,
"sync_read") == 0)
375 uint8_t id_1 = atoi(
param[0]);
376 uint8_t id_2 = atoi(
param[1]);
377 uint8_t
id[2] = {id_1, id_2};
380 int32_t data[2] = {0, 0};
381 uint8_t handler_index = atoi(
param[2]);
384 if (wb_result ==
false)
395 if (wb_result ==
false)
403 printf(
"id[%d] : %d, id[%d] : %d\n", id_1, data[0], id_2, data[1]);
407 else if (strcmp(cmd,
"id") == 0)
409 uint8_t
id = atoi(
param[0]);
410 uint8_t new_id = atoi(
param[1]);
413 if (wb_result ==
false)
423 else if (strcmp(cmd,
"baud") == 0)
425 uint8_t
id = atoi(
param[0]);
426 uint32_t new_baud = atoi(
param[1]);
429 if (wb_result ==
false)
440 else if (strcmp(cmd,
"torque_on") == 0)
442 uint8_t
id = atoi(
param[0]);
445 if (wb_result ==
false)
455 else if (strcmp(cmd,
"torque_off") == 0)
457 uint8_t
id = atoi(
param[0]);
460 if (wb_result ==
false)
470 else if (strcmp(cmd,
"joint") == 0)
472 uint8_t
id = atoi(
param[0]);
473 uint32_t goal = atoi(
param[1]);
476 if (wb_result ==
false)
487 if (wb_result ==
false)
497 else if (strcmp(cmd,
"wheel") == 0)
499 uint8_t
id = atoi(
param[0]);
500 uint32_t goal = atoi(
param[1]);
503 if (wb_result ==
false)
514 if (wb_result ==
false)
524 else if (strcmp(cmd,
"write") == 0)
526 uint8_t
id = atoi(
param[0]);
527 int32_t value = atoi(
param[2]);
530 if (wb_result ==
false)
533 printf(
"Failed to write\n");
541 else if (strcmp(cmd,
"read") == 0)
543 uint8_t
id = atoi(
param[0]);
548 if (wb_result ==
false)
551 printf(
"Failed to read\n");
557 printf(
"read data : %d\n", data);
560 else if (strcmp(cmd,
"reboot") == 0)
562 uint8_t
id = atoi(
param[0]);
565 if (wb_result ==
false)
568 printf(
"Failed to reboot\n");
576 else if (strcmp(cmd,
"reset") == 0)
578 uint8_t
id = atoi(
param[0]);
581 if (wb_result ==
false)
584 printf(
"Failed to reset\n");
594 printf(
"Wrong command\n");
599 printf(
"Please check ID\n");
611 if (
get_id[dxl_cnt] ==
id)
620 printf(
"-------------------------------------\n");
621 printf(
"Set begin before scan or ping\n");
622 printf(
"-------------------------------------\n");
624 printf(
"begin (BAUD)\n");
625 printf(
"scan (RANGE)\n");
626 printf(
"ping (ID)\n");
627 printf(
"control_table (ID)\n");
628 printf(
"id (ID) (NEW_ID)\n");
629 printf(
"baud (ID) (NEW_BAUD)\n");
630 printf(
"torque_on (ID)\n");
631 printf(
"torque_off (ID)\n");
632 printf(
"joint (ID) (GOAL_POSITION)\n");
633 printf(
"wheel (ID) (GOAL_VELOCITY)\n");
634 printf(
"write (ID) (ADDRESS_NAME) (DATA)\n");
635 printf(
"read (ID) (ADDRESS_NAME)\n");
636 printf(
"sync_write_handler (Ref_ID) (ADDRESS_NAME)\n");
637 printf(
"sync_write (ID_1) (ID_2) (HANDLER_INDEX) (PARAM_1) (PARAM_2)\n");
638 printf(
"sync_read_handler (Ref_ID) (ADDRESS_NAME)\n");
639 printf(
"sync_read (ID_1) (ID_2) (HANDLER_INDEX)\n");
640 printf(
"bulk_write_handler\n");
641 printf(
"bulk_write_param (ID) (ADDRESS_NAME) (PARAM)\n");
642 printf(
"bulk_write\n");
643 printf(
"bulk_read_handler\n");
644 printf(
"bulk_read_param (ID) (ADDRESS_NAME)\n");
645 printf(
"bulk_read\n");
646 printf(
"reboot (ID) \n");
647 printf(
"reset (ID) \n");
649 printf(
"-------------------------------------\n");
650 printf(
"Press Enter Key\n");
655 struct termios oldt, newt;
658 tcgetattr( STDIN_FILENO, &oldt );
660 newt.c_lflag &= ~(ICANON | ECHO);
662 newt.c_cc[VTIME] = 1;
663 tcsetattr( STDIN_FILENO, TCSANOW, &newt );
665 tcsetattr( STDIN_FILENO, TCSANOW, &oldt );
672 struct termios oldt, newt;
676 tcgetattr(STDIN_FILENO, &oldt);
678 newt.c_lflag &= ~(ICANON | ECHO);
679 tcsetattr(STDIN_FILENO, TCSANOW, &newt);
680 oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
681 fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
685 tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
686 fcntl(STDIN_FILENO, F_SETFL, oldf);