31 #endif // HAVE_CONFIG_H 34 #include <readline/readline.h> 35 #include <readline/history.h> 36 #endif // HAVE_LIBREADLINE 159 {
HELP, {
"help" }, 0 },
160 {
EXIT, {
"exit" }, 0 }
164 sigjmp_buf ctrlc_capture;
165 #endif // HAVE_SIGLONGJMP 171 siglongjmp(ctrlc_capture, 1);
172 #endif // HAVE_SIGLONGJMP 188 } mode = MODE_COMMAND;
192 lcoordinate = *coordinate;
194 argv =
strtok_r(line,
" >\t\n\r", &toksave);
205 for (i = 0; i <
CS_MAX; i++)
219 if (strcmp(argv, SPUR_COMMAND[i].
name) == 0)
221 spur = SPUR_COMMAND[i];
228 spur.
arg[mode - MODE_ARG] = atof(argv);
230 spur.
narg = mode - MODE_ARG;
231 if (mode > MODE_ARG + 3)
235 argv =
strtok_r(NULL,
" >\t\n\r", &toksave);
240 *coordinate = lcoordinate;
246 fprintf(stderr,
"error: too few arguments to %s\n", spur.
name);
270 printf(
"%f %f %f\n", spur.
arg[0], spur.
arg[1], spur.
arg[2]);
274 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
278 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
282 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
286 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
290 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
355 printf(
"%d\n", error);
368 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
372 printf(
"%f %f\n", spur.
arg[0], spur.
arg[1]);
403 printf(
"%f\n", spur.
arg[1]);
407 printf(
"%f\n", spur.
arg[1]);
411 printf(
"%f\n", spur.
arg[1]);
415 printf(
"%f\n", spur.
arg[1]);
425 printf(
" > command arg1 arg2 arg3\n");
426 printf(
" > coordinate\n");
427 printf(
" > coordinate command arg1 arg2 arg3\n");
428 printf(
" > coordinate> command arg1 arg2 arg3\n");
429 printf(
"Commands:\n");
432 printf(
" %s\n", SPUR_COMMAND[i].
name);
434 printf(
"Coordinates:\n");
435 for (i = 0; i <
CS_MAX + 1; i++)
444 printf(
"unknown command\n");
458 line_div =
strtok_r(line,
";\n\r", &toksave);
466 line_div =
strtok_r(NULL,
";\n\r", &toksave);
473 fprintf(stderr,
"USAGE: %s\n", argv[0]);
474 fputs(
"\t-V | --set-vel VALUE : [m/s] set max vel of SPUR to VALUE\n", stderr);
475 fputs(
"\t-W | --set-angvel VALUE : [rad/s] set max angvel of SPUR to VALUE\n", stderr);
476 fputs(
"\t-A | --set-accel VALUE : [m/ss] set accel of SPUR to VALUE\n", stderr);
477 fputs(
"\t-O | --set-angaccel VALUE : [rad/ss] set angaccel of SPUR to VALUE\n", stderr);
478 fputs(
"\t-c | --command \"VALUE\" : execute command VALUE\n", stderr);
479 fputs(
"\t-q | --msq-id VALUE : set message-queue id\n", stderr);
480 fputs(
"\t-s | --socket IP:PORT : use socket ipc\n", stderr);
481 fputs(
"\t-h | --help : print this help\n", stderr);
484 int main(
int argc,
char *argv[])
486 int coordinate =
CS_FS;
498 int set_angaccel = 0;
500 struct option options[9] =
502 {
"set-vel", 1, 0,
'V' },
503 {
"set-angvel", 1, 0,
'W' },
504 {
"set-accel", 1, 0,
'A' },
505 {
"set-angaccel", 1, 0,
'O' },
506 {
"command", 1, 0,
'c' },
507 {
"msq-id", 1, 0,
'q' },
508 {
"socket", 1, 0,
's' },
509 {
"help", 0, 0,
'h' },
516 while ((opt = getopt_long(argc, argv,
"V:W:A:O:c:q:s:h", options, NULL)) != -1)
525 angvel = atof(optarg);
529 accel = atof(optarg);
533 angaccel = atof(optarg);
547 msqid = atoi(optarg);
550 strncpy(ip, optarg, 64);
556 fprintf(stderr,
"USAGE: %s -s ip:port\n", argv[0]);
574 signal(SIGINT,
ctrlc);
575 #endif // HAVE_SIGLONGJMP 595 read_history(
".spurip_history");
596 #endif // HAVE_LIBREADLINE 599 static char *line = NULL;
600 static char *line_prev = NULL;
602 #if !HAVE_LIBREADLINE 605 #endif // HAVE_GETLINE 606 #endif // !HAVE_LIBREADLINE 608 if (sigsetjmp(ctrlc_capture, 1) != 0)
611 write_history(
".spurip_history");
612 #endif // HAVE_LIBREADLINE 615 #endif // HAVE_SIGLONGJMP 640 fprintf(stderr,
"WARN: YPSpur-coordinator terminated.\n");
643 signal(SIGINT, NULL);
644 #endif // HAVE_SIGLONGJMP 654 fprintf(stderr,
"INFO: YPSpur-coordinator started.\n");
657 signal(SIGINT,
ctrlc);
658 #endif // HAVE_SIGLONGJMP 663 snprintf(text,
sizeof(text),
"%s> ",
YPSpur_CSName[coordinate]);
666 line = readline(text);
672 if (strlen(line) > 0)
674 if (line && line_prev)
676 if (strcmp(line, line_prev) != 0)
692 getline(&line, &len, stdin);
697 fgets(line, 512, stdin);
698 #endif // HAVE_GETLINE 700 #endif // HAVE_LIBREADLINE 712 write_history(
".spurip_history");
713 #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)
void YP_request_device_dump(int id, int block)
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)