00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 extern "C" {
00023 #ifndef UNDER_CE
00024 #include <stdio.h>
00025 #endif
00026 #include <stdlib.h>
00027 #include <string.h>
00028 #include <ctype.h>
00029 }
00030
00031 #include "emc.hh"
00032 #include "posemath.h"
00033 #include "inifile.h"
00034 #include "initraj.hh"
00035 #include "emcglb.h"
00036
00037 #ifdef UNDER_CE
00038 #include "rcs_ce.h"
00039 #endif
00040
00041
00042 static INIFILE *trajInifile = 0;
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073 static int loadTraj()
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
00096 }
00097 else {
00098
00099 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00100 rcs_print("invalid inifile value for [TRAJ] AXES: %s\n", inistring);
00101 }
00102 axes = 0;
00103 }
00104 #else
00105 axes = atol(inistring);
00106 #endif
00107 }
00108 else {
00109
00110 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00111 rcs_print("can't find [TRAJ] AXES, using default\n");
00112 }
00113 axes = 0;
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
00126 }
00127 else {
00128
00129 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00130 rcs_print("invalid inifile value for [TRAJ] LINEAR_UNITS: %s\n", inistring);
00131 }
00132 linearUnits = 1;
00133 }
00134 #else
00135 linearUnits = RCS_CE_ATOF(inistring);
00136 #endif
00137 }
00138 else {
00139
00140 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00141 rcs_print("can't find [TRAJ] LINEAR_UNITS, using default\n");
00142 }
00143 linearUnits = 1;
00144 }
00145
00146 if (NULL != (inistring = trajInifile->find("ANGULAR_UNITS", "TRAJ"))) {
00147 #ifndef UNDER_CE
00148 if (1 == sscanf(inistring, "%lf", &angularUnits)) {
00149
00150 }
00151 else {
00152
00153 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00154 rcs_print("invalid inifile value for [TRAJ] ANGULAR_UNITS: %s\n", inistring);
00155 }
00156 angularUnits = 1;
00157 }
00158 #else
00159 angularUnits = RCS_CE_ATOF(inistring);
00160 #endif
00161 }
00162 else {
00163
00164 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00165 rcs_print("can't find [TRAJ] ANGULAR_UNITS, using default\n");
00166 }
00167 angularUnits = 1;
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
00181 }
00182 else {
00183
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;
00188 }
00189 #else
00190 cycleTime = RCS_CE_ATOF(inistring);
00191 #endif
00192 }
00193 else {
00194
00195 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00196 rcs_print("can't find [TRAJ] CYCLE_TIME, using default\n");
00197 }
00198 cycleTime = 1.0;
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
00211 }
00212 else {
00213
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;
00218 }
00219 #else
00220 vel = RCS_CE_ATOF(inistring);
00221 #endif
00222 }
00223 else {
00224
00225 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00226 rcs_print("can't find [TRAJ] DEFAULT_VELOCITY, using default\n");
00227 }
00228 vel = 1.0;
00229 }
00230
00231
00232 TRAJ_DEFAULT_VELOCITY = vel;
00233
00234
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
00246 }
00247 else {
00248
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;
00253 }
00254 #else
00255 acc = RCS_CE_ATOF(inistring);
00256 #endif
00257 }
00258 else {
00259
00260 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00261 rcs_print("can't find [TRAJ] DEFAULT_ACCELERATION, using default\n");
00262 }
00263 acc = 1.0;
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
00276 }
00277 else {
00278
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;
00283 }
00284 #else
00285 vel = RCS_CE_ATOF(inistring);
00286 #endif
00287 }
00288 else {
00289
00290 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00291 rcs_print("can't find [TRAJ] MAX_VELOCITY, using default\n");
00292 }
00293 vel = 1.0;
00294 }
00295
00296
00297 TRAJ_MAX_VELOCITY = vel;
00298
00299
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
00308
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
00315 }
00316 else {
00317
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;
00322 }
00323 #else
00324 acc = RCS_CE_ATOF(inistring);
00325 #endif
00326 }
00327 else {
00328
00329 if (EMC_DEBUG & EMC_DEBUG_DEFAULTS) {
00330 rcs_print("can't find [TRAJ] MAX_ACCELERATION, using default\n");
00331 }
00332 acc = 1.0;
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
00342
00343 if (NULL != (inistring = trajInifile->find("COORDINATES", "TRAJ"))) {
00344 len = strlen(inistring);
00345
00346
00347 for (t = 0; t < 6; t++) {
00348 coordinateMark[t] = 0;
00349 }
00350
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
00368
00369 }
00370
00371 if (NULL != (inistring = trajInifile->find("HOME", "TRAJ"))) {
00372
00373 strcpy(homes, inistring);
00374 len = 0;
00375 for (t = 0; t < 6; t++) {
00376 if (! coordinateMark[t]) {
00377 continue;
00378 }
00379 #ifndef UNDER_CE
00380
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
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
00402 len += strlen(home);
00403
00404 while ((homes[len] == ' ' || home[len] == '\t') &&
00405 len < INIFILE_MAX_LINELEN) {
00406 len++;
00407 }
00408 if (len >= INIFILE_MAX_LINELEN) {
00409 break;
00410 }
00411 }
00412 else {
00413
00414 rcs_print("invalid inifile value for [TRAJ] HOME: %s\n", inistring);
00415 break;
00416 }
00417 }
00418 }
00419 else {
00420
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
00438 }
00439 else {
00440
00441 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00442 rcs_print("invalid inifile value for [TRAJ] PROBE_POLARITY: %s\n", inistring);
00443 }
00444 polarity = 1;
00445 }
00446 #else
00447 polarity = atol(inistring);
00448 #endif
00449 }
00450 else {
00451
00452 polarity = 1;
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
00468 }
00469 else {
00470
00471 if (EMC_DEBUG & EMC_DEBUG_INVALID) {
00472 rcs_print("invalid inifile value for [TRAJ] PROBE_INDEX: %s\n", inistring);
00473 }
00474 index = 0;
00475 }
00476 #else
00477 index = atol(inistring);
00478 #endif
00479 }
00480 else {
00481
00482 index = 1;
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 }
00496
00497
00498
00499
00500
00501
00502 int iniTraj(const char *filename)
00503 {
00504 int retval = 0;
00505
00506 trajInifile = new INIFILE;
00507
00508 if (-1 == trajInifile->open(filename))
00509 {
00510 return -1;
00511 }
00512
00513
00514 if (0 != loadTraj())
00515 {
00516 retval = -1;
00517 }
00518
00519
00520 trajInifile->close();
00521 delete trajInifile;
00522
00523 return retval;
00524 }