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);
00015
00016 if(!_configfile.good())
00017 throw ConfigFileStateException();
00018
00019
00020 do {
00021 memset(line,0,sizeof(line));
00022 _configfile.getline(line, sizeof(line));
00023 strtok(line,"\r");
00024 } while (strcmp(line,section));
00025
00026 if (_configfile.eof())
00027 throw ConfigFileSectionNotFoundException(section);
00028
00029 do {
00030 memset(line,0,sizeof(line));
00031 _configfile.getline(line, sizeof(line));
00032 strtok(line,"\r");
00033 } while (strcmp(line,subsection));
00034 if (_configfile.eof())
00035 throw ConfigFileSubsectionNotFoundException(subsection);
00036
00037 do {
00038 memset(line,0,sizeof(line));
00039 _configfile.getline(line, sizeof(line));
00040 strtok(line,"\r");
00041 } while (strncmp(line,entry,strlen(entry)));
00042 if (_configfile.eof())
00043 throw ConfigFileEntryNotFoundException(entry);
00044
00045
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
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
00194
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
00205
00206
00207
00208
00209
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