kmlFactories.cpp
Go to the documentation of this file.
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 


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32