00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <math.h>
00022 #include <stdio.h>
00023 #include <stdlib.h>
00024 #include <string.h>
00025 #include <strings.h>
00026 #include <unistd.h>
00027
00028 #include <fcntl.h>
00029 #include <sys/stat.h>
00030 #include <sys/time.h>
00031 #include <sys/types.h>
00032 #include <time.h>
00033
00034 #ifdef HAVE_CONFIG_H
00035 #include <config.h>
00036 #endif // HAVE_CONFIG_H
00037
00038
00039 #include <shvel-param.h>
00040
00041
00042 #include <communication.h>
00043 #include <serial.h>
00044 #include <param.h>
00045 #include <control.h>
00046 #include <command.h>
00047 #include <utility.h>
00048 #include <yprintf.h>
00049 #include <formula-calc.h>
00050
00051
00052 #include <ypspur.h>
00053
00054 #include <pthread.h>
00055
00056 double g_P[YP_PARAM_NUM][YP_PARAM_MAX_MOTOR_NUM];
00057 int g_P_changed[YP_PARAM_NUM][YP_PARAM_MAX_MOTOR_NUM];
00058 int g_P_set[YP_PARAM_NUM][YP_PARAM_MAX_MOTOR_NUM];
00059 struct rpf_t *g_Pf[YP_PARAM_NUM][YP_PARAM_MAX_MOTOR_NUM];
00060 char g_state[YP_STATE_NUM];
00061 Parameters g_param;
00062 int g_param_init = 1;
00063
00064 int state(YPSpur_state id)
00065 {
00066 return (g_state[id] == ENABLE);
00067 }
00068
00069 void enable_state(YPSpur_state id)
00070 {
00071 g_state[id] = ENABLE;
00072 }
00073
00074 void disable_state(YPSpur_state id)
00075 {
00076 g_state[id] = DISABLE;
00077 }
00078
00079 double p(YPSpur_param id, enum motor_id motor)
00080 {
00081 return g_P[id][motor];
00082 }
00083
00084 int isset_p(YPSpur_param id, enum motor_id motor)
00085 {
00086 return g_P_set[id][motor];
00087 }
00088
00089 int ischanged_p(YPSpur_param id, enum motor_id motor)
00090 {
00091 return g_P_changed[id][motor];
00092 }
00093
00094 double *pp(YPSpur_param id, enum motor_id motor)
00095 {
00096 return &g_P[id][motor];
00097 }
00098
00099 int option(ParamOptions option)
00100 {
00101 if (g_param.option & option)
00102 return 1;
00103 return 0;
00104 }
00105
00106 ParamOutputLv output_lv(void)
00107 {
00108 return g_param.output_lv;
00109 }
00110
00111 ParametersPtr get_param_ptr()
00112 {
00113 return &g_param;
00114 }
00115
00116 int is_character(int c);
00117 int is_number(int c);
00118
00119
00120 void arg_help(int argc, char *argv[])
00121 {
00122 fprintf(stderr, "USAGE: %s [OPTION]...\n\n", argv[0]);
00123 fprintf(stderr, " -v, --version Display version info and exit.\n");
00124 fprintf(stderr, " -d, --device <device> Specify a B-Loco device. e.g. /dev/ttyUSB0\n");
00125 fprintf(stderr, " -p, --param <tile> Specify a robot parameter file.\n");
00126 fprintf(stderr, " --verbose Display detail information.\n");
00127 fprintf(stderr, " --quiet Display nothing.\n");
00128 fprintf(stderr, " -h, --help This help\n");
00129 fprintf(stderr, " --long-help Long help\n");
00130 fprintf(stderr, " --param-help Comments of parameters\n\n");
00131 }
00132
00133
00134 void arg_longhelp(int argc, char *argv[])
00135 {
00136 arg_help(argc, argv);
00137 fprintf(stderr, " -o, --show-odometry Display estimated robot position.\n");
00138 fprintf(stderr, " -t, --show-timestamp Display timestamp of serial communication \n"
00139 " with the B-Loco device.\n");
00140 fprintf(stderr, " --reconnect Try reconnect device when device was closed.\n");
00141 fprintf(stderr, " --without-ssm Run without ssm output.\n");
00142 fprintf(stderr, " -q, --msq-key <MSQKEY> Run with message que key MSQKEY (default = 28741).\n");
00143 fprintf(stderr, " -s, --speed <BAUDRATE> Set baudrate.\n");
00144 fprintf(stderr, " --admask <ADMASK> Get AD data of ADMASK from B-Loco device.\n");
00145 fprintf(stderr, " --enable-get-digital-io Enable digital IO port.\n");
00146 fprintf(stderr, " -c, --without-control Run without control.\n");
00147 fprintf(stderr, " --without-device Run without B-Loco device.\n");
00148 fprintf(stderr, " --no-yp-protocol Run without checking plotocol of B-Loco device.\n");
00149 fprintf(stderr, " --passive Passive run mode.\n");
00150 fprintf(stderr, " --update-param Automatically reload parameter file.\n");
00151 fprintf(stderr, " --high-resolution[=ARG] Use high resolution velocity control mode (default = yes).\n");
00152 fprintf(stderr, " --ssm-id <SSMID> Change ssm id (default = 0).\n");
00153 fprintf(stderr, " --socket <port> Use socket ipc.\n");
00154 fprintf(stderr, " --daemon Run in daemon mode.\n");
00155 }
00156
00157
00158 void param_help(void)
00159 {
00160 int i;
00161 char param_names[YP_PARAM_NUM][32] = YP_PARAM_NAME;
00162 char param_comments[YP_PARAM_NUM][128] = YP_PARAM_COMMENT;
00163
00164 fprintf(stderr, "INFO: Comments of parameters (parameter version %.1f)\n\n", YP_PARAM_REQUIRED_VERSION);
00165 for (i = 0; i < YP_PARAM_NUM; i++)
00166 {
00167 if (param_names[i][0] == '_')
00168 continue;
00169 fprintf(stderr, " %20s: %s\n", param_names[i], param_comments[i]);
00170 }
00171 }
00172
00173
00174 int arg_analyze(int argc, char *argv[])
00175 {
00176 int i;
00177
00178 g_param.option = OPTION_DEFAULT;
00179 g_param.msq_key = YPSPUR_MSQ_KEY;
00180 g_param.output_lv = OUTPUT_LV_INFO;
00181 g_param.speed = 0;
00182 g_param.ssm_id = 0;
00183
00184 strcpy(g_param.parameter_filename, DEFAULT_PARAMETER_FILE);
00185 strcpy(g_param.device_name, DEFAULT_DEVICE_NAME);
00186
00187 for (i = 1; i < argc; i++)
00188 {
00189 if (!strcmp(argv[i], "--help") || !strcmp(argv[i], "-h"))
00190 {
00191 g_param.option |= OPTION_SHOW_HELP;
00192 }
00193 else if (!strcmp(argv[i], "--long-help"))
00194 {
00195 g_param.option |= OPTION_SHOW_LONGHELP;
00196 }
00197 else if (!strcmp(argv[i], "--daemon"))
00198 {
00199 g_param.option |= OPTION_DAEMON;
00200 }
00201 else if (!strcmp(argv[i], "--param-help"))
00202 {
00203 g_param.option |= OPTION_SHOW_PARAMHELP;
00204 }
00205 else if (!strcmp(argv[i], "--show-odometry") || !strcmp(argv[i], "-o"))
00206 {
00207 g_param.option |= OPTION_SHOW_ODOMETRY;
00208 }
00209 else if (!strcmp(argv[i], "--show-timestamp") || !strcmp(argv[i], "-t"))
00210 {
00211 g_param.option |= OPTION_SHOW_TIMESTAMP;
00212 }
00213 else if (!strcmp(argv[i], "--param") || !strcmp(argv[i], "-p"))
00214 {
00215 if (i + 1 < argc)
00216 {
00217 i++;
00218 strcpy(g_param.parameter_filename, argv[i]);
00219 g_param.option |= OPTION_PARAM_FILE;
00220 }
00221 else
00222 break;
00223 }
00224 else if (!strcmp(argv[i], "--socket"))
00225 {
00226 if (i + 1 < argc)
00227 {
00228 i++;
00229 g_param.port = atoi(argv[i]);
00230 g_param.option |= OPTION_SOCKET;
00231 }
00232 else
00233 break;
00234 }
00235 else if (!strcmp(argv[i], "--admask"))
00236 {
00237 if (i + 1 < argc)
00238 {
00239 char *pos;
00240
00241 i++;
00242 g_param.admask = 0;
00243 for (pos = argv[i]; *pos != 0; pos++)
00244 {
00245 g_param.admask = g_param.admask << 1;
00246 if (*pos == '1')
00247 g_param.admask |= 1;
00248 }
00249 }
00250 else
00251 break;
00252 }
00253 else if (!strcmp(argv[i], "--device") || !strcmp(argv[i], "-d"))
00254 {
00255 if (i + 1 < argc)
00256 {
00257 i++;
00258 strcpy(g_param.device_name, argv[i]);
00259 }
00260 else
00261 break;
00262 }
00263 else if (!strcmp(argv[i], "--without-control") || !strcmp(argv[i], "-c"))
00264 {
00265 g_param.option |= OPTION_PARAM_CONTROL;
00266 disable_state(YP_STATE_MOTOR);
00267 disable_state(YP_STATE_VELOCITY);
00268 disable_state(YP_STATE_BODY);
00269 disable_state(YP_STATE_TRACKING);
00270 }
00271 else if (!strcmp(argv[i], "--without-device"))
00272 {
00273 g_param.option |= OPTION_WITHOUT_DEVICE;
00274 g_param.option |= OPTION_DO_NOT_USE_YP;
00275 g_param.option |= OPTION_PARAM_CONTROL;
00276 disable_state(YP_STATE_MOTOR);
00277 disable_state(YP_STATE_VELOCITY);
00278 disable_state(YP_STATE_BODY);
00279 disable_state(YP_STATE_TRACKING);
00280 }
00281 else if (!strcmp(argv[i], "--high-resolution") ||
00282 !strcmp(argv[i], "--high-resolution=yes"))
00283 {
00284 g_param.option |= OPTION_HIGH_PREC;
00285 }
00286 else if (!strcmp(argv[i], "--high-resolution=no"))
00287 {
00288 g_param.option &= ~OPTION_HIGH_PREC;
00289 }
00290 else if (!strcmp(argv[i], "--version") || !strcmp(argv[i], "-v"))
00291 {
00292 g_param.option |= OPTION_VERSION;
00293 }
00294 else if (!strcmp(argv[i], "--verbose"))
00295 {
00296 g_param.output_lv = OUTPUT_LV_DEBUG;
00297 }
00298 else if (!strcmp(argv[i], "--quiet"))
00299 {
00300 g_param.output_lv = OUTPUT_LV_WARNING;
00301 }
00302 else if (!strcmp(argv[i], "--reconnect"))
00303 {
00304 g_param.option |= OPTION_RECONNECT;
00305 }
00306 else if (!strcmp(argv[i], "--enable-set-bs"))
00307 {
00308 g_param.option |= OPTION_ENABLE_SET_BS;
00309 }
00310 else if (!strcmp(argv[i], "--enable-get-digital-io"))
00311 {
00312 g_param.option |= OPTION_ENABLE_GET_DIGITAL_IO;
00313 }
00314 else if (!strcmp(argv[i], "--no-yp-protocol"))
00315 {
00316 g_param.option |= OPTION_DO_NOT_USE_YP;
00317 }
00318 else if (!strcmp(argv[i], "--without-ssm"))
00319 {
00320 g_param.option |= OPTION_WITHOUT_SSM;
00321 }
00322 else if (!strcmp(argv[i], "--msq-key") || !strcmp(argv[i], "-q"))
00323 {
00324 if (i + 1 < argc)
00325 {
00326 i++;
00327 g_param.msq_key = atoi(argv[i]);
00328 }
00329 else
00330 break;
00331 }
00332 else if (!strcmp(argv[i], "--speed") || !strcmp(argv[i], "-s"))
00333 {
00334 if (i + 1 < argc)
00335 {
00336 i++;
00337 g_param.speed = atoi(argv[i]);
00338 }
00339 else
00340 break;
00341 }
00342 else if (!strcmp(argv[i], "--passive"))
00343 {
00344 g_param.option |= OPTION_PASSIVE;
00345 }
00346 else if (!strcmp(argv[i], "--update-param"))
00347 {
00348 g_param.option |= OPTION_UPDATE_PARAM;
00349 }
00350 else if (!strcmp(argv[i], "--ssm-id"))
00351 {
00352 if (i + 1 < argc)
00353 {
00354 i++;
00355 g_param.ssm_id = atoi(argv[i]);
00356 }
00357 else
00358 {
00359 break;
00360 }
00361 }
00362 else
00363 {
00364 yprintf(OUTPUT_LV_ERROR, "ERROR : invalid option -- '%s'.\n", argv[i]);
00365 return 0;
00366 }
00367 }
00368
00369 if (i < argc)
00370 {
00371 yprintf(OUTPUT_LV_ERROR, "ERROR : option requires an argument -- '%s'.\n", argv[i]);
00372 return 0;
00373 }
00374
00375 return 1;
00376 }
00377
00378
00379 int parameter_set(char param, char id, long long int value64)
00380 {
00381 char buf[7];
00382 int value;
00383
00384 if (value64 > 0x7FFFFFFF || value64 < -0x7FFFFFFE)
00385 {
00386 yprintf(OUTPUT_LV_ERROR, "ERROR: parameter out of range (id: %d)\n", param);
00387 return -1;
00388 }
00389 value = value64;
00390
00391 if (option(OPTION_WITHOUT_DEVICE))
00392 {
00393 return (0);
00394 }
00395
00396 buf[0] = param;
00397 buf[1] = id;
00398 buf[2] = ((Int_4Char)value).byte[3];
00399 buf[3] = ((Int_4Char)value).byte[2];
00400 buf[4] = ((Int_4Char)value).byte[1];
00401 buf[5] = ((Int_4Char)value).byte[0];
00402 encode_write(buf, 6);
00403
00404 return (0);
00405 }
00406
00407 int is_character(int c)
00408 {
00409 if (c >= 'A' && c <= 'Z')
00410 return 1;
00411 if (c >= 'a' && c <= 'z')
00412 return 1;
00413 if (c == '_')
00414 return 1;
00415 return 0;
00416 }
00417
00418 int is_number(int c)
00419 {
00420 if (c >= '0' && c <= '9')
00421 return 1;
00422 if (c == '-')
00423 return 1;
00424 if (c == '.')
00425 return 1;
00426 return 0;
00427 }
00428
00429 void param_calc()
00430 {
00431 int i, j;
00432 for (i = 0; i < YP_PARAM_NUM; i++)
00433 {
00434 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
00435 {
00436 if (!g_param.motor_enable[j])
00437 continue;
00438 if (g_Pf[i][j])
00439 {
00440 double d;
00441 d = formula_eval(g_Pf[i][j]);
00442 yprintf(OUTPUT_LV_DEBUG, "Evaluated: [%d][%d] %f\n", i, j, d);
00443 }
00444 }
00445 }
00446 }
00447
00448
00449 int set_paramptr(FILE *paramfile)
00450 {
00451 char param_names[YP_PARAM_NUM][20] = YP_PARAM_NAME;
00452 char param_names0[YP_PARAM_NUM][24] = YP_PARAM_NAME;
00453 char param_names1[YP_PARAM_NUM][24] = YP_PARAM_NAME;
00454 int param_necessary[YP_PARAM_NUM] = YP_PARAM_NECESSARY;
00455 #define VARIABLE_NUM 37
00456 char variable_names[VARIABLE_NUM][20] =
00457 {
00458 "X", "Y", "THETA", "V", "W",
00459 "WHEEL_VEL[0]", "WHEEL_VEL[1]",
00460 "WHEEL_VEL[2]", "WHEEL_VEL[3]",
00461 "WHEEL_VEL[4]", "WHEEL_VEL[5]",
00462 "WHEEL_VEL[6]", "WHEEL_VEL[7]",
00463 "WHEEL_VEL[8]", "WHEEL_VEL[9]",
00464 "WHEEL_VEL[10]", "WHEEL_VEL[11]",
00465 "WHEEL_VEL[12]", "WHEEL_VEL[13]",
00466 "WHEEL_VEL[14]", "WHEEL_VEL[15]",
00467 "WHEEL_ANGLE[0]", "WHEEL_ANGLE[1]",
00468 "WHEEL_ANGLE[2]", "WHEEL_ANGLE[3]",
00469 "WHEEL_ANGLE[4]", "WHEEL_ANGLE[5]",
00470 "WHEEL_ANGLE[6]", "WHEEL_ANGLE[7]",
00471 "WHEEL_ANGLE[8]", "WHEEL_ANGLE[9]",
00472 "WHEEL_ANGLE[10]", "WHEEL_ANGLE[11]",
00473 "WHEEL_ANGLE[12]", "WHEEL_ANGLE[13]",
00474 "WHEEL_ANGLE[14]", "WHEEL_ANGLE[15]",
00475 };
00476 struct variables_t variables[YP_PARAM_NUM * 3 + 1 + VARIABLE_NUM];
00477 struct
00478 {
00479 YPSpur_param alias;
00480 YPSpur_param param;
00481 int motor;
00482 } param_alias[YP_PARAM_ALIAS_NUM] = YP_PARAM_ALIAS;
00483 char name[32], value_str[100];
00484 int i, j, c;
00485 int str_wp;
00486 int read_state;
00487 int param_num, motor_num;
00488 OdometryPtr odm;
00489 int param_error = 0;
00490
00491 for (i = 0; i < YP_PARAM_NUM; i++)
00492 {
00493 strcat(param_names0[i], "[0]");
00494 strcat(param_names1[i], "[1]");
00495 variables[i * 3 + 0].name = param_names[i];
00496 variables[i * 3 + 0].pointer = &g_P[i][0];
00497 variables[i * 3 + 1].name = param_names0[i];
00498 variables[i * 3 + 1].pointer = &g_P[i][0];
00499 variables[i * 3 + 2].name = param_names1[i];
00500 variables[i * 3 + 2].pointer = &g_P[i][1];
00501 }
00502 i = i * 3;
00503 for (j = 0; j < VARIABLE_NUM; j++)
00504 {
00505 variables[i + j].name = variable_names[j];
00506 }
00507 odm = get_odometry_ptr();
00508 variables[i++].pointer = &odm->x;
00509 variables[i++].pointer = &odm->y;
00510 variables[i++].pointer = &odm->theta;
00511 variables[i++].pointer = &odm->v;
00512 variables[i++].pointer = &odm->w;
00513 for (j = 0; j < 16; j++)
00514 variables[i++].pointer = &odm->wvel[j];
00515 for (j = 0; j < 16; j++)
00516 variables[i++].pointer = &odm->wang[j];
00517 variables[i].name = NULL;
00518 variables[i].pointer = NULL;
00519
00520 for (i = 0; i < YP_PARAM_NUM; i++)
00521 {
00522 int j;
00523 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
00524 {
00525 g_P_changed[i][j] = 0;
00526 }
00527 }
00528 if (g_param_init)
00529 {
00530 int j;
00531 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
00532 {
00533 g_P_changed[YP_PARAM_PWM_MAX][j] = 1;
00534 g_param.motor_enable[j] = 0;
00535 }
00536
00537 for (i = 0; i < YP_PARAM_NUM; i++)
00538 {
00539 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
00540 {
00541 g_P[i][j] = 0;
00542 g_P_set[i][j] = 0;
00543 g_Pf[i][j] = NULL;
00544 }
00545 }
00546 }
00547
00548
00549 read_state = 0;
00550 str_wp = 0;
00551 motor_num = 0;
00552 param_num = YP_PARAM_NUM;
00553 while (1)
00554 {
00555 int eof = 0;
00556
00557 c = getc(paramfile);
00558 if (c == EOF)
00559 {
00560 eof = 1;
00561 c = '\n';
00562 }
00563
00564 switch (read_state)
00565 {
00566 case 0:
00567
00568 if (c == '#')
00569 {
00570 read_state = 1;
00571 }
00572 if (is_character(c) || c == '-')
00573 {
00574 name[0] = c;
00575 read_state = 2;
00576 str_wp = 1;
00577 }
00578 break;
00579 case 1:
00580 if (c == '\n')
00581 {
00582 read_state = 0;
00583 }
00584 break;
00585 case 2:
00586 name[str_wp] = c;
00587 if (!(is_character(c) || is_number(c) || c == '[' || c == ']' || c == '-'))
00588 {
00589 name[str_wp] = 0;
00590 read_state = 3;
00591 str_wp = 0;
00592 }
00593 else
00594 {
00595 str_wp++;
00596 }
00597 break;
00598 case 3:
00599 if (is_number(c))
00600 {
00601 str_wp = 0;
00602 value_str[str_wp] = c;
00603 str_wp++;
00604 read_state = 4;
00605 }
00606 else if (c == '=')
00607 {
00608 strcpy(value_str, name);
00609 strcat(value_str, "=");
00610 str_wp = strlen(value_str);
00611 read_state = 5;
00612 }
00613 if (read_state != 3)
00614 {
00615 char *num_start;
00616 char *num_end = NULL;
00617 int num;
00618
00619 param_num = YP_PARAM_NUM;
00620 num = -1;
00621
00622 num_start = strchr(name, '[');
00623 if (num_start)
00624 {
00625 num_start++;
00626 num_end = strchr(name, ']');
00627 if (!num_end)
00628 {
00629 read_state = 4;
00630 yprintf(OUTPUT_LV_ERROR, "Error: Invalid parameter -- '%s'.\n", name);
00631 param_error = 1;
00632 continue;
00633 }
00634 *(num_start - 1) = 0;
00635 *num_end = 0;
00636 num = atoi(num_start);
00637 }
00638 motor_num = num;
00639 for (i = 0; i < YP_PARAM_NUM; i++)
00640 {
00641 if (!strcmp(name, param_names[i]))
00642 {
00643 int j;
00644 for (j = 0; j < YP_PARAM_ALIAS_NUM; j++)
00645 {
00646 if (i == param_alias[j].alias)
00647 {
00648 yprintf(OUTPUT_LV_WARNING, "Parameter alias: %s -> %s[%d]\n",
00649 param_names[i], param_names[param_alias[j].param], param_alias[j].motor);
00650 i = param_alias[j].param;
00651 motor_num = param_alias[j].motor;
00652 break;
00653 }
00654 }
00655 param_num = i;
00656 break;
00657 }
00658 }
00659 if (num_start)
00660 {
00661 *(num_start - 1) = '[';
00662 *num_end = ']';
00663 }
00664 }
00665 break;
00666 case 4:
00667 if (!is_number(c))
00668 {
00669 value_str[str_wp] = 0;
00670 str_wp = 0;
00671 if (param_num == YP_PARAM_NUM)
00672 {
00673 yprintf(OUTPUT_LV_ERROR, "Error: Invalid parameter -- '%s'.\n", name);
00674 param_error = 1;
00675 }
00676 else if (motor_num == -1)
00677 {
00678 int j;
00679 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
00680 {
00681 g_P[param_num][j] = strtod(value_str, 0);
00682 g_P_set[param_num][j] = 1;
00683 g_P_changed[param_num][j] = 1;
00684 if (g_Pf[param_num][j])
00685 {
00686 formula_free(g_Pf[param_num][j]);
00687 g_Pf[param_num][j] = NULL;
00688 }
00689 }
00690 yprintf(OUTPUT_LV_DEBUG, "%d %s %f\n", param_num, param_names[param_num], g_P[param_num][0]);
00691 }
00692 else
00693 {
00694 g_P[param_num][motor_num] = strtod(value_str, 0);
00695 g_P_set[param_num][motor_num] = 1;
00696 g_P_changed[param_num][motor_num] = 1;
00697 g_param.motor_enable[motor_num] = 1;
00698 if (g_Pf[param_num][motor_num])
00699 {
00700 formula_free(g_Pf[param_num][motor_num]);
00701 g_Pf[param_num][motor_num] = NULL;
00702 }
00703 yprintf(OUTPUT_LV_DEBUG, "%d %s[%d] %f\n", param_num, param_names[param_num], motor_num,
00704 g_P[param_num][motor_num]);
00705 }
00706 param_num = YP_PARAM_NUM;
00707 read_state = 0;
00708 str_wp = 0;
00709 break;
00710 }
00711 else
00712 {
00713 value_str[str_wp] = c;
00714 str_wp++;
00715 }
00716 break;
00717 case 5:
00718 if (c == '#' || c == '\n')
00719 {
00720 value_str[str_wp] = 0;
00721 str_wp = 0;
00722 if (param_num == YP_PARAM_NUM)
00723 {
00724 yprintf(OUTPUT_LV_ERROR, "Error: Invalid parameter -- '%s'.\n", name);
00725 param_error = 1;
00726 }
00727 else if (motor_num == -1)
00728 {
00729 int j;
00730 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
00731 {
00732 g_P[param_num][j] = 0;
00733 g_P_set[param_num][j] = 1;
00734 if (g_Pf[param_num][j])
00735 formula_free(g_Pf[param_num][j]);
00736 formula(value_str, &g_Pf[param_num][j], variables);
00737 }
00738 if (g_Pf[param_num][0] == NULL)
00739 {
00740 yprintf(OUTPUT_LV_ERROR, "Error: Invalid formula -- '%s'.\n", value_str);
00741 param_error = 1;
00742 }
00743 else
00744 {
00745 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
00746 {
00747 g_Pf[param_num][j] = formula_optimize(g_Pf[param_num][j]);
00748 }
00749 yprintf(OUTPUT_LV_DEBUG, "%d %s ", param_num, param_names[param_num]);
00750 if (output_lv() >= OUTPUT_LV_DEBUG)
00751 formula_print(stderr, g_Pf[param_num][0]);
00752 yprintf(OUTPUT_LV_DEBUG, "\n");
00753 }
00754 }
00755 else
00756 {
00757 g_P[param_num][motor_num] = 0;
00758 g_P_set[param_num][motor_num] = 1;
00759 if (g_Pf[param_num][motor_num])
00760 formula_free(g_Pf[param_num][motor_num]);
00761 formula(value_str, &g_Pf[param_num][motor_num], variables);
00762 if (g_Pf[param_num][motor_num] == NULL)
00763 {
00764 yprintf(OUTPUT_LV_ERROR, "Error: Invalid formula -- '%s'.\n", value_str);
00765 param_error = 1;
00766 }
00767 else
00768 {
00769 g_Pf[param_num][motor_num] = formula_optimize(g_Pf[param_num][motor_num]);
00770 yprintf(OUTPUT_LV_DEBUG, "%d %s[%d] ", param_num, param_names[param_num], motor_num);
00771 if (output_lv() >= OUTPUT_LV_DEBUG)
00772 formula_print(stderr, g_Pf[param_num][motor_num]);
00773 yprintf(OUTPUT_LV_DEBUG, "\n");
00774 }
00775 }
00776 param_num = YP_PARAM_NUM;
00777 read_state = 0;
00778 str_wp = 0;
00779 break;
00780 }
00781 else
00782 {
00783 value_str[str_wp] = c;
00784 str_wp++;
00785 }
00786 break;
00787 }
00788 if (eof)
00789 break;
00790 }
00791
00792 fclose(paramfile);
00793 if (g_P[YP_PARAM_VERSION][0] < YP_PARAM_REQUIRED_VERSION)
00794 {
00795 yprintf(OUTPUT_LV_ERROR, "Error: Your parameter file format is too old!\n");
00796 yprintf(OUTPUT_LV_ERROR, "Error: This program requires %3.1f.\n", YP_PARAM_REQUIRED_VERSION);
00797 return 0;
00798 }
00799 if (g_P[YP_PARAM_VERSION][0] > YP_PARAM_SUPPORTED_VERSION)
00800 {
00801 yprintf(OUTPUT_LV_ERROR, "Error: Your parameter file format is unsupported!\n");
00802 yprintf(OUTPUT_LV_ERROR, "Error: Please install newer version of YP-Spur.\n");
00803 yprintf(OUTPUT_LV_ERROR, "Error: This program supports %3.1f.\n", YP_PARAM_SUPPORTED_VERSION);
00804 return 0;
00805 }
00806
00807 for (i = 0; i < YP_PARAM_NUM; i++)
00808 {
00809 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
00810 {
00811 if (!g_param.motor_enable[j])
00812 continue;
00813 if (param_necessary[i] && !g_P_set[i][j])
00814 {
00815 yprintf(OUTPUT_LV_ERROR, "Error: %s[%d] undefined!\n", param_names[i], j);
00816 param_error = 1;
00817 }
00818 }
00819 }
00820 if (param_error)
00821 {
00822 return 0;
00823 }
00824
00825 g_param.num_motor_enable = 0;
00826 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
00827 {
00828
00829 if (g_param.motor_enable[j])
00830 g_param.num_motor_enable++;
00831 }
00832 if (g_param.num_motor_enable == 0)
00833 {
00834
00835 g_param.motor_enable[0] = g_param.motor_enable[1] = 1;
00836 g_param.num_motor_enable = 2;
00837 }
00838
00839 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
00840 {
00841 if (!g_param.motor_enable[j])
00842 continue;
00843
00844
00845 if (g_P_changed[YP_PARAM_ENCODER_DENOMINATOR][j] &&
00846 g_P_set[YP_PARAM_ENCODER_DENOMINATOR][j])
00847 {
00848 if (g_P[YP_PARAM_VERSION][0] < 5.0)
00849 {
00850 yprintf(
00851 OUTPUT_LV_ERROR,
00852 "ERROR: Using ENCODER_DENOMINATOR requires parameter version >= 5.0.\n"
00853 " Please make sure that all other motor parameters are based on mechanical motor revolution instead of electrical.\n");
00854 return 0;
00855 }
00856 yprintf(
00857 OUTPUT_LV_WARNING,
00858 "Warn: ENCODER_DENOMINATOR parameter support is experimental.\n"
00859 " The behavior of this parameter may be changed in the future update.\n");
00860 }
00861
00862
00863 if (!g_P_set[YP_PARAM_TORQUE_LIMIT][j])
00864 {
00865 yprintf(OUTPUT_LV_WARNING, "Warn: TORQUE_LIMIT[%d] undefined. TORQUE_MAX[%d] will be used.\n", j, j);
00866 g_P[YP_PARAM_TORQUE_LIMIT][j] = g_P[YP_PARAM_TORQUE_MAX][j];
00867 g_P_changed[YP_PARAM_TORQUE_LIMIT][j] = ischanged_p(YP_PARAM_TORQUE_MAX, j);
00868 }
00869
00870 if (!g_P_set[YP_PARAM_VEHICLE_CONTROL][j])
00871 {
00872 double default_value;
00873 if (j < 2)
00874 default_value = 1.0;
00875 else
00876 default_value = 0.0;
00877
00878 if (g_P[YP_PARAM_VEHICLE_CONTROL][j] != default_value)
00879 g_P_changed[YP_PARAM_VEHICLE_CONTROL][j] = 1;
00880 g_P[YP_PARAM_VEHICLE_CONTROL][j] = default_value;
00881 }
00882
00883 if (!g_P_set[YP_PARAM_ENCODER_TYPE][j])
00884 {
00885 if (g_P[YP_PARAM_ENCODER_TYPE][j] != 2.0)
00886 g_P_changed[YP_PARAM_ENCODER_TYPE][j] = 1;
00887 g_P[YP_PARAM_ENCODER_TYPE][j] = 2.0;
00888 }
00889 if (!g_P_set[YP_PARAM_INDEX_GEAR][j])
00890 {
00891 if (g_P[YP_PARAM_INDEX_GEAR][j] != 1.0)
00892 g_P_changed[YP_PARAM_INDEX_GEAR][j] = 1;
00893 g_P[YP_PARAM_INDEX_GEAR][j] = 1.0;
00894 }
00895 if (!g_P_set[YP_PARAM_ENCODER_DENOMINATOR][j])
00896 {
00897 if (g_P[YP_PARAM_ENCODER_DENOMINATOR][j] != 1.0)
00898 g_P_changed[YP_PARAM_ENCODER_DENOMINATOR][j] = 1;
00899 g_P[YP_PARAM_ENCODER_DENOMINATOR][j] = 1.0;
00900 }
00901
00902
00903 g_P[YP_PARAM_TORQUE_UNIT][j] = 1.0 / g_P[YP_PARAM_TORQUE_FINENESS][j];
00904 g_P_changed[YP_PARAM_TORQUE_UNIT][j] = ischanged_p(YP_PARAM_TORQUE_FINENESS, j);
00905 }
00906
00907
00908 calc_param_inertia2ff();
00909
00910
00911 enable_state(YP_STATE_MOTOR);
00912 enable_state(YP_STATE_VELOCITY);
00913 enable_state(YP_STATE_BODY);
00914 enable_state(YP_STATE_TRACKING);
00915
00916 yprintf(OUTPUT_LV_DEBUG, "Info: %d motors defined\n", g_param.num_motor_enable);
00917
00918 return 1;
00919 }
00920
00921
00922 int set_param(char *filename, char *concrete_path)
00923 {
00924 FILE *paramfile;
00925 paramfile = fopen(filename, "r");
00926
00927 if (!paramfile)
00928 {
00929 #if HAVE_PKG_CONFIG
00930 char dir_name[256];
00931 char file_name[256];
00932 char *pret;
00933 FILE *fd;
00934 #endif // HAVE_PKG_CONFIG
00935
00936 yprintf(OUTPUT_LV_DEBUG, "Warn: File [%s] is not exist.\n", filename);
00937
00938 #if HAVE_PKG_CONFIG
00939 if (!strchr(filename, '/'))
00940 {
00941
00942
00943 fd = popen("pkg-config --variable=YP_PARAMS_DIR yp-robot-params", "r");
00944 if ((fd == NULL))
00945 {
00946 yprintf(OUTPUT_LV_ERROR,
00947 "Error: Cannot open pipe 'pkg-config --variable=YP_PARAMS_DIR yp-robot-params'.\n");
00948 return 0;
00949 }
00950 pret = fgets(dir_name, sizeof(dir_name), fd);
00951
00952 if ((pclose(fd) == 0) && (pret != 0))
00953 {
00954 dir_name[strlen(dir_name) - 1] = '\0';
00955
00956 sprintf(file_name, "%s/%s", dir_name, filename);
00957
00958 paramfile = fopen(file_name, "r");
00959
00960 if (!paramfile)
00961 {
00962 yprintf(OUTPUT_LV_WARNING, "Warn: File [%s] is not exist.\n", file_name);
00963 return 0;
00964 }
00965 }
00966 else
00967 {
00968 yprintf(OUTPUT_LV_ERROR,
00969 "Error: Cannot read pipe 'pkg-config --variable=YP_PARAMS_DIR yp-robot-params'.\n");
00970 return 0;
00971 }
00972
00973 if (concrete_path)
00974 strcpy(concrete_path, file_name);
00975 strcpy(file_name, filename);
00976 }
00977 else
00978 #endif // HAVE_PKG_CONFIG
00979 {
00980 return 0;
00981 }
00982 }
00983 else
00984 {
00985 if (concrete_path)
00986 strcpy(concrete_path, filename);
00987 }
00988
00989 return set_paramptr(paramfile);
00990 }
00991
00992 void init_param_update_thread(pthread_t *thread, char *filename)
00993 {
00994 g_param.parameter_applying = 0;
00995 if (pthread_create(thread, NULL, (void *)param_update, filename) != 0)
00996 {
00997 yprintf(OUTPUT_LV_ERROR, "Can't create command thread\n");
00998 }
00999 }
01000
01001 void param_update_loop_cleanup(void *data)
01002 {
01003 yprintf(OUTPUT_LV_INFO, "Parameter updater stopped.\n");
01004 }
01005
01006 void param_update(void *filename)
01007 {
01008 struct stat prev_status;
01009
01010 yprintf(OUTPUT_LV_INFO, "Parameter updater started. Watching %s\n", filename);
01011 pthread_cleanup_push(param_update_loop_cleanup, filename);
01012
01013 stat(filename, &prev_status);
01014
01015 while (1)
01016 {
01017 int i;
01018 struct stat status;
01019
01020
01021 for (i = 0; i < 3; i++)
01022 {
01023 yp_usleep(100000);
01024 pthread_testcancel();
01025 }
01026 g_param.parameter_applying = 0;
01027 stat(filename, &status);
01028
01029 if (difftime(status.st_mtime, prev_status.st_mtime) != 0.0)
01030 {
01031 yp_usleep(100000);
01032 set_param(filename, NULL);
01033 if (!(option(OPTION_PARAM_CONTROL)))
01034 {
01035 g_param.parameter_applying = 1;
01036 apply_robot_params();
01037 }
01038 }
01039
01040 prev_status = status;
01041 }
01042 pthread_cleanup_pop(1);
01043 }
01044
01045
01046 int apply_robot_params()
01047 {
01048 yprintf(OUTPUT_LV_INFO, "Applying parameters.\n");
01049
01050 int j;
01051
01052 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
01053 {
01054 if (g_param.motor_enable[j])
01055 parameter_set(PARAM_watch_dog_limit, j, 1200);
01056 }
01057
01058 if (g_param_init)
01059 {
01060 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
01061 {
01062 if (g_param.motor_enable[j])
01063 parameter_set(PARAM_w_ref, j, 0);
01064 }
01065 g_param_init = 0;
01066 }
01067
01068 if (g_param.device_version >= 10)
01069 {
01070 int version, age;
01071 sscanf(YP_PROTOCOL_NAME, "YPP:%d:%d", &version, &age);
01072 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
01073 {
01074 if (g_param.motor_enable[j])
01075 parameter_set(PARAM_protocol_version, j, version);
01076 }
01077 }
01078
01079
01080 if (set_param_motor() < 1)
01081 return 0;
01082 yp_usleep(30000);
01083
01084
01085 if (set_param_velocity() < 1)
01086 return 0;
01087 yp_usleep(100000);
01088
01089 return 1;
01090 }
01091
01092
01093
01094 void calc_param_inertia2ff(void)
01095 {
01096 int i;
01097 double A, B, C, D;
01098 double M, J;
01099 double Gr, Gl;
01100 double Jmr, Jml;
01101 double Jtr, Jtl;
01102 double Rr, Rl;
01103 double T;
01104
01105
01106 M = g_P[YP_PARAM_MASS][0];
01107 J = g_P[YP_PARAM_MOMENT_INERTIA][0];
01108 Gr = fabs(g_P[YP_PARAM_GEAR][0]);
01109 Gl = fabs(g_P[YP_PARAM_GEAR][1]);
01110 Jmr = g_P[YP_PARAM_MOTOR_M_INERTIA][0];
01111 Jml = g_P[YP_PARAM_MOTOR_M_INERTIA][1];
01112 Jtr = g_P[YP_PARAM_TIRE_M_INERTIA][0];
01113 Jtl = g_P[YP_PARAM_TIRE_M_INERTIA][1];
01114 Rr = g_P[YP_PARAM_RADIUS][0];
01115 Rl = g_P[YP_PARAM_RADIUS][1];
01116 T = g_P[YP_PARAM_TREAD][0];
01117
01118
01119 A = (Gr * Gr * Jmr + Jtr + Rr * Rr / 2.0 * (M / 2.0 + J / (T * T))) / Gr;
01120 B = (Gl * Gl * Jml + Jtl + Rl * Rl / 2.0 * (M / 2.0 + J / (T * T))) / Gl;
01121 C = (Rr * Rl / 2.0 * (M / 2.0 - J / (T * T))) / Gr;
01122 D = (Rr * Rl / 2.0 * (M / 2.0 - J / (T * T))) / Gl;
01123
01124
01125 g_P[YP_PARAM_GAIN_A][0] = A;
01126 g_P[YP_PARAM_GAIN_B][0] = B;
01127 g_P[YP_PARAM_GAIN_C][0] = C;
01128 g_P[YP_PARAM_GAIN_D][0] = D;
01129
01130 g_P[YP_PARAM_INERTIA_SELF][0] = A;
01131 g_P[YP_PARAM_INERTIA_SELF][1] = B;
01132 g_P[YP_PARAM_INERTIA_CROSS][0] = C;
01133 g_P[YP_PARAM_INERTIA_CROSS][1] = D;
01134
01135 if (ischanged_p(YP_PARAM_MASS, 0) ||
01136 ischanged_p(YP_PARAM_MOMENT_INERTIA, 0) ||
01137 ischanged_p(YP_PARAM_GEAR, 0) ||
01138 ischanged_p(YP_PARAM_GEAR, 1) ||
01139 ischanged_p(YP_PARAM_MOTOR_M_INERTIA, 0) ||
01140 ischanged_p(YP_PARAM_MOTOR_M_INERTIA, 1) ||
01141 ischanged_p(YP_PARAM_TIRE_M_INERTIA, 0) ||
01142 ischanged_p(YP_PARAM_TIRE_M_INERTIA, 1) ||
01143 ischanged_p(YP_PARAM_RADIUS, 0) ||
01144 ischanged_p(YP_PARAM_RADIUS, 1) ||
01145 ischanged_p(YP_PARAM_TREAD, 0))
01146 {
01147 g_P_changed[YP_PARAM_GAIN_A][0] = 1;
01148 g_P_changed[YP_PARAM_GAIN_B][0] = 1;
01149 g_P_changed[YP_PARAM_GAIN_C][0] = 1;
01150 g_P_changed[YP_PARAM_GAIN_D][0] = 1;
01151 g_P_changed[YP_PARAM_INERTIA_SELF][0] = 1;
01152 g_P_changed[YP_PARAM_INERTIA_CROSS][0] = 1;
01153 g_P_changed[YP_PARAM_INERTIA_SELF][1] = 1;
01154 g_P_changed[YP_PARAM_INERTIA_CROSS][1] = 1;
01155 }
01156 for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
01157 {
01158 if (!g_param.motor_enable[i])
01159 continue;
01160 if (p(YP_PARAM_VEHICLE_CONTROL, i) > 0)
01161 continue;
01162
01163 g_P[YP_PARAM_INERTIA_SELF][i] =
01164 fabs(g_P[YP_PARAM_GEAR][i]) * g_P[YP_PARAM_MOTOR_M_INERTIA][i] + g_P[YP_PARAM_TIRE_M_INERTIA][i] / fabs(g_P[YP_PARAM_GEAR][i]);
01165 g_P[YP_PARAM_INERTIA_CROSS][i] = 0;
01166
01167 if (ischanged_p(YP_PARAM_GEAR, i) ||
01168 ischanged_p(YP_PARAM_MOTOR_M_INERTIA, i) ||
01169 ischanged_p(YP_PARAM_TIRE_M_INERTIA, i))
01170 {
01171 g_P_changed[YP_PARAM_INERTIA_SELF][i] = 1;
01172 g_P_changed[YP_PARAM_INERTIA_CROSS][i] = 1;
01173 }
01174 }
01175
01176 for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
01177 {
01178 if (!g_param.motor_enable[i])
01179 continue;
01180
01181 yprintf(OUTPUT_LV_DEBUG, " LOAD_INERTIA_SELF[%d] %f\n", i, g_P[YP_PARAM_INERTIA_SELF][i]);
01182 yprintf(OUTPUT_LV_DEBUG, " LOAD_INERTIA_CROSS[%d] %f\n", i, g_P[YP_PARAM_INERTIA_CROSS][i]);
01183 }
01184 }
01185
01186
01187 int set_param_motor(void)
01188 {
01189 int j;
01190
01191 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
01192 {
01193 if (!g_param.motor_enable[j])
01194 continue;
01195 if (ischanged_p(YP_PARAM_VOLT, j))
01196 {
01197 parameter_set(PARAM_vsrc, j, g_P[YP_PARAM_VOLT][j] * 256);
01198 }
01199 if (ischanged_p(YP_PARAM_CYCLE, j))
01200 {
01201 parameter_set(PARAM_control_cycle, j, g_P[YP_PARAM_CYCLE][j] * 1000);
01202 }
01203 if (ischanged_p(YP_PARAM_MOTOR_PHASE, j))
01204 {
01205 parameter_set(PARAM_motor_phase, j, g_P[YP_PARAM_MOTOR_PHASE][j]);
01206 }
01207 if (ischanged_p(YP_PARAM_PHASE_OFFSET, j) ||
01208 ischanged_p(YP_PARAM_ENCODER_DENOMINATOR, j) ||
01209 ischanged_p(YP_PARAM_COUNT_REV, j))
01210 {
01211 const double enc_rev_phase = g_P[YP_PARAM_COUNT_REV][j] / g_P[YP_PARAM_ENCODER_DENOMINATOR][j];
01212 parameter_set(PARAM_phase_offset, j,
01213 g_P[YP_PARAM_PHASE_OFFSET][j] * enc_rev_phase / (2.0 * M_PI));
01214 }
01215 if (ischanged_p(YP_PARAM_ENCODER_TYPE, j))
01216 {
01217 parameter_set(PARAM_enc_type, j, g_P[YP_PARAM_ENCODER_TYPE][j]);
01218 }
01219 if (ischanged_p(YP_PARAM_ENCODER_DIV, j))
01220 {
01221 parameter_set(PARAM_enc_div, j, g_P[YP_PARAM_ENCODER_DIV][j]);
01222 }
01223 if (ischanged_p(YP_PARAM_PWM_MAX, j) ||
01224 ischanged_p(YP_PARAM_MOTOR_R, j) ||
01225 ischanged_p(YP_PARAM_TORQUE_UNIT, j) ||
01226 ischanged_p(YP_PARAM_MOTOR_TC, j) ||
01227 ischanged_p(YP_PARAM_VOLT, j))
01228 {
01229
01230
01231
01232
01233
01234
01235 long long int ki;
01236 ki = (double)(65536.0 * g_P[YP_PARAM_PWM_MAX][j] * g_P[YP_PARAM_MOTOR_R][j] * g_P[YP_PARAM_ENCODER_DENOMINATOR][j] /
01237 (g_P[YP_PARAM_TORQUE_UNIT][j] * g_P[YP_PARAM_MOTOR_TC][j] *
01238 g_P[YP_PARAM_VOLT][j]));
01239 yprintf(OUTPUT_LV_DEBUG, "Info: TORQUE_CONSTANT[%d]: %d\n", j, (int)ki);
01240 if (ki == 0)
01241 {
01242 yprintf(OUTPUT_LV_ERROR, "ERROR: TORQUE_CONSTANT[%d] fixed point value underflow\n", j);
01243 yprintf(OUTPUT_LV_ERROR, "ERROR: Increase TORQUE_FINENESS[%d]\n", j);
01244 }
01245 parameter_set(PARAM_p_ki, j, ki);
01246 }
01247
01248 if (ischanged_p(YP_PARAM_PWM_MAX, j) ||
01249 ischanged_p(YP_PARAM_MOTOR_VC, j) ||
01250 ischanged_p(YP_PARAM_COUNT_REV, j) ||
01251 ischanged_p(YP_PARAM_CYCLE, j) ||
01252 ischanged_p(YP_PARAM_VOLT, j))
01253 {
01254
01255
01256
01257
01258
01259
01260 parameter_set(PARAM_p_kv, j,
01261 (double)(65536.0 * g_P[YP_PARAM_PWM_MAX][j] * 60.0 /
01262 (g_P[YP_PARAM_MOTOR_VC][j] * g_P[YP_PARAM_VOLT][j] * g_P[YP_PARAM_COUNT_REV][j] *
01263 g_P[YP_PARAM_CYCLE][j])));
01264 }
01265
01266
01267 if (ischanged_p(YP_PARAM_TORQUE_NEWTON, j) || ischanged_p(YP_PARAM_TORQUE_UNIT, j))
01268 {
01269 parameter_set(PARAM_p_fr_plus, j, g_P[YP_PARAM_TORQUE_NEWTON][j] * g_P[YP_PARAM_TORQUE_UNIT][j]);
01270 }
01271
01272 if (g_P_set[YP_PARAM_TORQUE_NEWTON_NEG][j])
01273 {
01274 if (ischanged_p(YP_PARAM_TORQUE_NEWTON_NEG, j) || ischanged_p(YP_PARAM_TORQUE_UNIT, j))
01275 {
01276 parameter_set(PARAM_p_fr_minus, j, g_P[YP_PARAM_TORQUE_NEWTON_NEG][j] * g_P[YP_PARAM_TORQUE_UNIT][j]);
01277 }
01278 }
01279 else
01280 {
01281 if (ischanged_p(YP_PARAM_TORQUE_NEWTON, j) || ischanged_p(YP_PARAM_TORQUE_UNIT, j))
01282 {
01283 parameter_set(PARAM_p_fr_minus, j, g_P[YP_PARAM_TORQUE_NEWTON][j] * g_P[YP_PARAM_TORQUE_UNIT][j]);
01284 }
01285 }
01286
01287
01288 if (ischanged_p(YP_PARAM_COUNT_REV, j) ||
01289 ischanged_p(YP_PARAM_CYCLE, j))
01290 {
01291 const double tvc = (2.0 * M_PI / g_P[YP_PARAM_COUNT_REV][j]) / g_P[YP_PARAM_CYCLE][j];
01292 if (ischanged_p(YP_PARAM_TORQUE_VISCOS, j) || ischanged_p(YP_PARAM_TORQUE_UNIT, j))
01293 {
01294 parameter_set(PARAM_p_fr_wplus, j, g_P[YP_PARAM_TORQUE_VISCOS][j] * g_P[YP_PARAM_TORQUE_UNIT][j] * tvc);
01295 }
01296 if (g_P_set[YP_PARAM_TORQUE_NEWTON_NEG][j])
01297 {
01298 if (ischanged_p(YP_PARAM_TORQUE_VISCOS_NEG, j) || ischanged_p(YP_PARAM_TORQUE_UNIT, j))
01299 {
01300 parameter_set(PARAM_p_fr_wminus, j, g_P[YP_PARAM_TORQUE_VISCOS_NEG][j] * g_P[YP_PARAM_TORQUE_UNIT][j] * tvc);
01301 }
01302 }
01303 else
01304 {
01305 if (ischanged_p(YP_PARAM_TORQUE_VISCOS_NEG, j) || ischanged_p(YP_PARAM_TORQUE_UNIT, j))
01306 {
01307 parameter_set(PARAM_p_fr_wminus, j, g_P[YP_PARAM_TORQUE_VISCOS][j] * g_P[YP_PARAM_TORQUE_UNIT][j] * tvc);
01308 }
01309 }
01310 }
01311
01312 if (ischanged_p(YP_PARAM_TORQUE_OFFSET, j) || ischanged_p(YP_PARAM_TORQUE_UNIT, j))
01313 {
01314 parameter_set(PARAM_p_toq_offset, j, g_P[YP_PARAM_TORQUE_OFFSET][j] * g_P[YP_PARAM_TORQUE_UNIT][j]);
01315 }
01316
01317 if (ischanged_p(YP_PARAM_TORQUE_MAX, j) || ischanged_p(YP_PARAM_TORQUE_UNIT, j))
01318 {
01319 parameter_set(PARAM_toq_max, j, g_P[YP_PARAM_TORQUE_MAX][j] * g_P[YP_PARAM_TORQUE_UNIT][j]);
01320 parameter_set(PARAM_toq_min, j, -g_P[YP_PARAM_TORQUE_MAX][j] * g_P[YP_PARAM_TORQUE_UNIT][j]);
01321 }
01322
01323 if (ischanged_p(YP_PARAM_TORQUE_LIMIT, j) || ischanged_p(YP_PARAM_TORQUE_UNIT, j))
01324 {
01325 parameter_set(PARAM_toq_limit, j, g_P[YP_PARAM_TORQUE_LIMIT][j] * g_P[YP_PARAM_TORQUE_UNIT][j]);
01326 }
01327
01328 if (ischanged_p(YP_PARAM_PWM_MAX, j))
01329 {
01330 parameter_set(PARAM_pwm_max, j, g_P[YP_PARAM_PWM_MAX][j]);
01331 parameter_set(PARAM_pwm_min, j, -g_P[YP_PARAM_PWM_MAX][j]);
01332 }
01333
01334 if (ischanged_p(YP_PARAM_COUNT_REV, j))
01335 {
01336 parameter_set(PARAM_enc_rev, j, g_P[YP_PARAM_COUNT_REV][j]);
01337 }
01338 if (ischanged_p(YP_PARAM_ENCODER_DENOMINATOR, j) &&
01339 isset_p(YP_PARAM_ENCODER_DENOMINATOR, j))
01340 {
01341 if (g_param.device_version <= 9)
01342 {
01343 yprintf(OUTPUT_LV_ERROR, "ERROR: the device doesn't support ENCODER_DENOMINATOR\n");
01344 return 0;
01345 }
01346 parameter_set(PARAM_enc_denominator, j, g_P[YP_PARAM_ENCODER_DENOMINATOR][j]);
01347 }
01348
01349
01350 yp_usleep(20000);
01351 }
01352 return 1;
01353 }
01354
01355 int set_param_velocity(void)
01356 {
01357 int j;
01358
01359 if (g_param.device_version <= 6)
01360 {
01361 double ffr, ffl;
01362 int ffr_changed = 0, ffl_changed = 0;
01363
01364
01365
01366
01367
01368
01369
01370
01371 if (ischanged_p(YP_PARAM_TORQUE_UNIT, 0) ||
01372 ischanged_p(YP_PARAM_COUNT_REV, 0) ||
01373 ischanged_p(YP_PARAM_GEAR, 0))
01374 {
01375 ffr_changed = 1;
01376 }
01377 ffr = 256.0 * 2.0 * M_PI * g_P[YP_PARAM_TORQUE_UNIT][0] / (g_P[YP_PARAM_COUNT_REV][0] * fabs(g_P[YP_PARAM_GEAR][0]));
01378 if (ischanged_p(YP_PARAM_GAIN_A, 0) || ffr_changed)
01379 {
01380 yprintf(OUTPUT_LV_DEBUG, "Info: GAIN_A: %d\n",
01381 (int)(g_P[YP_PARAM_GAIN_A][0] * ffr));
01382 if (abs(g_P[YP_PARAM_GAIN_A][0] * ffr) == 0)
01383 {
01384 yprintf(OUTPUT_LV_ERROR, "ERROR: GAIN_A fixed point value underflow\n");
01385 yprintf(OUTPUT_LV_ERROR, "ERROR: Decrease TORQUE_FINENESS[%d]\n", 0);
01386 return 0;
01387 }
01388 parameter_set(PARAM_p_A, 0, g_P[YP_PARAM_GAIN_A][0] * ffr);
01389 }
01390 if (ischanged_p(YP_PARAM_GAIN_C, 0) || ffr_changed)
01391 parameter_set(PARAM_p_C, 0, g_P[YP_PARAM_GAIN_C][0] * ffr);
01392 if (ischanged_p(YP_PARAM_GAIN_E, 0) || ffr_changed)
01393 parameter_set(PARAM_p_E, 0, g_P[YP_PARAM_GAIN_E][0] * ffr);
01394
01395 if (ischanged_p(YP_PARAM_TORQUE_UNIT, 1) ||
01396 ischanged_p(YP_PARAM_COUNT_REV, 1) ||
01397 ischanged_p(YP_PARAM_GEAR, 1))
01398 {
01399 ffl_changed = 1;
01400 }
01401 ffl = 256.0 * 2.0 * M_PI * g_P[YP_PARAM_TORQUE_UNIT][1] / (g_P[YP_PARAM_COUNT_REV][1] * fabs(g_P[YP_PARAM_GEAR][1]));
01402 if (ischanged_p(YP_PARAM_GAIN_B, 0) || ffl_changed)
01403 {
01404 yprintf(OUTPUT_LV_DEBUG, "Info: GAIN_B: %d\n",
01405 (int)(g_P[YP_PARAM_GAIN_A][0] * ffl));
01406 if (abs(g_P[YP_PARAM_GAIN_B][0] * ffl) == 0)
01407 {
01408 yprintf(OUTPUT_LV_ERROR, "ERROR: GAIN_B fixed point value underflow\n");
01409 yprintf(OUTPUT_LV_ERROR, "ERROR: Decrease TORQUE_FINENESS[%d]\n", 1);
01410 return 0;
01411 }
01412 parameter_set(PARAM_p_B, 0, g_P[YP_PARAM_GAIN_B][0] * ffl);
01413 }
01414 if (ischanged_p(YP_PARAM_GAIN_D, 0) || ffl_changed)
01415 parameter_set(PARAM_p_D, 0, g_P[YP_PARAM_GAIN_D][0] * ffl);
01416 if (ischanged_p(YP_PARAM_GAIN_F, 0) || ffl_changed)
01417 parameter_set(PARAM_p_F, 0, g_P[YP_PARAM_GAIN_F][0] * ffl);
01418 }
01419
01420 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
01421 {
01422 if (!g_param.motor_enable[j])
01423 continue;
01424
01425 if (g_param.device_version > 6)
01426 {
01427 int ff_changed = 0;
01428 double ff = 256.0 * 2.0 * M_PI * g_P[YP_PARAM_TORQUE_UNIT][j] /
01429 (g_P[YP_PARAM_COUNT_REV][j] * fabs(g_P[YP_PARAM_GEAR][j]));
01430 if (ischanged_p(YP_PARAM_TORQUE_UNIT, j) ||
01431 ischanged_p(YP_PARAM_COUNT_REV, j) ||
01432 ischanged_p(YP_PARAM_GEAR, j))
01433 {
01434 ff_changed = 1;
01435 }
01436 if (ischanged_p(YP_PARAM_INERTIA_SELF, j) || ff_changed)
01437 {
01438 yprintf(OUTPUT_LV_DEBUG, "Info: INERTIA_SELF[%d]: %d\n", j,
01439 (int)(g_P[YP_PARAM_INERTIA_SELF][j] * ff));
01440 if (abs(g_P[YP_PARAM_INERTIA_SELF][j] * ff) == 0)
01441 {
01442 yprintf(OUTPUT_LV_ERROR, "ERROR: INERTIA_SELF[%d] fixed point value underflow\n", j);
01443 yprintf(OUTPUT_LV_ERROR, "ERROR: Decrease TORQUE_FINENESS[%d]\n", j);
01444 return 0;
01445 }
01446 parameter_set(PARAM_p_inertia_self, j, g_P[YP_PARAM_INERTIA_SELF][j] * ff);
01447 }
01448 if (ischanged_p(YP_PARAM_INERTIA_CROSS, j) || ff_changed)
01449 {
01450 parameter_set(PARAM_p_inertia_cross, j, g_P[YP_PARAM_INERTIA_CROSS][j] * ff);
01451 }
01452 }
01453
01454
01455 if (ischanged_p(YP_PARAM_GAIN_KP, j))
01456 parameter_set(PARAM_p_pi_kp, j, g_P[YP_PARAM_GAIN_KP][j]);
01457
01458 if (ischanged_p(YP_PARAM_GAIN_KI, j))
01459 parameter_set(PARAM_p_pi_ki, j, g_P[YP_PARAM_GAIN_KI][j]);
01460
01461 if (ischanged_p(YP_PARAM_INTEGRAL_MAX, j) ||
01462 ischanged_p(YP_PARAM_COUNT_REV, j) ||
01463 ischanged_p(YP_PARAM_GEAR, j))
01464 {
01465 parameter_set(PARAM_int_max, j,
01466 g_P[YP_PARAM_INTEGRAL_MAX][j] * g_P[YP_PARAM_COUNT_REV][j] * fabs(g_P[YP_PARAM_GEAR][j]));
01467 parameter_set(PARAM_int_min, j,
01468 -g_P[YP_PARAM_INTEGRAL_MAX][j] * g_P[YP_PARAM_COUNT_REV][j] * fabs(g_P[YP_PARAM_GEAR][j]));
01469 }
01470 }
01471
01472
01473 for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
01474 {
01475 if (!g_param.motor_enable[j])
01476 continue;
01477 parameter_set(PARAM_watch_dog_limit, j, 300);
01478 }
01479 return 1;
01480 }