31 #endif // HAVE_CONFIG_H 34 #include <readline/readline.h> 35 #include <readline/history.h> 36 #endif // HAVE_LIBREADLINE 157 {
HELP, {
"help" }, 0 },
158 {
EXIT, {
"exit" }, 0 }
162 sigjmp_buf ctrlc_capture;
163 #endif // HAVE_SIGLONGJMP 169 siglongjmp(ctrlc_capture, 1);
170 #endif // HAVE_SIGLONGJMP 186 } mode = MODE_COMMAND;
190 lcoordinate = *coordinate;
192 argv =
strtok_r(line,
" >\t\n\r", &toksave);
203 for (i = 0; i <
CS_MAX; i++)
217 if (strcmp(argv, SPUR_COMMAND[i].
name) == 0)
219 spur = SPUR_COMMAND[i];
226 spur.
arg[mode - MODE_ARG] = atof(argv);
228 spur.
narg = mode - MODE_ARG;
229 if (mode > MODE_ARG + 3)
233 argv =
strtok_r(NULL,
" >\t\n\r", &toksave);
238 *coordinate = lcoordinate;
244 fprintf(stderr,
"error: too few arguments to %s\n", spur.
name);
268 printf(
"%f %f %f\n", spur.
arg[0], spur.
arg[1], spur.
arg[2]);
272 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
276 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
280 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
284 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
288 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
353 printf(
"%d\n", error);
366 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
370 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
401 printf(
"%f\n", spur.
arg[1]);
405 printf(
"%f\n", spur.
arg[1]);
409 printf(
"%f\n", spur.
arg[1]);
413 printf(
"%f\n", spur.
arg[1]);
420 printf(
" > command arg1 arg2 arg3\n");
421 printf(
" > coordinate\n");
422 printf(
" > coordinate command arg1 arg2 arg3\n");
423 printf(
" > coordinate> command arg1 arg2 arg3\n");
424 printf(
"Commands:\n");
427 printf(
" %s\n", SPUR_COMMAND[i].
name);
429 printf(
"Coordinates:\n");
430 for (i = 0; i <
CS_MAX + 1; i++)
439 printf(
"unknown command\n");
453 line_div =
strtok_r(line,
";\n\r", &toksave);
461 line_div =
strtok_r(NULL,
";\n\r", &toksave);
468 fprintf(stderr,
"USAGE: %s\n", argv[0]);
469 fputs(
"\t-V | --set-vel VALUE : [m/s] set max vel of SPUR to VALUE\n", stderr);
470 fputs(
"\t-W | --set-angvel VALUE : [rad/s] set max angvel of SPUR to VALUE\n", stderr);
471 fputs(
"\t-A | --set-accel VALUE : [m/ss] set accel of SPUR to VALUE\n", stderr);
472 fputs(
"\t-O | --set-angaccel VALUE : [rad/ss] set angaccel of SPUR to VALUE\n", stderr);
473 fputs(
"\t-c | --command \"VALUE\" : execute command VALUE\n", stderr);
474 fputs(
"\t-q | --msq-id VALUE : set message-queue id\n", stderr);
475 fputs(
"\t-s | --socket IP:PORT : use socket ipc\n", stderr);
476 fputs(
"\t-h | --help : print this help\n", stderr);
479 int main(
int argc,
char *argv[])
481 int coordinate =
CS_FS;
493 int set_angaccel = 0;
495 struct option options[9] =
497 {
"set-vel", 1, 0,
'V' },
498 {
"set-angvel", 1, 0,
'W' },
499 {
"set-accel", 1, 0,
'A' },
500 {
"set-angaccel", 1, 0,
'O' },
501 {
"command", 1, 0,
'c' },
502 {
"msq-id", 1, 0,
'q' },
503 {
"socket", 1, 0,
's' },
504 {
"help", 0, 0,
'h' },
511 while ((opt = getopt_long(argc, argv,
"V:W:A:O:c:q:s:h", options, NULL)) != -1)
520 angvel = atof(optarg);
524 accel = atof(optarg);
528 angaccel = atof(optarg);
542 msqid = atoi(optarg);
545 strncpy(ip, optarg, 64);
551 fprintf(stderr,
"USAGE: %s -s ip:port\n", argv[0]);
569 signal(SIGINT,
ctrlc);
570 #endif // HAVE_SIGLONGJMP 590 read_history(
".spurip_history");
591 #endif // HAVE_LIBREADLINE 594 static char *line = NULL;
595 static char *line_prev = NULL;
597 #if !HAVE_LIBREADLINE 600 #endif // HAVE_GETLINE 601 #endif // !HAVE_LIBREADLINE 603 if (sigsetjmp(ctrlc_capture, 1) != 0)
606 write_history(
".spurip_history");
607 #endif // HAVE_LIBREADLINE 610 #endif // HAVE_SIGLONGJMP 635 fprintf(stderr,
"WARN: YPSpur-coordinator terminated.\n");
638 signal(SIGINT, NULL);
639 #endif // HAVE_SIGLONGJMP 649 fprintf(stderr,
"INFO: YPSpur-coordinator started.\n");
652 signal(SIGINT,
ctrlc);
653 #endif // HAVE_SIGLONGJMP 661 line = readline(text);
667 if (strlen(line) > 0)
669 if (line && line_prev)
671 if (strcmp(line, line_prev) != 0)
687 getline(&line, &len, stdin);
692 fgets(line, 512, stdin);
693 #endif // HAVE_GETLINE 695 #endif // HAVE_LIBREADLINE 707 write_history(
".spurip_history");
708 #endif // HAVE_LIBREADLINE int proc_spur_cmd(char *line, int *coordinate)
double YP_get_wheel_vref(double *wrref, double *wlref)
double YPSpur_get_force(double *trans, double *angular)
void print_help(char *argv[])
int YPSpur_over_line(int cs, double x, double y, double theta)
double YP_get_joint_vref(int id, double *v)
int YPSpur_initex(int msq_key)
int YPSpur_near_ang(int cs, double th, double d)
int YPSpur_unfreeze(void)
int proc_spur(char *line, int *coordinate)
int YPSpur_near_pos(int cs, double x, double y, double r)
int YP_set_io_data(unsigned char data)
int YP_get_ad_value(int num)
double YP_get_joint_ang(int id, double *a)
double p(YPSpur_param id, enum motor_id motor)
int YPSpur_vel(double v, double w)
double YPSpur_get_vel(double *v, double *w)
char * strtok_r(char *str, const char *delim, char **nextp)
int YPSpur_set_pos(int cs, double x, double y, double theta)
int YP_joint_ang_vel(int id, double a, double v)
int YP_wheel_ang(double r, double l)
int YPSpur_set_angaccel(double w)
double YPSpur_get_pos(int cs, double *x, double *y, double *theta)
int YP_wheel_vel(double r, double l)
void hook_pre_global(void)
int YP_set_io_dir(unsigned char dir)
int YPSpur_circle(int cs, double x, double y, double r)
double YP_get_joint_torque(int id, double *t)
double YP_get_device_error_state(int id, int *err)
static const char YPSpur_CSName[CS_MAX][16]
int YPSpur_isfreeze(void)
int YP_set_wheel_vel(double r, double l)
int YP_joint_ang(int id, double a)
int YPSpur_spin(int cs, double theta)
int YPSpur_set_angvel(double w)
int YP_joint_torque(int id, double t)
int YPSpur_line(int cs, double x, double y, double theta)
struct SPUR_COMMAND SpurCommand
int YPSpur_init_socket(char *ip, int port)
int YPSpur_set_vel(double v)
int YPSpur_set_accel(double v)
double YP_get_wheel_vel(double *wr, double *wl)
int YP_set_joint_vel(int id, double v)
int option(ParamOptions option)
int YPSpur_stop_line(int cs, double x, double y, double theta)
int YP_get_error_state(void)
int YP_joint_vel(int id, double v)
double YP_get_wheel_torque(double *torque_r, double *torque_l)
int YPSpur_adjust_pos(int cs, double x, double y, double theta)
int YP_set_wheel_accel(double r, double l)
double YP_get_vref(double *vref, double *wref)
double YP_get_joint_vel(int id, double *v)
int main(int argc, char *argv[])
int YPSpur_orient(int cs, double theta)
int YP_wheel_torque(double r, double l)
double YP_get_wheel_ang(double *theta_r, double *theta_l)
int YP_set_joint_accel(int id, double a)