void Scan() { fprintf(stderr, "\n"); PingInfo *data = new PingInfo(); for(int i = 1; i < 253; i++) { if(DXL.Ping(i, data, 0) == COMM_RXSUCCESS) { fprintf(stderr, "\n ... SUCCESS \r"); fprintf(stderr, " [ID:%.3d] Model No : %.5d (0x%.2X 0x%.2X) \n", data->ID, data->ModelNumber, DXL_LOBYTE(data->ModelNumber), DXL_HIBYTE(data->ModelNumber)); } else fprintf(stderr, "."); if(kbhit()) { char c = _getch(); if(c == 0x1b) break; } } fprintf(stderr, "\n\n"); }
int main(int argc, char *argv[]) { fprintf(stderr, "\n***********************************************************************\n"); fprintf(stderr, "* DXL Protocol 2.0 Monitor *\n"); fprintf(stderr, "***********************************************************************\n\n"); char *dev = (char*)"/dev/ttyUSB0"; /* parameter parsing */ while(1) { int option_index = 0, c = 0; static struct option long_options[] = { {"h", no_argument, 0, 0}, {"help", no_argument, 0, 0}, {"d", required_argument, 0, 0}, {"device", required_argument, 0, 0}, {0, 0, 0, 0} }; /* parsing all parameters according to the list above is sufficent */ c = getopt_long_only(argc, argv, "", long_options, &option_index); /* no more options to parse */ if(c == -1) break; /* unrecognized option */ if(c == '?') { Usage(argv[0]); return 0; } /* dispatch the given options */ switch(option_index) { /* h, help */ case 0: case 1: Usage(argv[0]); return 0; break; /* d, device */ case 2: case 3: Usage(argv[0]); dev = strdup(optarg); DXL.ComPort->SetPortName(dev); break; default: Usage(argv[0]); return 0; } } if(DXL.Connect() == false) { fprintf(stderr, " Fail to open USB2Dyanmixel! [%s] \n\n", dev); return 0; } char input[128]; char cmd[80]; char param[20][30]; int num_param; char* token; while(1) { printf("[CMD] "); gets(input); fflush(stdin); if(strlen(input) == 0) continue; token = strtok(input, " "); if(token == 0) continue; strcpy(cmd, token); token = strtok(0, " "); num_param = 0; while(token != 0) { strcpy(param[num_param++], token); token = strtok(0, " "); } if(strcmp(cmd, "help") == 0 || strcmp(cmd, "h") == 0 || strcmp(cmd, "?") == 0) { Help(); } else if(strcmp(cmd, "ping") == 0) { if(num_param == 0) { fprintf(stderr, " Invalid parameters! \n"); continue; } fprintf(stderr, "\n"); PingInfo *data = new PingInfo(); for(int i = 0; i < num_param; i++) { if(DXL.Ping(atoi(param[i]), data, 0) == COMM_RXSUCCESS) { fprintf(stderr, " ... SUCCESS \r"); fprintf(stderr, " [ID:%.3d] Model No : %.5d (0x%.2X 0x%.2X) \n", data->ID, data->ModelNumber, DXL_LOBYTE(data->ModelNumber), DXL_HIBYTE(data->ModelNumber)); } else { fprintf(stderr, " ... FAIL \r"); fprintf(stderr, " [ID:%.3d] \n", atoi(param[i])); } } fprintf(stderr, "\n"); } else if(strcmp(cmd, "scan") == 0) { Scan(); } else if(strcmp(cmd, "broadcast") == 0) { Broadcast(); } else if(strcmp(cmd, "baud") == 0) { if(num_param != 1) { fprintf(stderr, " Invalid parameters! \n"); continue; } if(DXL.SetBaudrate(atoi(param[0])) == false) fprintf(stderr, " Failed to change baudrate! \n"); else fprintf(stderr, " Success to change baudrate! [ BAUD NUM: %d ]\n", atoi(param[0])); } else if(strcmp(cmd, "wrb") == 0 || strcmp(cmd, "w") == 0) { if(num_param != 3) { fprintf(stderr, " Invalid parameters! \n"); continue; } Write(atoi(param[0]), atoi(param[1]), atoi(param[2]), 1); } else if(strcmp(cmd, "wrw") == 0) { if(num_param != 3) { fprintf(stderr, " Invalid parameters! \n"); continue; } Write(atoi(param[0]), atoi(param[1]), atoi(param[2]), 2); } else if(strcmp(cmd, "wrd") == 0) { if(num_param != 3) { fprintf(stderr, " Invalid parameters! \n"); continue; } Write(atoi(param[0]), atoi(param[1]), atol(param[2]), 4); } else if(strcmp(cmd, "rdb") == 0) { if(num_param != 2) { fprintf(stderr, " Invalid parameters! \n"); continue; } Read(atoi(param[0]), atoi(param[1]), 1); } else if(strcmp(cmd, "rdw") == 0) { if(num_param != 2) { fprintf(stderr, " Invalid parameters! \n"); continue; } Read(atoi(param[0]), atoi(param[1]), 2); } else if(strcmp(cmd, "rdd") == 0) { if(num_param != 2) { fprintf(stderr, " Invalid parameters! \n"); continue; } Read(atoi(param[0]), atoi(param[1]), 4); } else if(strcmp(cmd, "r") == 0) { if(num_param != 3) { fprintf(stderr, " Invalid parameters! \n"); continue; } Dump(atoi(param[0]), atoi(param[1]), atoi(param[2])); } else if(strcmp(cmd, "mon") == 0) { int len = 0; if(num_param > 2 && strcmp(param[2],"b") == 0) len = 1; else if(num_param > 2 && strcmp(param[2],"w") == 0) len = 2; else if(num_param > 2 && strcmp(param[2],"d") == 0) len = 4; if(num_param != 3 || len == 0) { fprintf(stderr, " Invalid parameters! \n"); continue; } Refresh(atoi(param[0]), atoi(param[1]), len); } else if(strcmp(cmd, "bp") == 0) { if(num_param != 0) { fprintf(stderr, " Invalid parameters! \n"); continue; } BroadcastPing(); } else if(strcmp(cmd, "reboot") == 0) { if(num_param != 1) { fprintf(stderr, " Invalid parameters! \n"); continue; } int result = DXL.Reboot(atoi(param[0]), 0); if(result != COMM_RXSUCCESS) fprintf(stderr, "\n Fail to reboot! \n\n"); else fprintf(stderr, "\n Success to reboot! \n\n"); } else if(strcmp(cmd, "reset") == 0) { if(num_param != 2) { fprintf(stderr, " Invalid parameters! \n"); continue; } int result = DXL.FactoryReset(atoi(param[0]), atoi(param[1]), 0); if(result != COMM_RXSUCCESS) fprintf(stderr, "\n Fail to reset! \n\n"); else fprintf(stderr, "\n Success to reset! \n\n"); } else if(strcmp(cmd, "exit") == 0) { DXL.Disconnect(); return 0; } } }