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 <strings.h>
00025 #include <unistd.h>
00026
00027 #include <fcntl.h>
00028 #include <setjmp.h>
00029 #include <signal.h>
00030 #include <sys/stat.h>
00031 #include <sys/time.h>
00032 #include <sys/types.h>
00033 #include <time.h>
00034
00035 #ifdef HAVE_CONFIG_H
00036 #include <config.h>
00037 #endif // HAVE_CONFIG_H
00038
00039
00040 #include <shvel-param.h>
00041
00042
00043 #include <ypspur-coordinator.h>
00044 #include <serial.h>
00045 #include <param.h>
00046 #include <control.h>
00047 #include <command.h>
00048 #include <odometry.h>
00049 #include <ypprotocol.h>
00050 #include <adinput.h>
00051 #include <ssm_spur_handler.h>
00052 #include <utility.h>
00053 #include <yprintf.h>
00054
00055
00056 #include <ypspur.h>
00057 #include <cartesian2d.h>
00058
00059 #include <pthread.h>
00060
00061 void escape_road(void);
00062 void emergency(int);
00063 #if HAVE_SIGLONGJMP
00064 sigjmp_buf ctrlc_capture;
00065 #elif HAVE_LONGJMP
00066 jmp_buf ctrlc_capture;
00067 #endif // HAVE_SIGLONGJMP
00068 int g_emergency;
00069
00070 #if defined(__MINGW32__)
00071 BOOL WINAPI win32_ctrlc_handler(DWORD type)
00072 {
00073 fprintf(stderr, "\n");
00074 g_emergency = 1;
00075 #ifdef HAVE_SSM
00076
00077 if (!option(OPTION_WITHOUT_SSM))
00078 end_ypspurSSM();
00079 #endif // HAVE_SSM
00080 if (!(option(OPTION_WITHOUT_DEVICE)))
00081 {
00082 serial_close();
00083 }
00084
00085 return TRUE;
00086 }
00087 #else
00088 void emergency(int sig)
00089 {
00090 fprintf(stderr, "\n");
00091 g_emergency = 1;
00092 #if HAVE_SIGLONGJMP
00093 siglongjmp(ctrlc_capture, 1);
00094 #elif HAVE_LONGJMP
00095 longjmp(ctrlc_capture, 1);
00096 #else
00097 #ifdef HAVE_SSM
00098
00099 if (!option(OPTION_WITHOUT_SSM))
00100 end_ypspurSSM();
00101 #endif // HAVE_SSM
00102 if (!(option(OPTION_WITHOUT_DEVICE)))
00103 {
00104 serial_close();
00105 }
00106
00107 exit(0);
00108 #endif // HAVE_SIGLONGJMP
00109 }
00110 #endif // defined(__MINGW32__)
00111
00112 void escape_road(void)
00113 {
00114 #if defined(__MINGW32__)
00115 if (!SetConsoleCtrlHandler(win32_ctrlc_handler, TRUE))
00116 {
00117 yprintf(OUTPUT_LV_ERROR, "Error: Win32 Ctrl+C handler registration failed.\n");
00118 }
00119 #else
00120 signal(SIGINT, emergency);
00121 #endif // defined(__MINGW32__)
00122 }
00123
00124
00125 int main(int argc, char *argv[])
00126 {
00127 pthread_t command_thread;
00128 pthread_t control_thread;
00129 pthread_t update_thread;
00130 int command_thread_en;
00131 int control_thread_en;
00132 int update_thread_en;
00133 Ver_t version;
00134 Param_t driver_param;
00135 int i, ret;
00136 ParametersPtr param;
00137 char paramfile[512];
00138 int quit;
00139
00140 hook_pre_global();
00141
00142 ret = arg_analyze(argc, argv);
00143 if (option(OPTION_DAEMON))
00144 {
00145 #if HAVE_FORK
00146 pid_t pid;
00147
00148 pid = fork();
00149 if (pid < 0)
00150 {
00151 return -1;
00152 }
00153 else if (pid != 0)
00154 {
00155 return 0;
00156 }
00157 setsid();
00158 if (chdir("/") < 0)
00159 {
00160 yprintf(OUTPUT_LV_ERROR, "Failed to chdir.\n");
00161 return EXIT_FAILURE;
00162 }
00163 close(STDIN_FILENO);
00164 close(STDOUT_FILENO);
00165 close(STDERR_FILENO);
00166 #else
00167 yprintf(OUTPUT_LV_ERROR, "Daemon mode is not supported in your system.\n");
00168 return EXIT_FAILURE;
00169 #endif // HAVE_FORK
00170 }
00171 if (option(OPTION_SHOW_HELP))
00172 {
00173 arg_help(argc, argv);
00174 return EXIT_SUCCESS;
00175 }
00176 if (option(OPTION_SHOW_LONGHELP))
00177 {
00178 arg_longhelp(argc, argv);
00179 return EXIT_SUCCESS;
00180 }
00181 if (option(OPTION_SHOW_PARAMHELP))
00182 {
00183 param_help();
00184 return EXIT_SUCCESS;
00185 }
00186 if (option(OPTION_VERSION))
00187 {
00188 fprintf(stderr, "YamabicoProject-Spur\n");
00189 fprintf(stderr, " Ver. %s\n", PROJECT_VERSION);
00190 return EXIT_SUCCESS;
00191 }
00192 if (!ret)
00193 return EXIT_FAILURE;
00194
00195 yprintf(OUTPUT_LV_INFO, "++++++++++++++++++++++++++++++++++++++++++++++++++\n");
00196 yprintf(OUTPUT_LV_INFO, "YamabicoProject-Spur\n");
00197 yprintf(OUTPUT_LV_INFO, " Ver. %s\n", PROJECT_VERSION);
00198 yprintf(OUTPUT_LV_INFO, "++++++++++++++++++++++++++++++++++++++++++++++++++\n");
00199
00200
00201 escape_road();
00202 g_emergency = 0;
00203
00204
00205 param = get_param_ptr();
00206
00207 #ifdef HAVE_SSM
00208
00209 if (!option(OPTION_WITHOUT_SSM))
00210 init_ypspurSSM(param->ssm_id);
00211 #endif // HAVE_SSM
00212
00213
00214 init_coordinate_systems();
00215 init_odometry();
00216 init_spur_command();
00217
00218 fflush(stderr);
00219
00220 command_thread_en = 0;
00221 command_thread_en = 0;
00222 do
00223 {
00224 FILE *temp_paramfile = NULL;
00225 quit = 0;
00226
00227 yprintf(OUTPUT_LV_INFO, "Device Information\n");
00228
00229 if (!(option(OPTION_WITHOUT_DEVICE)))
00230 {
00231 yprintf(OUTPUT_LV_INFO, " Port : %s \n", param->device_name);
00232 if (!serial_connect(param->device_name))
00233 {
00234
00235 break;
00236 }
00237 if (!(option(OPTION_DO_NOT_USE_YP)))
00238 {
00239 int current, age;
00240 int device_current, device_age;
00241 sscanf(YP_PROTOCOL_NAME, "YPP:%d:%d", ¤t, &age);
00242
00243 yprintf(OUTPUT_LV_INFO, " Checking device information");
00244 for (i = 0; i < 3; i++)
00245 {
00246 yprintf(OUTPUT_LV_INFO, ".");
00247
00248 if (get_version(&version) == -1)
00249 {
00250 continue;
00251 }
00252 if (strstr(version.protocol, "YPP:") != version.protocol)
00253 {
00254 continue;
00255 }
00256 sscanf(version.protocol, "YPP:%d:%d", &device_current, &device_age);
00257 param->device_version = device_current;
00258 param->device_version_age = device_age;
00259 if (device_current - device_age > current ||
00260 device_current < current - age)
00261 {
00262 continue;
00263 }
00264 break;
00265 }
00266 ansi_clear_line(OUTPUT_LV_INFO);
00267 yprintf(OUTPUT_LV_INFO, " Vender : %s\n", version.vender);
00268 yprintf(OUTPUT_LV_INFO, " Product : %s\n", version.product);
00269 yprintf(OUTPUT_LV_INFO, " Firmware: %s\n", version.firmware);
00270 yprintf(OUTPUT_LV_INFO, " Protcol : %s\n", version.protocol);
00271 yprintf(OUTPUT_LV_INFO, " Serialno: %s\n", version.serialno);
00272 yprintf(OUTPUT_LV_INFO, "++++++++++++++++++++++++++++++++++++++++++++++++++\n");
00273 if (i == 3)
00274 {
00275 yprintf(OUTPUT_LV_ERROR, "Error: Device doesn't have available YP protocol version.\n(Device: %s, coordinator: %s)\n",
00276 version.protocol, YP_PROTOCOL_NAME);
00277 if (option(OPTION_RECONNECT) && g_emergency == 0)
00278 {
00279 yp_usleep(500000);
00280 continue;
00281 }
00282 break;
00283 }
00284 if (device_current != current)
00285 {
00286 if (device_current < current)
00287 {
00288 yprintf(OUTPUT_LV_WARNING, "Recommendation: Device protocol version is not latest.\n");
00289 yprintf(OUTPUT_LV_WARNING, "Recommendation: Firmware update is recommended.\n");
00290 }
00291 else
00292 {
00293 yprintf(OUTPUT_LV_WARNING, "Recommendation: ypspur-coordinator protocol version is not latest.\n");
00294 yprintf(OUTPUT_LV_WARNING, "Recommendation: Software update is recommended.\n");
00295 }
00296 yprintf(OUTPUT_LV_WARNING, "++++++++++++++++++++++++++++++++++++++++++++++++++\n");
00297 }
00298 }
00299 fflush(stderr);
00300
00301 if (get_parameter(&driver_param) == -1)
00302 {
00303 continue;
00304 }
00305 yprintf(OUTPUT_LV_DEBUG, "Driver depending parameters\n");
00306 yprintf(OUTPUT_LV_DEBUG, " Name : %s\n", driver_param.robot_name);
00307 yprintf(OUTPUT_LV_DEBUG, " PWM resolution: %s\n", driver_param.pwm_resolution);
00308 yprintf(OUTPUT_LV_DEBUG, " Motor number : %s\n", driver_param.motor_num);
00309 yprintf(OUTPUT_LV_DEBUG, "++++++++++++++++++++++++++++++++++++++++++++++++++\n");
00310
00311 if (strlen(driver_param.pwm_resolution) <= 0 ||
00312 strlen(driver_param.motor_num) <= 0)
00313 {
00314 yprintf(OUTPUT_LV_ERROR, "Error: Failed to load driver parameters.\n");
00315 if (option(OPTION_RECONNECT) && g_emergency == 0)
00316 {
00317 yp_usleep(500000);
00318 continue;
00319 }
00320 break;
00321 }
00322 }
00323 else
00324 {
00325 yprintf(OUTPUT_LV_INFO, " Port : n/a (--without-device mode)\n");
00326 }
00327 if (!(option(OPTION_PARAM_FILE)))
00328 {
00329
00330 if (strcmp(driver_param.robot_name, "embedded") == 0)
00331 {
00332 char param[2048];
00333
00334 yprintf(OUTPUT_LV_INFO, "Reading device embedded parameter.\n");
00335 temp_paramfile = tmpfile();
00336 if (!temp_paramfile)
00337 {
00338 yprintf(OUTPUT_LV_ERROR, "Error: Failed to create temporary file.\n");
00339 break;
00340 }
00341 if (!get_embedded_param(param))
00342 {
00343 yprintf(OUTPUT_LV_ERROR, "Error: Failed to read embedded parameters.\n");
00344 if (option(OPTION_RECONNECT) && g_emergency == 0)
00345 {
00346 yp_usleep(500000);
00347 continue;
00348 }
00349 break;
00350 }
00351 fprintf(temp_paramfile, "%s", param);
00352 fseek(temp_paramfile, 0L, SEEK_SET);
00353 }
00354
00355 else if (strlen(driver_param.robot_name) > 0 && strcmp(driver_param.robot_name, "unknown") != 0)
00356 {
00357 strcpy(param->parameter_filename, driver_param.robot_name);
00358 strcat(param->parameter_filename, ".param");
00359 }
00360 }
00361 if (temp_paramfile)
00362 {
00363 yprintf(OUTPUT_LV_DEBUG, "Embedded parameter file\n");
00364 if (!set_paramptr(temp_paramfile))
00365 {
00366 yprintf(OUTPUT_LV_ERROR, "Error: Cannot use embedded parameter.\n");
00367 break;
00368 }
00369 }
00370 else
00371 {
00372 yprintf(OUTPUT_LV_DEBUG, "Parameter file: %s\n", param->parameter_filename);
00373 if (!set_param(param->parameter_filename, paramfile))
00374 {
00375 yprintf(OUTPUT_LV_ERROR, "Error: Cannot load parameter file.\n");
00376 break;
00377 }
00378 }
00379 {
00380 int i;
00381 for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
00382 {
00383 *pp(YP_PARAM_PWM_MAX, i) = atoi(driver_param.pwm_resolution);
00384 }
00385 }
00386 yprintf(OUTPUT_LV_DEBUG, "++++++++++++++++++++++++++++++++++++++++++++++++++\n\n");
00387
00388 if (!(option(OPTION_WITHOUT_DEVICE)))
00389 {
00390
00391 if (param->speed)
00392 {
00393 yprintf(OUTPUT_LV_INFO, "Setting baudrate to %d baud.\n", param->speed);
00394 }
00395 else
00396 {
00397
00398 param->speed = DEFAULT_BAUDRATE;
00399 }
00400
00401 ret = set_baudrate(param->speed);
00402 if (ret == 0)
00403 {
00404
00405 yprintf(OUTPUT_LV_WARNING, "Error: Failed to change baudrate.\n");
00406 break;
00407 }
00408 if (ret == 4)
00409 {
00410
00411 yprintf(OUTPUT_LV_INFO, "Info: Baudrate setting is not supported on this device.\n");
00412 }
00413 else
00414 {
00415
00416
00417 }
00418
00419 if (param->admask)
00420 {
00421 yprintf(OUTPUT_LV_INFO, "Setting admask to %x.\n", param->admask);
00422 set_admask(param->admask);
00423 }
00424
00425 if (option(OPTION_ENABLE_GET_DIGITAL_IO))
00426 {
00427 yprintf(OUTPUT_LV_INFO, "Enabling digital io input.\n");
00428 set_diomask(1);
00429 }
00430
00431 if (!(option(OPTION_PARAM_CONTROL)))
00432 if (apply_robot_params() < 1)
00433 break;
00434
00435
00436 SpurUserParamsPtr spur;
00437 spur = get_spur_user_param_ptr();
00438 for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
00439 {
00440 spur->wheel_mode[i] = MOTOR_CONTROL_VEL;
00441 spur->wheel_mode_prev[i] = -1;
00442 }
00443 }
00444
00445 yprintf(OUTPUT_LV_INFO, "YP-Spur coordinator started.\n");
00446
00447
00448 init_command_thread(&command_thread);
00449 command_thread_en = 1;
00450
00451 if (!(option(OPTION_WITHOUT_DEVICE)))
00452 {
00453 init_control_thread(&control_thread);
00454 control_thread_en = 1;
00455 }
00456 else
00457 {
00458 control_thread_en = 0;
00459 }
00460 if (option(OPTION_UPDATE_PARAM))
00461 {
00462 yprintf(OUTPUT_LV_WARNING, "==================== Warning! ====================\n");
00463 yprintf(OUTPUT_LV_WARNING, "Running in automatic parameter update mode.\n");
00464 yprintf(OUTPUT_LV_WARNING, "Keep in mind that the coordinator doesn't stop\n");
00465 yprintf(OUTPUT_LV_WARNING, "even if the parameter file gets invalid.\n");
00466 yprintf(OUTPUT_LV_WARNING, "==================================================\n");
00467 init_param_update_thread(&update_thread, paramfile);
00468 update_thread_en = 1;
00469 }
00470 else
00471 {
00472 update_thread_en = 0;
00473 }
00474
00475
00476 #if HAVE_SIGLONGJMP
00477 if (sigsetjmp(ctrlc_capture, 1) != 0)
00478 {
00479 quit = 1;
00480 }
00481 else
00482 #elif HAVE_LONGJMP
00483 if (setjmp(ctrlc_capture) != 0)
00484 {
00485 quit = 1;
00486 }
00487 else
00488 #endif // HAVE_SIGLONGJMP
00489 {
00490 if (!(option(OPTION_WITHOUT_DEVICE)))
00491 {
00492 odometry_receive_loop();
00493 }
00494 else
00495 {
00496 while (1)
00497 yp_usleep(1000000);
00498 }
00499 yprintf(OUTPUT_LV_INFO, "Connection to %s was closed.\n", param->device_name);
00500 }
00501
00502
00503 if (update_thread_en)
00504 {
00505 pthread_cancel(update_thread);
00506 pthread_join(update_thread, NULL);
00507 update_thread_en = 0;
00508 }
00509 if (control_thread_en)
00510 {
00511 pthread_cancel(control_thread);
00512 pthread_join(control_thread, NULL);
00513 control_thread_en = 0;
00514 }
00515 if (command_thread_en)
00516 {
00517 pthread_cancel(command_thread);
00518 pthread_join(command_thread, NULL);
00519 command_thread_en = 0;
00520 }
00521
00522 if (option(OPTION_RECONNECT) && quit == 0)
00523 {
00524 init_spur_command();
00525 yp_usleep(500000);
00526 if (!(option(OPTION_WITHOUT_DEVICE)))
00527 {
00528 serial_close();
00529 while (!serial_tryconnect(param->device_name))
00530 {
00531 yp_usleep(200000);
00532 }
00533 }
00534 yprintf(OUTPUT_LV_INFO, "++++++++++++++++++++++++++++++++++++++++++++++++++\n");
00535 yp_usleep(500000);
00536 continue;
00537 }
00538 break;
00539 } while (1);
00540
00541 if (!(option(OPTION_WITHOUT_DEVICE)))
00542 serial_close();
00543
00544 #ifdef HAVE_SSM
00545
00546 if (!option(OPTION_WITHOUT_SSM))
00547 end_ypspurSSM();
00548 #endif // HAVE_SSM
00549
00550 yp_usleep(200000);
00551 fflush(stderr);
00552
00553 return (quit ? EXIT_SUCCESS : EXIT_FAILURE);
00554 }