$search
00001 00002 #include "KNI/kmlFactories.h" 00003 00004 namespace KNI { 00005 00006 kmlFactory::kmlFactory() : _configfile() {} 00007 00008 void kmlFactory::_readEntry(char* dest, int destsz, const char* section, const char* subsection, const char* entry) { 00009 00010 char line[256]; 00011 short pos = 0; 00012 short idx = 0; 00013 00014 _configfile.seekg(0); //goto the begin 00015 00016 if(!_configfile.good()) 00017 throw ConfigFileStateException(); 00018 00019 00020 do { //search section 00021 memset(line,0,sizeof(line)); 00022 _configfile.getline(line, sizeof(line)); 00023 strtok(line,"\r"); // strip off the CR 00024 } while (strcmp(line,section)); 00025 00026 if (_configfile.eof()) 00027 throw ConfigFileSectionNotFoundException(section); 00028 00029 do { //search subsection 00030 memset(line,0,sizeof(line)); 00031 _configfile.getline(line, sizeof(line)); 00032 strtok(line,"\r"); // strip off the CR 00033 } while (strcmp(line,subsection)); 00034 if (_configfile.eof()) 00035 throw ConfigFileSubsectionNotFoundException(subsection); 00036 00037 do { //search entry 00038 memset(line,0,sizeof(line)); 00039 _configfile.getline(line, sizeof(line)); 00040 strtok(line,"\r"); // strip off the CR 00041 } while (strncmp(line,entry,strlen(entry))); 00042 if (_configfile.eof()) 00043 throw ConfigFileEntryNotFoundException(entry); 00044 00045 //parse input line the detect entry value 00046 00047 while (line[pos++] != '=') { 00048 if (pos == 256) 00049 throw ConfigFileSyntaxErrorException(line); 00050 } 00051 while (line[pos++] != '"') { 00052 if (pos == 256) 00053 throw ConfigFileSyntaxErrorException(line); 00054 } 00055 00056 memset(dest,0,destsz); 00057 while (line[pos] != '"') { 00058 dest[idx++] = line[pos++]; 00059 if (pos == 256) 00060 throw ConfigFileSyntaxErrorException(line); 00061 } 00062 while (line[pos++] != ';') { 00063 if (pos == 256) 00064 throw ConfigFileSyntaxErrorException(line); 00065 } 00066 00067 } 00068 00069 00070 TKatGNL kmlFactory::getGNL() { 00071 char input[256]; 00072 TKatGNL gnl; 00073 00074 _readEntry(input,sizeof(input),"[KATANA]","[GENERAL]","addr"); 00075 gnl.adr = atoi(input); 00076 00077 _readEntry(input,sizeof(input),"[KATANA]","[GENERAL]","modelName"); 00078 sprintf(gnl.modelName, "%s", input); 00079 00080 return gnl; 00081 } 00082 00083 00084 00085 TKatMOT kmlFactory::getMOT() { 00086 char input[256]; 00087 TKatMOT mot; 00088 _readEntry(input,sizeof(input),"[KATANA]","[GENERAL]","motcnt"); 00089 mot.cnt = atoi(input); 00090 mot.arr = NULL; 00091 mot.desc = getMotDesc(mot.cnt); 00092 return mot; 00093 } 00094 00095 00096 00097 TKatSCT kmlFactory::getSCT() { 00098 char input[256]; 00099 TKatSCT katsct; 00100 _readEntry(input,sizeof(input),"[KATANA]","[GENERAL]","sctcnt"); 00101 katsct.cnt = atoi(input); 00102 katsct.arr = NULL; 00103 katsct.desc = getSctDesc(katsct.cnt); 00104 return katsct; 00105 } 00106 00107 int kmlFactory::getType(){ 00108 char input[256]; 00109 _readEntry(input,sizeof(input),"[KATANA]","[GENERAL]","type"); 00110 return atoi(input); 00111 } 00112 00113 int kmlFactory::getKinematics(){ 00114 char input [256]; 00115 _readEntry(input,sizeof(input),"[KATANA]","[GENERAL]","kinematics"); 00116 if(!strcmp("Analytical", input)) 00117 return 0; 00118 if(!strcmp("RobAnaGuess", input)) 00119 return 1; 00120 //default: RobAnaGuess 00121 return 1; 00122 } 00123 00124 00125 TKatEFF kmlFactory::getEFF() { 00126 char input[256]; 00127 TKatEFF eff; 00128 _readEntry(input,sizeof(input),"[ENDEFFECTOR]","[GENERAL]","segment1"); 00129 eff.arr_segment[0] = atof(input) ; 00130 _readEntry(input,sizeof(input),"[ENDEFFECTOR]","[GENERAL]","segment2"); 00131 eff.arr_segment[1] = atof(input) ; 00132 _readEntry(input,sizeof(input),"[ENDEFFECTOR]","[GENERAL]","segment3"); 00133 eff.arr_segment[2] = atof(input) ; 00134 _readEntry(input,sizeof(input),"[ENDEFFECTOR]","[GENERAL]","segment4"); 00135 eff.arr_segment[3] = atof(input) ; 00136 return eff; 00137 } 00138 00139 00140 00141 TMotDesc* kmlFactory::getMotDesc(short count) { 00142 char input[256]; 00143 char section[256]; 00144 00145 TMotDesc* motdesc = new TMotDesc[count]; 00146 for(int i = 0; i < count; ++i) { 00147 memset(section,0,sizeof(section)); 00148 sprintf(section,"[MOT[%d]]",i); 00149 _readEntry(input,sizeof(input),section,"[GENERAL]","slvID"); 00150 motdesc[i].slvID = atoi(input); 00151 } 00152 return motdesc; 00153 } 00154 00155 TSctDesc* kmlFactory::getSctDesc(short count) { 00156 char input[256]; 00157 char section[256]; 00158 00159 TSctDesc* sctdesc = new TSctDesc[count]; 00160 00161 for(int i = 0; i < count; ++i) { 00162 memset(section,0,sizeof(section)); 00163 sprintf(section,"[SCT[%d]]", i); 00164 _readEntry(input,sizeof(input),section,"[GENERAL]","ctrlID"); 00165 sctdesc[i].ctrlID = atoi(input); 00166 _readEntry(input,sizeof(input),section,"[GENERAL]","sens_res"); 00167 sctdesc[i].sens_res = atoi(input); 00168 _readEntry(input,sizeof(input),section,"[GENERAL]","sens_count"); 00169 sctdesc[i].sens_count = atoi(input); 00170 } 00171 return sctdesc; 00172 } 00173 00174 00175 TMotCLB kmlFactory::getMotCLB(short number) { 00176 char input[256]; 00177 char section[256]; 00178 00179 TMotCLB clb; 00180 00181 memset(section,0,sizeof(section)); 00182 sprintf(section,"[MOT[%d]]", number); 00183 00184 _readEntry(input,sizeof(input),section,"[CALIBRATION]","enable"); 00185 clb.enable = strcmp("TRUE",input) ? false : true; 00186 00187 _readEntry(input,sizeof(input),section,"[CALIBRATION]","order"); 00188 clb.order = atoi(input); 00189 00190 _readEntry(input,sizeof(input),section,"[CALIBRATION]","dir"); 00191 clb.dir = strcmp("DIR_POSITIVE",input) ? DIR_NEGATIVE : DIR_POSITIVE; 00192 00193 //_readEntry(input,sizeof(input),section,"[CALIBRATION]","diff"); 00194 //clbarr[i].diff = atoi(input); 00195 00196 _readEntry(input,sizeof(input),section,"[CALIBRATION]","mcf"); 00197 if(!strcmp("MCF_OFF", input)) 00198 clb.mcf = MCF_OFF; 00199 if(!strcmp("MCF_ON", input)) 00200 clb.mcf = MCF_ON; 00201 if(!strcmp("MCF_FREEZE",input)) 00202 clb.mcf = MCF_FREEZE; 00203 00204 //_readEntry(input,sizeof(input),section,"[CALIBRATION]","timeout"); 00205 //clbarr[i].timeout = strcmp("TM_ENDLESS", input) ? atoi(input) : TM_ENDLESS; 00206 00207 00208 //_readEntry(input,sizeof(input),section,"[CALIBRATION]","enc_tolerance"); 00209 //clbarr[i].enc_tolerance = atoi(input); 00210 00211 _readEntry(input,sizeof(input),section,"[CALIBRATION]","encoderPositionAfter"); 00212 clb.encoderPositionAfter = atoi(input); 00213 00214 return clb; 00215 } 00216 00217 TMotSCP kmlFactory::getMotSCP(short number) { 00218 TMotSCP scp; 00219 char input[256]; 00220 char section[256]; 00221 00222 00223 memset(section,0,sizeof(section)); 00224 sprintf(section,"[MOT[%d]]", number); 00225 00226 _readEntry(input,sizeof(input),section,"[STATIC]","maxppwm"); 00227 scp.maxppwm = atoi(input); 00228 00229 _readEntry(input,sizeof(input),section,"[STATIC]","maxnpwm"); 00230 scp.maxnpwm = atoi(input); 00231 00232 _readEntry(input,sizeof(input),section,"[STATIC]","kP"); 00233 scp.kP = atoi(input); 00234 00235 _readEntry(input,sizeof(input),section,"[STATIC]","kI"); 00236 scp.kI = atoi(input); 00237 00238 _readEntry(input,sizeof(input),section,"[STATIC]","kD"); 00239 scp.kD = atoi(input); 00240 00241 _readEntry(input,sizeof(input),section,"[STATIC]","kARW"); 00242 scp.kARW = atoi(input); 00243 00244 _readEntry(input,sizeof(input),section,"[STATIC]","kP_speed"); 00245 scp.kP_speed = atoi(input); 00246 00247 _readEntry(input,sizeof(input),section,"[STATIC]","kI_speed"); 00248 scp.kI_speed = atoi(input); 00249 00250 _readEntry(input,sizeof(input),section,"[STATIC]","kD_speed"); 00251 scp.kD_speed = atoi(input); 00252 00253 _readEntry(input,sizeof(input),section,"[STATIC]","maxppwm_nmp"); 00254 scp.maxppwm_nmp = atoi(input); 00255 00256 _readEntry(input,sizeof(input),section,"[STATIC]","maxnpwm_nmp"); 00257 scp.maxnpwm_nmp = atoi(input); 00258 00259 _readEntry(input,sizeof(input),section,"[STATIC]","kspeed_nmp"); 00260 scp.kspeed_nmp = atoi(input); 00261 00262 _readEntry(input,sizeof(input),section,"[STATIC]","kpos_nmp"); 00263 scp.kpos_nmp = atoi(input); 00264 00265 _readEntry(input,sizeof(input),section,"[STATIC]","kI_nmp"); 00266 scp.kI_nmp = atoi(input); 00267 00268 _readEntry(input,sizeof(input),section,"[STATIC]","crash_limit_nmp"); 00269 scp.crash_limit_nmp = atoi(input); 00270 00271 _readEntry(input,sizeof(input),section,"[STATIC]","crash_limit_lin_nmp"); 00272 scp.crash_limit_lin_nmp = atoi(input); 00273 00274 return scp; 00275 } 00276 00277 TMotDYL kmlFactory::getMotDYL(short number) { 00278 TMotDYL dyl; 00279 char input[256]; 00280 char section[256]; 00281 00282 memset(section,0,sizeof(section)); 00283 sprintf(section,"[MOT[%d]]", number); 00284 00285 _readEntry(input,sizeof(input),section,"[DYNAMIC]","maxaccel"); 00286 dyl.maxaccel = atoi(input); 00287 00288 _readEntry(input,sizeof(input),section,"[DYNAMIC]","maxdecel"); 00289 dyl.maxdecel = atoi(input); 00290 00291 _readEntry(input,sizeof(input),section,"[DYNAMIC]","minpos"); 00292 dyl.minpos = atoi(input); 00293 00294 _readEntry(input,sizeof(input),section,"[DYNAMIC]","maxpspeed"); 00295 dyl.maxpspeed = atoi(input); 00296 00297 _readEntry(input,sizeof(input),section,"[DYNAMIC]","maxnspeed"); 00298 dyl.maxnspeed = atoi(input); 00299 00300 _readEntry(input,sizeof(input),section,"[DYNAMIC]","maxcurr"); 00301 dyl.maxcurr = atoi(input); 00302 00303 dyl.actcurr = 0; 00304 00305 _readEntry(input,sizeof(input),section,"[DYNAMIC]","maxaccel_nmp"); 00306 dyl.maxaccel_nmp = atoi(input); 00307 00308 _readEntry(input,sizeof(input),section,"[DYNAMIC]","maxpspeed_nmp"); 00309 dyl.maxpspeed_nmp = atoi(input); 00310 00311 _readEntry(input,sizeof(input),section,"[DYNAMIC]","maxnspeed_nmp"); 00312 dyl.maxnspeed_nmp = atoi(input); 00313 00314 _readEntry(input,sizeof(input),section,"[DYNAMIC]","maxcurr_nmp"); 00315 dyl.maxcurr_nmp = atoi(input); 00316 00317 return dyl; 00318 } 00319 00320 00321 TMotInit kmlFactory::getMotInit(short number) { 00322 char input[256]; 00323 char section[256]; 00324 TMotInit init; 00325 memset(section,0,sizeof(section)); 00326 sprintf(section,"[MOT[%d]]", number); 00327 00328 _readEntry(input,sizeof(input),section,"[INIT]","encodersPerCycle"); 00329 init.encodersPerCycle = atoi(input); 00330 00331 _readEntry(input,sizeof(input),section,"[INIT]","encoderOffset"); 00332 init.encoderOffset = atoi(input); 00333 00334 _readEntry(input,sizeof(input),section,"[INIT]","rotationDirection"); 00335 init.rotationDirection = strcmp("DIR_POSITIVE",input) ? -1 : 1; 00336 00337 _readEntry(input,sizeof(input),section,"[INIT]","angleOffset"); 00338 init.angleOffset = atof(input); 00339 00340 _readEntry(input,sizeof(input),section,"[INIT]","angleRange"); 00341 init.angleRange = atof(input); 00342 00343 return init; 00344 } 00345 00346 void 00347 kmlFactory::getGripperParameters(bool& isPresent, int& openEncoders, int& closeEncoders) { 00348 char input[256]; 00349 00350 _readEntry(input,sizeof(input),"[KATANA]","[GRIPPER]","isPresent"); 00351 isPresent = strcmp("YES",input) ? false : true; 00352 00353 _readEntry(input,sizeof(input),"[KATANA]","[GRIPPER]","openEncoders"); 00354 openEncoders = atoi(input); 00355 00356 _readEntry(input,sizeof(input),"[KATANA]","[GRIPPER]","closeEncoders"); 00357 closeEncoders = atoi(input); 00358 } 00359 00360 } 00361