run_ctrl.c

00001 /*
00002   走行制御ライブラリ
00003   Satofumi KAMIMURA
00004   $Id$
00005 */
00006 #define C_RUNCTRL_SOURCE
00007 
00008 #include "run_ctrl.h"
00009 #include "tRunCtrl.h"
00010 #include "connect_device.h"
00011 #include "commandCtrl.h"
00012 #include "get_keyword.h"
00013 #include "nodeAccess.h"
00014 #include "parameter_ctrl.h"
00015 #include <stdio.h>
00016 #include <string.h>
00017 #include <stdlib.h>
00018 
00019 extern void run_initCommandCtrl(int *Con, runCtrl_t *Run, nodeInfo_t *Node,
00020                                 runParams_t *Params, int *Unique_id);
00021 
00022 
00023 static char *Error_message = "connection device is not specified";
00024 static char *Error_device = "";
00025 static int Error_baudrate = RUN_CTRL_BAUDRATE;
00026 static int fd = -1;
00027 
00028 enum { NO_FILE_ERROR = -2 };
00029 
00035 #ifndef RUN_CONFIG_FILE
00036 #define RUN_CONFIG_FILE "b5conf.txt"
00037 #endif
00038 
00039 
00040 static int checkVersion(void) {
00041   int ret_value;
00042 #if 0
00043   long target_version = 0;
00044   if (recvVersion(&target_version) < 0) {
00045     Error_message = "Connection fail: recvVersion";
00046     return -1;
00047   }
00048 
00049   if (target_version/10 != PACKAGE_NUM_VERSION/10) {
00050     fprintf(stderr,
00051             "warnning: RunCtrl version mismatch between PC(%d) and SH2(%ld)\n",
00052             PACKAGE_NUM_VERSION, target_version);
00053     Error_message = "RunCtrl version mismatch between host and target";
00054     closeDevice(fd);
00055     return -1;
00056   }
00057 #endif
00058 
00059 #if 1
00060   ret_value = sendPositionInit();
00061   if (ret_value < 0) {
00062     Error_message = "Transmit fail: in sendPositionInit()";
00063     return ret_value;
00064   }
00065 #endif
00066 
00067   // サーボをかける
00068   if (sendServoCtrl(1) < 0) {
00069     Error_message = "Connection fail: sendServoCtrl";
00070     closeDevice(fd);
00071     return -1;
00072   }
00073 
00074   return 0;
00075 }
00076 
00077 
00078 #include "get_keyword.c"
00079 
00080 static int searchConfigFile(const char *path) {
00081   int path_length = strlen(path);
00082   int file_length = strlen(RUN_CONFIG_FILE);
00083   char *file_path = (char *)malloc(path_length + 1 + file_length + 1);
00084   long baudrate;
00085   char *baudrate_str;
00086   char *deviceName;
00087 
00088   strcpy(file_path, path);
00089   strcpy(&file_path[path_length], "/");
00090   strcpy(&file_path[path_length +1], RUN_CONFIG_FILE);
00091   file_path[path_length + 1 + file_length] = '\0';
00092 
00093   baudrate = RUN_CTRL_BAUDRATE;
00094   if ((baudrate_str = get_keyword(file_path, "run_baudrate"))) {
00095     baudrate = atoi(baudrate_str);
00096     free(baudrate_str);
00097   }
00098   deviceName = get_keyword(file_path, "run_port");
00099   free(file_path);
00100   if (deviceName) {
00101     int ret_value = initConnectDevice(deviceName, baudrate, SERIAL);
00102     if (ret_value >= 0) {
00103       fd = ret_value;
00104       ret_value = checkVersion();
00105     }
00106     free(deviceName);
00107     return ret_value;
00108   }
00109   return NO_FILE_ERROR;
00110 }
00111 
00112 
00113 static int connectDefault(void) {
00114   int ret_value = searchConfigFile(".");
00115   char *home;
00116   long baudrate;
00117   char *deviceName;
00118   char *baudrate_str;
00119 
00120   if ((ret_value >= 0) || (ret_value != NO_FILE_ERROR)) {
00121     return ret_value;
00122   }
00123 
00124   home = getenv("HOME");
00125   if (home) {
00126     ret_value = searchConfigFile(home);
00127     // !!! free する必要はあるのか?
00128     if ((ret_value >= 0) || (ret_value != NO_FILE_ERROR)) {
00129       return ret_value;
00130     }
00131   }
00132 
00133   baudrate = RUN_CTRL_BAUDRATE;
00134   deviceName = NULL;
00135   baudrate_str = NULL;
00136   if ((baudrate_str = getenv("RUN_BAUDRATE"))) {
00137     baudrate = atoi(baudrate_str);
00138   }
00139 
00140   deviceName = getenv("RUN_PORT");
00141   if (deviceName) {
00142     int ret_value = initConnectDevice(deviceName, baudrate, SERIAL);
00143     if (ret_value >= 0) {
00144       fd = ret_value;
00145       ret_value = checkVersion();
00146     }
00147     return ret_value;
00148   }
00149   return -1;
00150 }
00151 
00152 
00161 int initRunCtrl(int argc, char *argv[]) {
00162   enum { COORDINATE_NUM_MAX = 256 };
00163   static runCtrl_t Run;
00164   static nodeInfo_t Node;
00165   static runParams_t RunParam;
00166   static int Unique_id = 0;
00167   static offsetInfo_t CrdInfo[COORDINATE_NUM_MAX];
00168 
00169   long baudrate = RUN_CTRL_BAUDRATE;
00170   char *deviceName = NULL;
00171   int i;
00172   int ret_value = -1;
00173 
00174   // 走行制御テーブルの初期化
00175   initNodeInfo(&Node);
00176   run_initRunParam(&RunParam);
00177   run_initCommandCtrl(&fd, &Run, &Node, &RunParam, &Unique_id);
00178   _initCoordinateCtrl(CrdInfo, COORDINATE_NUM_MAX);
00179 
00180   // 通信資源の初期化
00181   for (i = 0; i < argc; ++i) {
00182     if (!strncmp("--run_port=", argv[i], 11) && (strlen(argv[i]) > 11)) {
00183       deviceName = &argv[i][11];
00184 
00185     } else if (!strncmp("--run_baudrate=", argv[i], 15) &&
00186                (strlen(argv[i]) > 15)) {
00187       baudrate = atoi(&argv[i][15]);
00188 
00189     } else if (!strcmp("-s", argv[i]) || !strcmp("--simulator", argv[i])) {
00190       int ret_value =
00191         initConnectDevice("localhost", SIMULATOR_T_RUN_CTRL_PORT, TCP_IP);
00192       if (ret_value < 0) {
00193         Error_device = "TCP/IP";
00194         Error_baudrate = SIMULATOR_T_RUN_CTRL_PORT;
00195         Error_message = "connection fail to simulator";
00196       } else {
00197         Error_message = "success";
00198         fd = ret_value;
00199         ret_value = checkVersion();
00200       }
00201       return ret_value;
00202     }
00203   }
00204 
00205   if (deviceName &&
00206       (!strcmp("auto", deviceName) || !strcmp("AUTO", deviceName))) {
00207     int id;
00208     for (id = 20; id >= 0; --id) {
00209       char checkDevice[16] = { '\0','\0','\0','\0','\0','\0','\0','\0',
00210                                '\0','\0','\0','\0','\0','\0','\0','\0' };
00211       sprintf(checkDevice, RUN_AUTO_PORT "%d", id);
00212       ret_value = initConnectDevice(checkDevice, baudrate, SERIAL);
00213       if (ret_value >= 0) {
00214         fd = ret_value;
00215         break;
00216       }
00217     }
00218     if (ret_value < 0) {
00219       Error_device = RUN_AUTO_PORT;
00220       Error_baudrate = baudrate;
00221       Error_message = "auto dvice detection is fail";
00222     } else {
00223       Error_message = "success";
00224       ret_value = checkVersion();
00225     }
00226     return ret_value;
00227   }
00228 
00229   if (!deviceName) {
00230     return connectDefault();
00231   }
00232   ret_value = initConnectDevice(deviceName, baudrate, SERIAL);
00233   if (ret_value >= 0) {
00234     fd = ret_value;
00235     ret_value = checkVersion();
00236   }
00237   return ret_value;
00238 }
00239 
00240 
00246 char* run_getError(void) {
00247   return Error_message;
00248 }
00249 

Generated on Mon Apr 13 22:52:03 2009 by  doxygen 1.5.7.1