Go to the documentation of this file.
31 #endif // HAVE_CONFIG_H
34 #include <readline/readline.h>
35 #include <readline/history.h>
36 #endif // HAVE_LIBREADLINE
108 { SPUR_LINE, {
"line" }, 3 },
109 { SPUR_STOP_LINE, {
"stop_line" }, 3 },
110 { SPUR_CIRCLE, {
"circle" }, 3 },
111 { SPUR_SPIN, {
"spin" }, 1 },
112 { SPUR_GETPOS, {
"get_pos" }, 0 },
113 { SPUR_GETVEL, {
"get_vel" }, 0 },
114 { SPUR_GETFORCE, {
"get_force" }, 0 },
115 { SPUR_SETPOS, {
"set_pos" }, 3 },
116 { SPUR_NEAR_POS, {
"near_pos" }, 3 },
117 { SPUR_NEAR_ANG, {
"near_ang" }, 2 },
118 { SPUR_OVER_LINE, {
"over_line" }, 3 },
119 { SPUR_ADJUSTPOS, {
"adjust_pos" }, 3 },
120 { SPUR_SETVEL, {
"set_vel" }, 1 },
121 { SPUR_SETANGVEL, {
"set_angvel" }, 1 },
122 { SPUR_SETACCEL, {
"set_accel" }, 1 },
123 { SPUR_SETANGACCEL, {
"set_angaccel" }, 1 },
124 { SPUR_FREE, {
"free" }, 0 },
125 { SPUR_OPENFREE, {
"openfree" }, 0 },
126 { SPUR_STOP, {
"stop" }, 0 },
127 { SPUR_INIT, {
"init" }, 0 },
128 { SPUR_FREEZE, {
"freeze" }, 0 },
129 { SPUR_UNFREEZE, {
"unfreeze" }, 0 },
130 { SPUR_ISFREEZE, {
"isfreeze" }, 0 },
131 { SPUR_GETAD, {
"get_ad_value" }, 1 },
132 { SPUR_SET_IO_DIR, {
"set_io_dir" }, 1 },
133 { SPUR_SET_IO_DATA, {
"set_io_data" }, 1 },
134 { SPUR_GET_DEVICE_ERROR_STATE, {
"get_device_error_state" }, 1 },
135 { SPUR_VEL, {
"vel" }, 2 },
136 { SPUR_WHEEL_VEL, {
"wheel_vel" }, 2 },
137 { SPUR_GET_WHEEL_VEL, {
"get_wheel_vel" }, 0 },
138 { SPUR_GET_WHEEL_ANG, {
"get_wheel_ang" }, 0 },
139 { SPUR_GET_WHEEL_TORQUE, {
"get_wheel_torque" }, 0 },
140 { SPUR_WHEEL_TORQUE, {
"wheel_torque" }, 2 },
141 { SPUR_ORIENT, {
"orient" }, 1 },
142 { SPUR_GET_VREF, {
"get_vref" }, 0 },
143 { SPUR_GET_WHEEL_VREF, {
"get_wheel_vref" }, 0 },
144 { SPUR_SET_WHEEL_VEL, {
"set_wheel_vel" }, 2 },
145 { SPUR_SET_WHEEL_ACCEL, {
"set_wheel_accel" }, 2 },
146 { SPUR_WHEEL_ANG, {
"wheel_ang" }, 2 },
147 { SPUR_JOINT_TORQUE, {
"joint_torque" }, 2 },
148 { SPUR_JOINT_VEL, {
"joint_vel" }, 2 },
149 { SPUR_JOINT_ANG, {
"joint_ang" }, 2 },
150 { SPUR_JOINT_ANG_VEL, {
"joint_ang_vel" }, 3 },
151 { SPUR_SET_JOINT_ACCEL, {
"set_joint_accel" }, 2 },
152 { SPUR_SET_JOINT_VEL, {
"set_joint_vel" }, 2 },
153 { SPUR_GET_JOINT_VEL, {
"get_joint_vel" }, 1 },
154 { SPUR_GET_JOINT_VREF, {
"get_joint_vref" }, 1 },
155 { SPUR_GET_JOINT_ANG, {
"get_joint_ang" }, 1 },
156 { SPUR_GET_JOINT_TORQUE, {
"get_joint_torque" }, 1 },
157 { SPUR_SLEEP, {
"sleep" }, 1 },
158 { SPUR_REQUEST_DEVICE_DUMP, {
"request_device_dump" }, 2 },
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++)
217 for (i = 0; i < SPUR_COMMAND_MAX; 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]);
280 case SPUR_GET_WHEEL_TORQUE:
282 printf(
"%f %f\n",
spur.arg[0],
spur.arg[1]);
286 printf(
"%f %f\n",
spur.arg[0],
spur.arg[1]);
288 case SPUR_GET_WHEEL_VREF:
290 printf(
"%f %f\n",
spur.arg[0],
spur.arg[1]);
319 case SPUR_SETANGACCEL:
347 case SPUR_SET_IO_DIR:
350 case SPUR_SET_IO_DATA:
353 case SPUR_GET_DEVICE_ERROR_STATE:
355 printf(
"%d\n", error);
363 case SPUR_WHEEL_TORQUE:
366 case SPUR_GET_WHEEL_VEL:
368 printf(
"%f %f\n",
spur.arg[0],
spur.arg[1]);
370 case SPUR_GET_WHEEL_ANG:
372 printf(
"%f %f\n",
spur.arg[0],
spur.arg[1]);
374 case SPUR_SET_WHEEL_VEL:
377 case SPUR_SET_WHEEL_ACCEL:
383 case SPUR_JOINT_TORQUE:
392 case SPUR_JOINT_ANG_VEL:
395 case SPUR_SET_JOINT_ACCEL:
398 case SPUR_SET_JOINT_VEL:
401 case SPUR_GET_JOINT_VEL:
403 printf(
"%f\n",
spur.arg[1]);
405 case SPUR_GET_JOINT_VREF:
407 printf(
"%f\n",
spur.arg[1]);
409 case SPUR_GET_JOINT_ANG:
411 printf(
"%f\n",
spur.arg[1]);
413 case SPUR_GET_JOINT_TORQUE:
415 printf(
"%f\n",
spur.arg[1]);
420 case SPUR_REQUEST_DEVICE_DUMP:
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");
430 for (i = 0; i < SPUR_COMMAND_MAX; i++)
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
struct SPUR_COMMAND SpurCommand
int YPSpur_set_vel(double v)
int YP_set_io_dir(unsigned char dir)
double YP_get_wheel_vref(double *wrref, double *wlref)
double YPSpur_get_pos(int cs, double *x, double *y, double *theta)
int YPSpur_initex(int msq_key)
double YPSpur_get_vel(double *v, double *w)
int YPSpur_adjust_pos(int cs, double x, double y, double theta)
int YPSpur_near_ang(int cs, double th, double d)
int YPSpur_set_angvel(double w)
void YP_request_device_dump(int id, int block)
int YP_get_ad_value(int num)
int YP_wheel_vel(double r, double l)
int YP_set_wheel_vel(double r, double l)
int YPSpur_over_line(int cs, double x, double y, double theta)
int YPSpur_set_pos(int cs, double x, double y, double theta)
int YP_joint_torque(int id, double t)
double YP_get_vref(double *vref, double *wref)
static const char YPSpur_CSName[CS_MAX][16]
void hook_pre_global(void)
double YP_get_wheel_ang(double *theta_r, double *theta_l)
int YPSpur_circle(int cs, double x, double y, double r)
char * strtok_r(char *str, const char *delim, char **nextp)
int YPSpur_isfreeze(void)
int proc_spur_cmd(char *line, int *coordinate)
void print_help(char *argv[])
int YP_set_io_data(unsigned char data)
double YP_get_joint_vref(int id, double *v)
int YP_joint_ang(int id, double a)
int YPSpur_unfreeze(void)
int YPSpur_set_angaccel(double w)
int proc_spur(char *line, int *coordinate)
int YP_wheel_ang(double r, double l)
double YP_get_joint_vel(int id, double *v)
int main(int argc, char *argv[])
double YP_get_wheel_vel(double *wr, double *wl)
@ SPUR_GET_DEVICE_ERROR_STATE
int YP_joint_ang_vel(int id, double a, double v)
double YP_get_joint_torque(int id, double *t)
int YPSpur_line(int cs, double x, double y, double theta)
double YP_get_wheel_torque(double *torque_r, double *torque_l)
int YP_set_joint_accel(int id, double a)
int YP_set_joint_vel(int id, double v)
@ SPUR_REQUEST_DEVICE_DUMP
double YPSpur_get_force(double *trans, double *angular)
int YP_get_error_state(void)
int YP_joint_vel(int id, double v)
int YPSpur_spin(int cs, double theta)
int YP_set_wheel_accel(double r, double l)
int YPSpur_stop_line(int cs, double x, double y, double theta)
double YP_get_device_error_state(int id, int *err)
int YPSpur_set_accel(double v)
int YP_wheel_torque(double r, double l)
int YPSpur_init_socket(char *ip, int port)
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)
int YPSpur_near_pos(int cs, double x, double y, double r)
int option(ParamOptions option)
int YPSpur_orient(int cs, double theta)
yp-spur
Author(s):
autogenerated on Fri Oct 20 2023 03:02:42