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]);
383 wb_result = dxl_wb.
syncRead(handler_index,
id, id_num, &log);
384 if (wb_result ==
false)
394 wb_result = dxl_wb.
getSyncReadData(handler_index,
id, id_num, data, &log);
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]);
412 wb_result = dxl_wb.
changeID(
id, new_id, &log);
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]);
444 wb_result = dxl_wb.
torqueOn(
id, &log);
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]);
475 wb_result = dxl_wb.
jointMode(
id, 0, 0, &log);
476 if (wb_result ==
false)
486 wb_result = dxl_wb.
goalPosition(
id, (int32_t)goal, &log);
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]);
502 wb_result = dxl_wb.
wheelMode(
id, 0, &log);
503 if (wb_result ==
false)
513 wb_result = dxl_wb.
goalVelocity(
id, (int32_t)goal, &log);
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]);
547 wb_result = dxl_wb.
readRegister(
id, param[1], &data, &log);
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]);
564 wb_result = dxl_wb.
reboot(
id, &log);
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]);
580 wb_result = dxl_wb.
reset(
id, &log);
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);
bool torqueOff(uint8_t id, const char **log=NULL)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val)
bool changeID(uint8_t id, uint8_t new_id, const char **log=NULL)
bool addBulkWriteParam(uint8_t id, uint16_t address, uint16_t length, int32_t data, const char **log=NULL)
bool addBulkReadParam(uint8_t id, uint16_t address, uint16_t length, const char **log=NULL)
bool getBulkReadData(int32_t *data, const char **log=NULL)
bool init(const char *device_name="/dev/ttyUSB0", uint32_t baud_rate=57600, const char **log=NULL)
#define ENTER_ASCII_VALUE
DynamixelWorkbench dxl_wb
bool torqueOn(uint8_t id, const char **log=NULL)
bool syncWrite(uint8_t index, int32_t *data, const char **log=NULL)
bool readRegister(uint8_t id, uint16_t address, uint16_t length, uint32_t *data, const char **log=NULL)
bool writeRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, const char **log=NULL)
bool goalVelocity(uint8_t id, int value, const char **log=NULL)
bool reset(uint8_t id, const char **log=NULL)
bool isAvailableID(uint8_t id)
bool addSyncWriteHandler(uint16_t address, uint16_t length, const char **log=NULL)
bool addSyncReadHandler(uint16_t address, uint16_t length, const char **log=NULL)
bool ping(uint8_t id, uint16_t *get_model_number, const char **log=NULL)
bool scan(uint8_t *get_id, uint8_t *get_the_number_of_id, uint8_t range=253, const char **log=NULL)
bool getSyncReadData(uint8_t index, int32_t *data, const char **log=NULL)
bool initBulkWrite(const char **log=NULL)
bool clearBulkReadParam(void)
bool goalPosition(uint8_t id, int value, const char **log=NULL)
bool changeBaudrate(uint8_t id, uint32_t new_baudrate, const char **log=NULL)
bool monitoring(const char *port_name)
bool initBulkRead(const char **log=NULL)
uint8_t getTheNumberOfBulkReadParam(void)
bool wheelMode(uint8_t id, int32_t acceleration=0, const char **log=NULL)
bool bulkWrite(const char **log=NULL)
const ControlItem * getControlTable(uint8_t id, const char **log=NULL)
bool reboot(uint8_t id, const char **log=NULL)
uint8_t getTheNumberOfControlItem(uint8_t id, const char **log=NULL)
bool bulkRead(const char **log=NULL)
int main(int argc, char *argv[])
bool syncRead(uint8_t index, const char **log=NULL)
bool jointMode(uint8_t id, int32_t velocity=0, int32_t acceleration=0, const char **log=NULL)
float getProtocolVersion(void)
bool setBaudrate(uint32_t baud_rate, const char **log=NULL)