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;
}
}
}