p_Monitor.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2  Copyright 2016 ROBOTIS CO., LTD.
3 
4  Licensed under the Apache License, Version 2.0 (the "License");
5  you may not use this file except in compliance with the License.
6  You may obtain a copy of the License at
7 
8  http://www.apache.org/licenses/LICENSE-2.0
9 
10  Unless required by applicable law or agreed to in writing, software
11  distributed under the License is distributed on an "AS IS" BASIS,
12  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  See the License for the specific language governing permissions and
14  limitations under the License.
15 *******************************************************************************/
16 
17 /* Authors: Taehun Lim (Darby) */
18 
19 #include <DynamixelWorkbench.h>
20 #include <fcntl.h> // FILE control
21 #include <termios.h> // Terminal IO
22 #include <iostream>
23 
24 using namespace std;
25 
26 #define ESC_ASCII_VALUE 0x1b
27 #define SPACEBAR_ASCII_VALUE 0x20
28 #define ENTER_ASCII_VALUE 0x0a
29 
30 uint8_t get_id[16];
31 uint8_t scan_cnt = 0;
32 uint8_t ping_cnt = 0;
33 
34 int getch(void);
35 int kbhit(void);
36 bool isAvailableID(uint8_t id);
37 void printInst();
38 bool monitoring(const char* port_name);
39 
41 
42 int main(int argc, char *argv[])
43 {
44  const char* port_name = "/dev/ttyUSB0";
45 
46  if (argc < 2)
47  {
48  printf("Please set '-port_name' arguments for connected Dynamixels\n");
49  return 0;
50  }
51  else
52  {
53  port_name = argv[1];
54  }
55 
56  printInst();
57 
58  while(true)
59  {
60  monitoring(port_name);
61  }
62 
63  return 0;
64 }
65 
66 bool monitoring(const char* port_name)
67 {
68  char input[128];
69  char cmd[80];
70  char param[20][30];
71  int num_param = 0;
72  char *token;
73  bool valid_cmd = false;
74 
75  bool wb_result = false;
76  const char *log;
77 
78  if (kbhit())
79  {
80  if (getchar() == ENTER_ASCII_VALUE)
81  {
82  printf("[CMD]");
83  fgets(input, sizeof(input), stdin);
84 
85  char *p;
86  if ((p = strchr(input, '\n'))!= NULL) *p = '\0';
87  fflush(stdin);
88 
89  if (strlen(input) == 0) return false;
90 
91  token = strtok(input, " ");
92 
93  if (token == 0) return false;
94 
95  strcpy(cmd, token);
96  token = strtok(0, " ");
97  num_param = 0;
98 
99  while (token != 0)
100  {
101  strcpy(param[num_param++], token);
102  token = strtok(0, " ");
103  }
104 
105  if (strcmp(cmd, "help") == 0 || strcmp(cmd, "h") == 0 || strcmp(cmd, "?") == 0)
106  {
107  printInst();
108  }
109  else if (strcmp(cmd, "begin") == 0)
110  {
111  uint32_t baud = 57600;
112 
113  baud = atoi(param[0]);
114  wb_result = dxl_wb.init(port_name, baud, &log);
115  if (wb_result == false)
116  {
117  printf("%s\n", log);
118  printf("Failed to init\n");
119  }
120  else
121  printf("Succeed to init(%d)\n", baud);
122  }
123  else if (strcmp(cmd, "end") == 0)
124  {
125  exit(0);
126  }
127  else if (strcmp(cmd, "scan") == 0)
128  {
129  uint8_t range = 253; // default
130 
131  range = atoi(param[0]);
132  wb_result = dxl_wb.scan(get_id, &scan_cnt, range, &log);
133  if (wb_result == false)
134  {
135  printf("%s\n", log);
136  printf("Failed to scan\n");
137  }
138  else
139  {
140  printf("Find %d Dynamixels\n", scan_cnt);
141 
142  for (int cnt = 0; cnt < scan_cnt; cnt++)
143  printf("id : %d\n", get_id[cnt]);
144  }
145  }
146  else if (strcmp(cmd, "ping") == 0)
147  {
148  get_id[ping_cnt] = 1; // default to 1
149 
150  get_id[ping_cnt] = atoi(param[0]);
151 
152  uint16_t model_number = 0;
153 
154  wb_result = dxl_wb.ping(get_id[ping_cnt], &model_number, &log);
155  if (wb_result == false)
156  {
157  printf("%s\n", log);
158  printf("Failed to ping\n");
159  }
160  else
161  {
162  printf("id : %d, model_number : %d\n", get_id[ping_cnt], model_number);
163  ping_cnt++;
164  }
165  }
166  else if (isAvailableID(atoi(param[0])))
167  {
168  if (strcmp(cmd, "control_table") == 0)
169  {
170  uint8_t id = atoi(param[0]);
171 
172  const ControlItem *control_item = dxl_wb.getControlTable(id);
173  uint8_t the_number_of_control_item = dxl_wb.getTheNumberOfControlItem(id);
174 
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;
177 
178  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length];
179 
180  if (control_item != NULL)
181  {
182  wb_result = dxl_wb.readRegister(id, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData, &log);
183  if (wb_result == false)
184  {
185  printf("%s\n", log);
186  return 0;
187  }
188  else
189  {
190  for (int index = 0; index < the_number_of_control_item; index++)
191  {
192  uint32_t data = 0;
193 
194  if (dxl_wb.getProtocolVersion() == 2.0f)
195  {
196  data = getAllRegisteredData[control_item[index].address];
197  printf("\t%s : %d\n", control_item[index].item_name, data);
198  }
199  else if (dxl_wb.getProtocolVersion() == 1.0f)
200  {
201  switch (control_item[index].data_length)
202  {
203  case BYTE:
204  data = getAllRegisteredData[control_item[index].address];
205  printf("\t%s : %d\n", control_item[index].item_name, data);
206  break;
207 
208  case WORD:
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);
211  break;
212 
213  case DWORD:
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);
217  break;
218 
219  default:
220  data = getAllRegisteredData[control_item[index].address];
221  break;
222  }
223  }
224  }
225  }
226  }
227  }
228  else if (strcmp(cmd, "sync_write_handler") == 0)
229  {
230  static uint8_t sync_write_handler_index = 0;
231  uint8_t id = atoi(param[0]);
232  wb_result = dxl_wb.addSyncWriteHandler(id, param[1], &log);
233  if (wb_result == false)
234  {
235  printf("%s\n", log);
236  printf("Failed to add sync write handler\n");
237  return 0;
238  }
239  else
240  printf("%s, sync_write_handler_index = %d\n", log, sync_write_handler_index++);
241  }
242  else if (strcmp(cmd, "sync_read_handler") == 0)
243  {
244  static uint8_t sync_read_handler_index = 0;
245  uint8_t id = atoi(param[0]);
246  wb_result = dxl_wb.addSyncReadHandler(id, param[1], &log);
247  if (wb_result == false)
248  {
249  printf("%s\n", log);
250  printf("Failed to add sync read handler\n");
251  return 0;
252  }
253  else
254  printf("%s, sync_read_handler_index = %d\n", log, sync_read_handler_index++);
255  }
256  else if (strcmp(cmd, "bulk_write_handler") == 0)
257  {
258  wb_result = dxl_wb.initBulkWrite(&log);
259  if (wb_result == false)
260  {
261  printf("%s\n", log);
262  printf("Failed to init bulk write handler\n");
263  return 0;
264  }
265  else
266  printf("%s\n", log);
267  }
268  else if (strcmp(cmd, "bulk_write_param") == 0)
269  {
270  uint8_t id = atoi(param[0]);
271  wb_result = dxl_wb.addBulkWriteParam(id, param[1], atoi(param[2]), &log);
272  if (wb_result == false)
273  {
274  printf("%s\n", log);
275  printf("Failed to add param for bulk write\n");
276  return 0;
277  }
278  else
279  printf("%s\n", log);
280  }
281  else if (strcmp(cmd, "bulk_write") == 0)
282  {
283  wb_result = dxl_wb.bulkWrite(&log);
284  if (wb_result == false)
285  {
286  printf("%s\n", log);
287  printf("Failed to bulk write\n");
288  return 0;
289  }
290  else
291  printf("%s\n", log);
292  }
293  else if (strcmp(cmd, "bulk_read_handler") == 0)
294  {
295  wb_result = dxl_wb.initBulkRead(&log);
296  if (wb_result == false)
297  {
298  printf("%s\n", log);
299  printf("Failed to init bulk read handler\n");
300  return 0;
301  }
302  else
303  printf("%s\n", log);
304  }
305  else if (strcmp(cmd, "bulk_read_param") == 0)
306  {
307  uint8_t id = atoi(param[0]);
308  wb_result = dxl_wb.addBulkReadParam(id, param[1], &log);
309  if (wb_result == false)
310  {
311  printf("%s\n", log);
312  printf("Failed to add param for bulk read\n");
313  return 0;
314  }
315  else
316  printf("%s\n", log);
317  }
318  else if (strcmp(cmd, "bulk_read") == 0)
319  {
320  wb_result = dxl_wb.bulkRead(&log);
321  if (wb_result == false)
322  {
323  printf("%s\n", log);
324  printf("Failed to bulk read\n");
325  return 0;
326  }
327  else
328  printf("%s\n", log);
329 
330  int32_t get_data[dxl_wb.getTheNumberOfBulkReadParam()];
331  wb_result = dxl_wb.getBulkReadData(&get_data[0], &log);
332  if (wb_result == false)
333  {
334  printf("%s\n", log);
335  printf("Failed to get bulk read data\n");
336  return 0;
337  }
338  else
339  {
340  printf("%s\n", log);
341  for (uint8_t index = 0; index < dxl_wb.getTheNumberOfBulkReadParam(); index++)
342  printf("data[%d] : %d ", index, get_data[index]);
343 
344  printf("\n");
345  }
346 
347  dxl_wb.clearBulkReadParam();
348  }
349  else if (isAvailableID(atoi(param[0])) && isAvailableID(atoi(param[1])))
350  {
351  if (strcmp(cmd, "sync_write") == 0)
352  {
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};
356  uint8_t id_num = 2;
357 
358  int32_t data[2] = {0, 0};
359  data[0] = atoi(param[3]);
360  data[1] = atoi(param[4]);
361 
362  uint8_t handler_index = atoi(param[2]);
363 
364  wb_result = dxl_wb.syncWrite(handler_index, id, id_num, (int32_t *)data, 1, &log);
365  if (wb_result == false)
366  {
367  printf("%s\n", log);
368  return 0;
369  }
370  else
371  printf("%s\n", log);
372  }
373  else if (strcmp(cmd, "sync_read") == 0)
374  {
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};
378  uint8_t id_num = 2;
379 
380  int32_t data[2] = {0, 0};
381  uint8_t handler_index = atoi(param[2]);
382 
383  wb_result = dxl_wb.syncRead(handler_index, id, id_num, &log);
384  if (wb_result == false)
385  {
386  printf("%s\n", log);
387  return 0;
388  }
389  else
390  {
391  printf("%s\n", log);
392  }
393 
394  wb_result = dxl_wb.getSyncReadData(handler_index, id, id_num, data, &log);
395  if (wb_result == false)
396  {
397  printf("%s\n", log);
398  return 0;
399  }
400  else
401  {
402  printf("%s\n", log);
403  printf("id[%d] : %d, id[%d] : %d\n", id_1, data[0], id_2, data[1]);
404  }
405  }
406  }
407  else if (strcmp(cmd, "id") == 0)
408  {
409  uint8_t id = atoi(param[0]);
410  uint8_t new_id = atoi(param[1]);
411 
412  wb_result = dxl_wb.changeID(id, new_id, &log);
413  if (wb_result == false)
414  {
415  printf("%s\n", log);
416  return 0;
417  }
418  else
419  {
420  printf("%s\n", log);
421  }
422  }
423  else if (strcmp(cmd, "baud") == 0)
424  {
425  uint8_t id = atoi(param[0]);
426  uint32_t new_baud = atoi(param[1]);
427 
428  wb_result = dxl_wb.changeBaudrate(id, new_baud, &log);
429  if (wb_result == false)
430  {
431  printf("%s\n", log);
432  return 0;
433  }
434  else
435  {
436  wb_result = dxl_wb.setBaudrate(new_baud, &log);
437  printf("%s\n", log);
438  }
439  }
440  else if (strcmp(cmd, "torque_on") == 0)
441  {
442  uint8_t id = atoi(param[0]);
443 
444  wb_result = dxl_wb.torqueOn(id, &log);
445  if (wb_result == false)
446  {
447  printf("%s\n", log);
448  return 0;
449  }
450  else
451  {
452  printf("%s\n", log);
453  }
454  }
455  else if (strcmp(cmd, "torque_off") == 0)
456  {
457  uint8_t id = atoi(param[0]);
458 
459  wb_result = dxl_wb.torqueOff(id, &log);
460  if (wb_result == false)
461  {
462  printf("%s\n", log);
463  return 0;
464  }
465  else
466  {
467  printf("%s\n", log);
468  }
469  }
470  else if (strcmp(cmd, "joint") == 0)
471  {
472  uint8_t id = atoi(param[0]);
473  uint32_t goal = atoi(param[1]);
474 
475  wb_result = dxl_wb.jointMode(id, 0, 0, &log);
476  if (wb_result == false)
477  {
478  printf("%s\n", log);
479  return 0;
480  }
481  else
482  {
483  printf("%s\n", log);
484  }
485 
486  wb_result = dxl_wb.goalPosition(id, (int32_t)goal, &log);
487  if (wb_result == false)
488  {
489  printf("%s\n", log);
490  return 0;
491  }
492  else
493  {
494  printf("%s\n", log);
495  }
496  }
497  else if (strcmp(cmd, "wheel") == 0)
498  {
499  uint8_t id = atoi(param[0]);
500  uint32_t goal = atoi(param[1]);
501 
502  wb_result = dxl_wb.wheelMode(id, 0, &log);
503  if (wb_result == false)
504  {
505  printf("%s\n", log);
506  return 0;
507  }
508  else
509  {
510  printf("%s\n", log);
511  }
512 
513  wb_result = dxl_wb.goalVelocity(id, (int32_t)goal, &log);
514  if (wb_result == false)
515  {
516  printf("%s\n", log);
517  return 0;
518  }
519  else
520  {
521  printf("%s\n", log);
522  }
523  }
524  else if (strcmp(cmd, "write") == 0)
525  {
526  uint8_t id = atoi(param[0]);
527  int32_t value = atoi(param[2]);
528 
529  wb_result = dxl_wb.writeRegister(id, param[1], value, &log);
530  if (wb_result == false)
531  {
532  printf("%s\n", log);
533  printf("Failed to write\n");
534  return 0;
535  }
536  else
537  {
538  printf("%s\n", log);
539  }
540  }
541  else if (strcmp(cmd, "read") == 0)
542  {
543  uint8_t id = atoi(param[0]);
544 
545  int32_t data = 0;
546 
547  wb_result = dxl_wb.readRegister(id, param[1], &data, &log);
548  if (wb_result == false)
549  {
550  printf("%s\n", log);
551  printf("Failed to read\n");
552  return 0;
553  }
554  else
555  {
556  printf("%s\n", log);
557  printf("read data : %d\n", data);
558  }
559  }
560  else if (strcmp(cmd, "reboot") == 0)
561  {
562  uint8_t id = atoi(param[0]);
563 
564  wb_result = dxl_wb.reboot(id, &log);
565  if (wb_result == false)
566  {
567  printf("%s\n", log);
568  printf("Failed to reboot\n");
569  return 0;
570  }
571  else
572  {
573  printf("%s\n", log);
574  }
575  }
576  else if (strcmp(cmd, "reset") == 0)
577  {
578  uint8_t id = atoi(param[0]);
579 
580  wb_result = dxl_wb.reset(id, &log);
581  if (wb_result == false)
582  {
583  printf("%s\n", log);
584  printf("Failed to reset\n");
585  return 0;
586  }
587  else
588  {
589  printf("%s\n", log);
590  }
591  }
592  else
593  {
594  printf("Wrong command\n");
595  }
596  }
597  else
598  {
599  printf("Please check ID\n");
600  }
601  }
602  }
603 
604  return 0;
605 }
606 
607 bool isAvailableID(uint8_t id)
608 {
609  for (int dxl_cnt = 0; dxl_cnt < (scan_cnt + ping_cnt); dxl_cnt++)
610  {
611  if (get_id[dxl_cnt] == id)
612  return true;
613  }
614 
615  return false;
616 }
617 
618 void printInst(void)
619 {
620  printf("-------------------------------------\n");
621  printf("Set begin before scan or ping\n");
622  printf("-------------------------------------\n");
623  printf("help\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");
648  printf("end\n");
649  printf("-------------------------------------\n");
650  printf("Press Enter Key\n");
651 }
652 
653 int getch()
654 {
655  struct termios oldt, newt;
656  int ch;
657 
658  tcgetattr( STDIN_FILENO, &oldt );
659  newt = oldt;
660  newt.c_lflag &= ~(ICANON | ECHO);
661  newt.c_cc[VMIN] = 0;
662  newt.c_cc[VTIME] = 1;
663  tcsetattr( STDIN_FILENO, TCSANOW, &newt );
664  ch = getchar();
665  tcsetattr( STDIN_FILENO, TCSANOW, &oldt );
666 
667  return ch;
668 }
669 
670 int kbhit()
671 {
672  struct termios oldt, newt;
673  int ch;
674  int oldf;
675 
676  tcgetattr(STDIN_FILENO, &oldt);
677  newt = 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);
682 
683  ch = getchar();
684 
685  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
686  fcntl(STDIN_FILENO, F_SETFL, oldf);
687 
688  if (ch != EOF)
689  {
690  ungetc(ch, stdin);
691  return 1;
692  }
693  return 0;
694 }
bool torqueOff(uint8_t id, const char **log=NULL)
bool param(const std::string &param_name, T &param_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)
uint16_t address
bool init(const char *device_name="/dev/ttyUSB0", uint32_t baud_rate=57600, const char **log=NULL)
#define ENTER_ASCII_VALUE
Definition: p_Monitor.cpp:28
DynamixelWorkbench dxl_wb
Definition: p_Monitor.cpp:40
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)
def DXL_MAKEWORD(a, b)
int getch(void)
Definition: p_Monitor.cpp:653
bool isAvailableID(uint8_t id)
Definition: p_Monitor.cpp:607
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)
#define DWORD
#define BYTE
bool initBulkWrite(const char **log=NULL)
bool clearBulkReadParam(void)
uint8_t ping_cnt
Definition: p_Monitor.cpp:32
void printInst()
Definition: p_Monitor.cpp:618
#define WORD
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)
Definition: p_Monitor.cpp:66
bool initBulkRead(const char **log=NULL)
uint8_t getTheNumberOfBulkReadParam(void)
uint8_t scan_cnt
Definition: p_Monitor.cpp:31
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[])
Definition: p_Monitor.cpp:42
uint16_t data_length
bool syncRead(uint8_t index, const char **log=NULL)
def DXL_MAKEDWORD(a, b)
bool jointMode(uint8_t id, int32_t velocity=0, int32_t acceleration=0, const char **log=NULL)
uint8_t get_id[16]
Definition: p_Monitor.cpp:30
float getProtocolVersion(void)
int kbhit(void)
Definition: p_Monitor.cpp:670
bool setBaudrate(uint32_t baud_rate, const char **log=NULL)


dynamixel_workbench_toolbox
Author(s): Darby Lim , Ryan Shim
autogenerated on Mon Sep 28 2020 03:37:05