#include <stdio.h>#include <stdlib.h>#include <string.h>#include <ctype.h>#include "emc.hh"#include "posemath.h"#include "inifile.h"#include "initraj.hh"#include "emcglb.h"Include dependency graph for initraj.cc:
Go to the source code of this file.
Functions | |
| int | loadTraj () |
| int | iniTraj (const char *filename) |
Variables | |
| INIFILE * | trajInifile = 0 |
|
|
Definition at line 502 of file initraj.cc. 00503 {
00504 int retval = 0;
00505
00506 trajInifile = new INIFILE;
00507
00508 if (-1 == trajInifile->open(filename))
00509 {
00510 return -1;
00511 }
00512
00513 // load trajectory values
00514 if (0 != loadTraj())
00515 {
00516 retval = -1;
00517 }
00518
00519 // close the inifile
00520 trajInifile->close();
00521 delete trajInifile;
00522
00523 return retval;
00524 }
|
|
|
Definition at line 73 of file initraj.cc. Referenced by iniTraj().
00074 {
00075 const char *inistring;
00076 int axes;
00077 double linearUnits;
00078 double angularUnits;
00079 double cycleTime;
00080 double vel;
00081 double acc;
00082 unsigned char coordinateMark[6] = {1, 1, 1, 0, 0, 0};
00083 int t;
00084 int len;
00085 char homes[INIFILE_MAX_LINELEN];
00086 char home[INIFILE_MAX_LINELEN];
00087 EmcPose homePose ={ { 0.0, 0.0, 0.0 } , 0.0, 0.0, 0.0};
00088 double d;
00089 int index;
00090 int polarity;
00091
00092 if (NULL != (inistring = trajInifile->find("AXES", "TRAJ"))) {
00093 #ifndef UNDER_CE
00094 if (1 == sscanf(inistring, "%d", &axes)) {
00095 // found, and valid
00096 }
00097 else {
00098 // found, but invalid
00099 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00100 rcs_print("invalid inifile value for [TRAJ] AXES: %s\n", inistring);
00101 }
00102 axes = 0; // default
00103 }
00104 #else
00105 axes = atol(inistring);
00106 #endif
00107 }
00108 else {
00109 // not found at all
00110 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00111 rcs_print("can't find [TRAJ] AXES, using default\n");
00112 }
00113 axes = 0; // default
00114 }
00115 if (0 != emcTrajSetAxes(axes)) {
00116 if (EMC_DEBUG & EMC_DEBUG_CONFIG) {
00117 rcs_print("bad return value from emcTrajSetAxes\n");
00118 }
00119 return -1;
00120 }
00121
00122 if (NULL != (inistring = trajInifile->find("LINEAR_UNITS", "TRAJ"))) {
00123 #ifndef UNDER_CE
00124 if (1 == sscanf(inistring, "%lf", &linearUnits)) {
00125 // found, and valid
00126 }
00127 else {
00128 // found, but invalid
00129 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00130 rcs_print("invalid inifile value for [TRAJ] LINEAR_UNITS: %s\n", inistring);
00131 }
00132 linearUnits = 1; // default
00133 }
00134 #else
00135 linearUnits = RCS_CE_ATOF(inistring);
00136 #endif
00137 }
00138 else {
00139 // not found at all
00140 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00141 rcs_print("can't find [TRAJ] LINEAR_UNITS, using default\n");
00142 }
00143 linearUnits = 1; // default
00144 }
00145
00146 if (NULL != (inistring = trajInifile->find("ANGULAR_UNITS", "TRAJ"))) {
00147 #ifndef UNDER_CE
00148 if (1 == sscanf(inistring, "%lf", &angularUnits)) {
00149 // found, and valid
00150 }
00151 else {
00152 // found, but invalid
00153 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00154 rcs_print("invalid inifile value for [TRAJ] ANGULAR_UNITS: %s\n", inistring);
00155 }
00156 angularUnits = 1; // default
00157 }
00158 #else
00159 angularUnits = RCS_CE_ATOF(inistring);
00160 #endif
00161 }
00162 else {
00163 // not found at all
00164 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00165 rcs_print("can't find [TRAJ] ANGULAR_UNITS, using default\n");
00166 }
00167 angularUnits = 1; // default
00168 }
00169
00170 if (0 != emcTrajSetUnits(linearUnits, angularUnits)) {
00171 if (EMC_DEBUG & EMC_DEBUG_CONFIG) {
00172 rcs_print("bad return value from emcTrajSetUnits\n");
00173 }
00174 return -1;
00175 }
00176
00177 if (NULL != (inistring = trajInifile->find("CYCLE_TIME", "TRAJ"))) {
00178 #ifndef UNDER_CE
00179 if (1 == sscanf(inistring, "%lf", &cycleTime)) {
00180 // found, and valid
00181 }
00182 else {
00183 // found, but invalid
00184 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00185 rcs_print("invalid inifile value for [TRAJ] CYCLE_TIME: %s\n", inistring);
00186 }
00187 cycleTime = 1.0; // default
00188 }
00189 #else
00190 cycleTime = RCS_CE_ATOF(inistring);
00191 #endif
00192 }
00193 else {
00194 // not found at all
00195 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00196 rcs_print("can't find [TRAJ] CYCLE_TIME, using default\n");
00197 }
00198 cycleTime = 1.0; // default
00199 }
00200 if (0 != emcTrajSetCycleTime(cycleTime)) {
00201 if (EMC_DEBUG & EMC_DEBUG_CONFIG) {
00202 rcs_print("bad return value from emcTrajSetCycleTime\n");
00203 }
00204 return -1;
00205 }
00206
00207 if (NULL != (inistring = trajInifile->find("DEFAULT_VELOCITY", "TRAJ"))) {
00208 #ifndef UNDER_CE
00209 if (1 == sscanf(inistring, "%lf", &vel)) {
00210 // found, and valid
00211 }
00212 else {
00213 // found, but invalid
00214 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00215 rcs_print("invalid inifile value for [TRAJ] DEFAULT_VELOCITY: %s\n", inistring);
00216 }
00217 vel = 1.0; // default
00218 }
00219 #else
00220 vel = RCS_CE_ATOF(inistring);
00221 #endif
00222 }
00223 else {
00224 // not found at all
00225 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00226 rcs_print("can't find [TRAJ] DEFAULT_VELOCITY, using default\n");
00227 }
00228 vel = 1.0; // default
00229 }
00230
00231 // set the corresponding global
00232 TRAJ_DEFAULT_VELOCITY = vel;
00233
00234 // and set dynamic value
00235 if (0 != emcTrajSetVelocity(vel)) {
00236 if (EMC_DEBUG & EMC_DEBUG_CONFIG) {
00237 rcs_print("bad return value from emcTrajSetVelocity\n");
00238 }
00239 return -1;
00240 }
00241
00242 if (NULL != (inistring = trajInifile->find("DEFAULT_ACCELERATION", "TRAJ"))) {
00243 #ifndef UNDER_CE
00244 if (1 == sscanf(inistring, "%lf", &acc)) {
00245 // found, and valid
00246 }
00247 else {
00248 // found, but invalid
00249 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00250 rcs_print("invalid inifile value for [TRAJ] DEFAULT_ACCELERATION: %s\n", inistring);
00251 }
00252 acc = 1.0; // default
00253 }
00254 #else
00255 acc = RCS_CE_ATOF(inistring);
00256 #endif
00257 }
00258 else {
00259 // not found at all
00260 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00261 rcs_print("can't find [TRAJ] DEFAULT_ACCELERATION, using default\n");
00262 }
00263 acc = 1.0; // default
00264 }
00265 if (0 != emcTrajSetAcceleration(acc)) {
00266 if (EMC_DEBUG & EMC_DEBUG_CONFIG) {
00267 rcs_print("bad return value from emcTrajSetAcceleration\n");
00268 }
00269 return -1;
00270 }
00271
00272 if (NULL != (inistring = trajInifile->find("MAX_VELOCITY", "TRAJ"))) {
00273 #ifndef UNDER_CE
00274 if (1 == sscanf(inistring, "%lf", &vel)) {
00275 // found, and valid
00276 }
00277 else {
00278 // found, but invalid
00279 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00280 rcs_print("invalid inifile value for [TRAJ] MAX_VELOCITY: %s\n", inistring);
00281 }
00282 vel = 1.0; // default
00283 }
00284 #else
00285 vel = RCS_CE_ATOF(inistring);
00286 #endif
00287 }
00288 else {
00289 // not found at all
00290 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00291 rcs_print("can't find [TRAJ] MAX_VELOCITY, using default\n");
00292 }
00293 vel = 1.0; // default
00294 }
00295
00296 // set the corresponding global
00297 TRAJ_MAX_VELOCITY = vel;
00298
00299 // and set dynamic value
00300 if (0 != emcTrajSetMaxVelocity(vel)) {
00301 if (EMC_DEBUG & EMC_DEBUG_CONFIG) {
00302 rcs_print("bad return value from emcTrajSetMaxVelocity\n");
00303 }
00304 return -1;
00305 }
00306
00307 // also set the global-- it's likely to be done in emcTrajSetMaxVelocity(),
00308 // but it can't hurt to make sure
00309 TRAJ_MAX_VELOCITY = vel;
00310
00311 if (NULL != (inistring = trajInifile->find("MAX_ACCELERATION", "TRAJ"))) {
00312 #ifndef UNDER_CE
00313 if (1 == sscanf(inistring, "%lf", &acc)) {
00314 // found, and valid
00315 }
00316 else {
00317 // found, but invalid
00318 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00319 rcs_print("invalid inifile value for [TRAJ] MAX_ACCELERATION: %s\n", inistring);
00320 }
00321 acc = 1.0; // default
00322 }
00323 #else
00324 acc = RCS_CE_ATOF(inistring);
00325 #endif
00326 }
00327 else {
00328 // not found at all
00329 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00330 rcs_print("can't find [TRAJ] MAX_ACCELERATION, using default\n");
00331 }
00332 acc = 1.0; // default
00333 }
00334 if (0 != emcTrajSetMaxAcceleration(acc)) {
00335 if (EMC_DEBUG & EMC_DEBUG_CONFIG) {
00336 rcs_print("bad return value from emcTrajSetMaxAcceleration\n");
00337 }
00338 return -1;
00339 }
00340
00341 // set coordinateMark[] to hold 1's for each coordinate present,
00342 // so that home position can be interpreted properly
00343 if (NULL != (inistring = trajInifile->find("COORDINATES", "TRAJ"))) {
00344 len = strlen(inistring);
00345 // there's an entry in ini file, so clear all the marks out first
00346 // so that defaults don't apply at all
00347 for (t = 0; t < 6; t++) {
00348 coordinateMark[t] = 0;
00349 }
00350 // now set actual marks
00351 for (t = 0; t < len; t++) {
00352 if (inistring[t] == 'X')
00353 coordinateMark[0] = 1;
00354 else if (inistring[t] == 'Y')
00355 coordinateMark[1] = 1;
00356 else if (inistring[t] == 'Z')
00357 coordinateMark[2] = 1;
00358 else if (inistring[t] == 'R')
00359 coordinateMark[3] = 1;
00360 else if (inistring[t] == 'P')
00361 coordinateMark[4] = 1;
00362 else if (inistring[t] == 'W')
00363 coordinateMark[5] = 1;
00364 }
00365 }
00366 else {
00367 // not there, use default
00368 // by leaving coordinateMark[] alone, default is X Y Z
00369 }
00370
00371 if (NULL != (inistring = trajInifile->find("HOME", "TRAJ"))) {
00372 // found it, now interpret it according to coordinateMark[]
00373 strcpy(homes, inistring);
00374 len = 0;
00375 for (t = 0; t < 6; t++) {
00376 if (! coordinateMark[t]) {
00377 continue; // position t at index of next non-zero mark
00378 }
00379 #ifndef UNDER_CE
00380 // there is a mark, so read the string for a value
00381 if (1 == sscanf(&homes[len], "%s", home) &&
00382 1 == sscanf(home, "%lf", &d)) {
00383 #else
00384 strcpy(home,&homes[len]);
00385 d = RCS_CE_ATOF(home);
00386 #endif
00387 // got an entry, index into coordinateMark[] is 't'
00388 if (t == 0)
00389 homePose.tran.x = d;
00390 else if (t == 1)
00391 homePose.tran.y = d;
00392 else if (t == 2)
00393 homePose.tran.z = d;
00394 else if (t == 3)
00395 homePose.a = d;
00396 else if (t == 4)
00397 homePose.b = d;
00398 else
00399 homePose.c = d;
00400
00401 // position string ptr past this value
00402 len += strlen(home);
00403 // and at start of next value
00404 while ((homes[len] == ' ' || home[len] == '\t') &&
00405 len < INIFILE_MAX_LINELEN) {
00406 len++;
00407 }
00408 if (len >= INIFILE_MAX_LINELEN) {
00409 break; // out of for loop
00410 }
00411 }
00412 else {
00413 // badly formatted entry
00414 rcs_print("invalid inifile value for [TRAJ] HOME: %s\n", inistring);
00415 break;
00416 }
00417 } // end of for-loop on coordinateMark[]
00418 }
00419 else {
00420 // not found at all
00421 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00422 rcs_print("can't find [TRAJ] HOME, using default\n");
00423 }
00424 }
00425
00426
00427 if (0 != emcTrajSetHome(homePose)) {
00428 if (EMC_DEBUG & EMC_DEBUG_CONFIG) {
00429 rcs_print("bad return value from emcTrajSetHome\n");
00430 }
00431 return -1;
00432 }
00433
00434 if (NULL != (inistring = trajInifile->find("PROBE_POLARITY", "TRAJ"))) {
00435 #ifndef UNDER_CE
00436 if (1 == sscanf(inistring, "%d", &polarity)) {
00437 // found, and valid
00438 }
00439 else {
00440 // found, but invalid
00441 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00442 rcs_print("invalid inifile value for [TRAJ] PROBE_POLARITY: %s\n", inistring);
00443 }
00444 polarity = 1; // default for polarities
00445 }
00446 #else
00447 polarity = atol(inistring);
00448 #endif
00449 }
00450 else {
00451 // not found at all
00452 polarity = 1; // default for polarities
00453 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00454 rcs_print("can't find [TRAJ] PROBE_POLARITY, using default\n");
00455 }
00456 }
00457 if (0 != emcTrajSetProbePolarity(polarity)) {
00458 if (EMC_DEBUG & EMC_DEBUG_CONFIG) {
00459 rcs_print("bad return from emcTrajSetPolarity\n");
00460 }
00461 return -1;
00462 }
00463
00464 if (NULL != (inistring = trajInifile->find("PROBE_INDEX", "TRAJ"))) {
00465 #ifndef UNDER_CE
00466 if (1 == sscanf(inistring, "%d", &index)) {
00467 // found, and valid
00468 }
00469 else {
00470 // found, but invalid
00471 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00472 rcs_print("invalid inifile value for [TRAJ] PROBE_INDEX: %s\n", inistring);
00473 }
00474 index = 0; // default for indexes
00475 }
00476 #else
00477 index = atol(inistring);
00478 #endif
00479 }
00480 else {
00481 // not found at all
00482 index = 1; // default for polarities
00483 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00484 rcs_print("can't find [TRAJ] PROBE_INDEX, using default\n");
00485 }
00486 }
00487 if (0 != emcTrajSetProbeIndex(index)) {
00488 if (EMC_DEBUG & EMC_DEBUG_CONFIG) {
00489 rcs_print("bad return from emcTrajSetIndex\n");
00490 }
00491 return -1;
00492 }
00493
00494 return 0;
00495 }
|
|
|
Definition at line 42 of file initraj.cc. |
1.2.11.1 written by Dimitri van Heesch,
© 1997-2001