usrmotintf.cc
1 /******************************************************************** 2 * Description: usrmotintf.cc 3 * Defs for interface functions (init, exit, read, write) for user 4 * processes which communicate with the real-time motion controller 5 * in emcmot.c 6 * 7 * Derived from a work by Fred Proctor & Will Shackleford 8 * 9 * Author: 10 * License: GPL Version 2 11 * System: Linux 12 * 13 * Copyright (c) 2004 All rights reserved. 14 ********************************************************************/ 15 16 #include "config.h" /* LINELEN definition */ 17 #include <stdlib.h> /* exit() */ 18 #include <sys/stat.h> 19 #include <string.h> /* memcpy() */ 20 #include <float.h> /* DBL_MIN */ 21 #include "motion.h" /* emcmot_status_t,CMD */ 22 #include "motion_debug.h" /* emcmot_debug_t */ 23 #include "motion_struct.h" /* emcmot_struct_t */ 24 #include "emcmotcfg.h" /* EMCMOT_ERROR_NUM,LEN */ 25 #include "emcmotglb.h" /* SHMEM_KEY */ 26 #include "usrmotintf.h" /* these decls */ 27 #include "_timer.h" 28 #include "rcs_print.hh" 29 30 #include "inifile.hh" 31 32 #define READ_TIMEOUT_SEC 0 /* seconds for timeout */ 33 #define READ_TIMEOUT_USEC 100000 /* microseconds for timeout */ 34 35 #include "rtapi.h" 36 37 #include "dbuf.h" 38 #include "stashf.h" 39 40 static int inited = 0; /* flag if inited */ 41 42 static emcmot_command_t *emcmotCommand = 0; 43 static emcmot_status_t *emcmotStatus = 0; 44 static emcmot_config_t *emcmotConfig = 0; 45 static emcmot_debug_t *emcmotDebug = 0; 46 static emcmot_error_t *emcmotError = 0; 47 static emcmot_struct_t *emcmotStruct = 0; 48 49 /* usrmotIniLoad() loads params (SHMEM_KEY, COMM_TIMEOUT) 50 from named ini file */ 51 int usrmotIniLoad(const char *filename) 52 { 53 IniFile inifile(IniFile::ERR_CONVERSION); // Enable exception. 54 55 /* open it */ 56 if (!inifile.Open(filename)) { 57 rtapi_print("can't find emcmot ini file %s\n", filename); 58 return -1; 59 } 60 61 try { 62 inifile.Find((int *)&SHMEM_KEY, "SHMEM_KEY", "EMCMOT"); 63 inifile.Find(&EMCMOT_COMM_TIMEOUT, "COMM_TIMEOUT", "EMCMOT"); 64 } 65 66 catch(IniFile::Exception &e){ 67 e.Print(); 68 return -1; 69 } 70 71 return 0; 72 } 73 74 /* writes command from c */ 75 int usrmotWriteEmcmotCommand(emcmot_command_t * c) 76 { 77 emcmot_status_t s; 78 static int commandNum = 0; 79 static unsigned char headCount = 0; 80 double end; 81 82 if (!MOTION_ID_VALID(c->id)) { 83 rcs_print("USRMOT: ERROR: invalid motion id: %d\n",c->id); 84 return EMCMOT_COMM_INVALID_MOTION_ID; 85 } 86 c->head = ++headCount; 87 c->tail = c->head; 88 c->commandNum = ++commandNum; 89 90 /* check for mapped mem still around */ 91 if (0 == emcmotCommand) { 92 rcs_print("USRMOT: ERROR: can't connect to shared memory\n"); 93 return EMCMOT_COMM_ERROR_CONNECT; 94 } 95 /* copy entire command structure to shared memory */ 96 *emcmotCommand = *c; 97 /* poll for receipt of command */ 98 /* set timeout for comm failure, now + timeout */ 99 end = etime() + EMCMOT_COMM_TIMEOUT; 100 /* now check to see if it got it */ 101 while (etime() < end) { 102 /* update status */ 103 if (( usrmotReadEmcmotStatus(&s) == 0 ) && ( s.commandNumEcho == commandNum )) { 104 /* now check emcmot status flag */ 105 if (s.commandStatus == EMCMOT_COMMAND_OK) { 106 return EMCMOT_COMM_OK; 107 } else { 108 rcs_print("USRMOT: ERROR: invalid command\n"); 109 return EMCMOT_COMM_ERROR_COMMAND; 110 } 111 } 112 esleep(25e-6); 113 } 114 rcs_print("USRMOT: ERROR: command timeout\n"); 115 return EMCMOT_COMM_ERROR_TIMEOUT; 116 } 117 118 /* copies status to s */ 119 int usrmotReadEmcmotStatus(emcmot_status_t * s) 120 { 121 int split_read_count; 122 123 /* check for shmem still around */ 124 if (0 == emcmotStatus) { 125 return EMCMOT_COMM_ERROR_CONNECT; 126 } 127 split_read_count = 0; 128 do { 129 /* copy status struct from shmem to local memory */ 130 memcpy(s, emcmotStatus, sizeof(emcmot_status_t)); 131 /* got it, now check head-tail matche */ 132 if (s->head == s->tail) { 133 /* head and tail match, done */ 134 return EMCMOT_COMM_OK; 135 } 136 /* inc counter and try again, max three times */ 137 } while ( ++split_read_count < 3 ); 138 return EMCMOT_COMM_SPLIT_READ_TIMEOUT; 139 } 140 141 /* copies config to s */ 142 int usrmotReadEmcmotConfig(emcmot_config_t * s) 143 { 144 int split_read_count; 145 146 /* check for shmem still around */ 147 if (0 == emcmotConfig) { 148 return EMCMOT_COMM_ERROR_CONNECT; 149 } 150 split_read_count = 0; 151 do { 152 /* copy config struct from shmem to local memory */ 153 memcpy(s, emcmotConfig, sizeof(emcmot_config_t)); 154 /* got it, now check head-tail matches */ 155 if (s->head == s->tail) { 156 /* head and tail match, done */ 157 return EMCMOT_COMM_OK; 158 } 159 /* inc counter and try again, max three times */ 160 } while ( ++split_read_count < 3 ); 161 printf("ReadEmcmotConfig COMM_SPLIT_READ_TIMEOUT\n" ); 162 return EMCMOT_COMM_SPLIT_READ_TIMEOUT; 163 } 164 165 /* copies debug to s */ 166 int usrmotReadEmcmotDebug(emcmot_debug_t * s) 167 { 168 int split_read_count; 169 170 /* check for shmem still around */ 171 if (0 == emcmotDebug) { 172 return EMCMOT_COMM_ERROR_CONNECT; 173 } 174 split_read_count = 0; 175 do { 176 /* copy debug struct from shmem to local memory */ 177 memcpy(s, emcmotDebug, sizeof(emcmot_debug_t)); 178 /* got it, now check head-tail matches */ 179 if (s->head == s->tail) { 180 /* head and tail match, done */ 181 return EMCMOT_COMM_OK; 182 } 183 /* inc counter and try again, max three times */ 184 } while ( ++split_read_count < 3 ); 185 printf("ReadEmcmotDebug COMM_SPLIT_READ_TIMEOUT\n" ); 186 return EMCMOT_COMM_SPLIT_READ_TIMEOUT; 187 } 188 189 /* copies error to s */ 190 int usrmotReadEmcmotError(char *e) 191 { 192 /* check to see if ptr still around */ 193 if (emcmotError == 0) { 194 return -1; 195 } 196 197 char data[EMCMOT_ERROR_LEN]; 198 struct dbuf d; 199 dbuf_init(&d, (unsigned char *)data, EMCMOT_ERROR_LEN); 200 201 /* returns 0 if something, -1 if not */ 202 int result = emcmotErrorGet(emcmotError, data); 203 if(result < 0) return result; 204 205 struct dbuf_iter di; 206 dbuf_iter_init(&di, &d); 207 208 result = snprintdbuf(e, EMCMOT_ERROR_LEN, &di); 209 if(result < 0) return result; 210 return 0; 211 } 212 213 /* 214 htostr() 215 216 converts short int to 0-1 style string, in s. Assumes a short is 2 bytes. 217 */ 218 /*! \todo Another #if 0 */ 219 #if 0 /*! \todo FIXME - don't know if this is still needed 220 */ 221 222 static int htostr(char *s, unsigned short h) 223 { 224 int t; 225 226 for (t = 15; t >= 0; t--) { 227 s[t] = h % 2 ? '1' : '0'; 228 h >>= 1; 229 } 230 s[16] = 0; /* null terminate */ 231 232 return 0; 233 } 234 #endif 235 236 void printEmcPose(EmcPose * pose) 237 { 238 printf("x=%f\ty=%f\tz=%f\tu=%f\tv=%f\tw=%f\ta=%f\tb=%f\tc=%f", 239 pose->tran.x, pose->tran.y, pose->tran.z, 240 pose->u, pose->v, pose->w, 241 pose->a, pose->b, pose->c); 242 } 243 244 void printTPstruct(TP_STRUCT * tp) 245 { 246 printf("queueSize=%d\n", tp->queueSize); 247 printf("cycleTime=%f\n", tp->cycleTime); 248 printf("vMax=%f\n", tp->vMax); 249 printf("aMax=%f\n", tp->aMax); 250 printf("vLimit=%f\n", tp->vLimit); 251 printf("wMax=%f\n", tp->wMax); 252 printf("wDotMax=%f\n", tp->wDotMax); 253 printf("nextId=%d\n", tp->nextId); 254 printf("execId=%d\n", tp->execId); 255 printf("termCond=%d\n", tp->termCond); 256 printf("currentPos :"); 257 printEmcPose(&tp->currentPos); 258 printf("\n"); 259 printf("goalPos :"); 260 printEmcPose(&tp->goalPos); 261 printf("\n"); 262 printf("done=%d\n", tp->done); 263 printf("depth=%d\n", tp->depth); 264 printf("activeDepth=%d\n", tp->activeDepth); 265 printf("aborting=%d\n", tp->aborting); 266 printf("pausing=%d\n", tp->pausing); 267 } 268 269 void usrmotPrintEmcmotDebug(emcmot_debug_t *d, int which) 270 { 271 // int t; 272 273 printf("running time: \t%f\n", d->running_time); 274 switch (which) { 275 /*! \todo Another #if 0 */ 276 #if 0 277 printf("\nferror: "); 278 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 279 printf("\t%f", d->ferrorCurrent[t]); 280 } 281 printf("\n"); 282 283 printf("\nferror High: "); 284 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 285 printf("\t%f", d->ferrorHighMark[t]); 286 } 287 printf("\n"); 288 break; 289 case 5: 290 printf("traj m/m/a:\t%f\t%f\t%f\n", d->tMin, d->tMax, d->tAvg); 291 printf("\n"); 292 printf("servo m/m/a:\t%f\t%f\t%f\n", d->sMin, d->sMax, d->sAvg); 293 printf("\n"); 294 printf("(off) m/m/a:\t%f\t%f\t%f\n", d->nMin, d->nMax, d->nAvg); 295 printf("\n"); 296 printf("(cycle to cycle time) m/m/a:\t%f\t%f\t%f\n", d->yMin, d->yMax, 297 d->yAvg); 298 printf("\n"); 299 printf("(frequency compute time) m/m/a:\t%f\t%f\t%f\n", d->fMin, 300 d->fMax, d->fAvg); 301 printf("\n"); 302 printf("(frequecy cycle to cycle time) m/m/a:\t%f\t%f\t%f\n", 303 d->fyMin, d->fyMax, d->fyAvg); 304 printf("\n"); 305 break; 306 #endif 307 308 case 6: 309 case 7: 310 case 8: 311 case 9: 312 case 10: 313 case 11: 314 // printf("jointPos[%d]: %f\n", which - 6, d->jointPos[(which - 6)]); 315 /*! \todo Another #if 0 */ 316 #if 0 /*! \todo FIXME - change to work with joint 317 structures */ 318 printf("coarseJointPos[%d]: %f\n", 319 which - 6, d->coarseJointPos[(which - 6)]); 320 printf("jointVel[%d]: %f\n", which - 6, d->jointVel[(which - 6)]); 321 printf("rawInput[%d]: %f\n", which - 6, d->rawInput[(which - 6)]); 322 printf("rawOutput[%d]: %f\n", which - 6, d->rawOutput[(which - 6)]); 323 #endif 324 // printf("bcompincr[%d]: %f\n", which - 6, d->bcompincr[(which - 6)]); 325 break; 326 327 case 12: 328 /*! \todo Another #if 0 */ 329 #if 0 /*! \todo FIXME - change to work with joint 330 structures */ 331 printf("\noldInput: "); 332 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 333 printf("\t%f", d->oldInput[t]); 334 } 335 printf("\nrawInput: "); 336 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 337 printf("\t%f", d->rawInput[t]); 338 } 339 printf("\ninverseInputScale: "); 340 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 341 printf("\t%f", d->inverseInputScale[t]); 342 } 343 #endif 344 printf("\n"); 345 346 default: 347 break; 348 } 349 350 } 351 352 void usrmotPrintEmcmotConfig(emcmot_config_t c, int which) 353 { 354 // int t; 355 // char m[32]; 356 357 switch (which) { 358 case 0: 359 printf("debug level \t%d\n", c.debug); 360 printf("traj time: \t%f\n", c.trajCycleTime); 361 printf("servo time: \t%f\n", c.servoCycleTime); 362 printf("interp rate: \t%d\n", c.interpolationRate); 363 printf("v limit: \t%f\n", c.limitVel); 364 printf("axis vlimit: \t"); 365 /*! \todo Another #if 0 */ 366 #if 0 /*! \todo FIXME - waiting for new structs */ 367 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 368 printf("%f ", c.axisLimitVel[t]); 369 } 370 printf("\n"); 371 printf("axis acc: \t"); 372 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 373 printf("%f ", c.axisLimitAcc[t]); 374 } 375 printf("\n"); 376 #endif 377 printf("\n"); 378 break; 379 380 case 1: 381 printf("pid stuff is obsolete\n"); 382 /*! \todo Another #if 0 */ 383 #if 0 384 printf 385 ("pid:\tP\tI\tD\tFF0\tFF1\tFF2\tBCKLSH\tBIAS\tMAXI\tDEADBAND\tCYCLE TIME\n"); 386 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 387 printf 388 ("\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%f\t%f\n", 389 c.pid[t].p, c.pid[t].i, c.pid[t].d, c.pid[t].ff0, 390 c.pid[t].ff1, c.pid[t].ff2, c.pid[t].backlash, c.pid[t].bias, 391 c.pid[t].maxError, c.pid[t].deadband, c.pid[t].cycleTime); 392 } 393 printf("\n"); 394 #endif 395 break; 396 397 case 3: 398 /*! \todo Another #if 0 */ 399 #if 0 /*! \todo FIXME - waiting for new structs */ 400 printf("pos limits: "); 401 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 402 printf("\t%f", c.maxLimit[t]); 403 } 404 405 printf("\nneg limits: "); 406 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 407 printf("\t%f", c.minLimit[t]); 408 } 409 410 printf("\nmax ferror: "); 411 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 412 printf("\t%f", c.maxFerror[t]); 413 } 414 printf("\n"); 415 416 printf("\nmin ferror: "); 417 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 418 printf("\t%f", c.minFerror[t]); 419 } 420 printf("\n"); 421 422 printf("\nhome offsets: "); 423 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 424 printf("\t%f", c.homeOffset[t]); 425 } 426 printf("\n"); 427 #endif 428 break; 429 430 default: 431 break; 432 } 433 434 } 435 436 /* status printing function */ 437 void usrmotPrintEmcmotStatus(emcmot_status_t *s, int which) 438 { 439 // int t; 440 // char m[32]; 441 442 switch (which) { 443 case 0: 444 printf("mode: \t%s\n", 445 s->motionFlag & EMCMOT_MOTION_TELEOP_BIT ? "teleop" : 446 (s->motionFlag & EMCMOT_MOTION_COORD_BIT ? "coord" : "free") 447 ); 448 printf("cmd: \t%d\n", s->commandEcho); 449 printf("cmd num: \t%d\n", s->commandNumEcho); 450 printf("heartbeat: \t%u\n", s->heartbeat); 451 /*! \todo Another #if 0 */ 452 #if 0 /*! \todo FIXME - change to work with joint 453 structures */ 454 printf("axes enabled: \t"); 455 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 456 printf("%d", s->axisFlag[t] & EMCMOT_JOINT_ENABLE_BIT ? 1 : 0); 457 } 458 printf("\n"); 459 #endif 460 printf("cmd pos: \t%f\t%f\t%f\t%f\t%f\t%f\n", 461 s->carte_pos_cmd.tran.x, s->carte_pos_cmd.tran.y, 462 s->carte_pos_cmd.tran.z, s->carte_pos_cmd.a, s->carte_pos_cmd.b, 463 s->carte_pos_cmd.c); 464 printf("act pos: \t%f\t%f\t%f\t%f\t%f\t%f\n", 465 s->carte_pos_fb.tran.x, s->carte_pos_fb.tran.y, 466 s->carte_pos_fb.tran.z, s->carte_pos_fb.a, s->carte_pos_fb.b, 467 s->carte_pos_fb.c); 468 printf("joint data:\n"); 469 /*! \todo Another #if 0 */ 470 #if 0 /*! \todo FIXME - change to work with joint 471 structures */ 472 printf(" cmd: "); 473 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 474 printf("\t%f", s->joint_pos_cmd[t]); 475 } 476 printf("\n"); 477 printf(" fb: "); 478 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 479 printf("\t%f", s->joint_pos_fb[t]); 480 } 481 printf("\n"); 482 printf(" vel: "); 483 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 484 printf("\t%f", s->joint_vel_cmd[t]); 485 } 486 printf("\n"); 487 printf(" ferr:"); 488 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 489 printf("\t%f", s->ferrorCurrent[t]); 490 } 491 printf("\n"); 492 printf(" lim:"); 493 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 494 printf("\t%f", s->ferrorLimit[t]); 495 } 496 printf("\n"); 497 printf(" max:"); 498 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 499 printf("\t%f", s->ferrorHighMark[t]); 500 } 501 printf("\n"); 502 #endif 503 printf("velocity: \t%f\n", s->vel); 504 printf("accel: \t%f\n", s->acc); 505 printf("id: \t%d\n", s->id); 506 printf("depth: \t%d\n", s->depth); 507 printf("active depth: \t%d\n", s->activeDepth); 508 printf("inpos: \t%d\n", 509 s->motionFlag & EMCMOT_MOTION_INPOS_BIT ? 1 : 0); 510 /*! \todo Another #if 0 */ 511 #if 0 /*! \todo FIXME - change to work with joint 512 structures */ 513 printf("homing: \t"); 514 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 515 printf("%d", s->axisFlag[0] & EMCMOT_JOINT_HOMING_BIT ? 1 : 0); 516 } 517 printf("\n"); 518 #endif 519 printf("enabled: \t%s\n", 520 s->motionFlag & EMCMOT_MOTION_ENABLE_BIT ? "ENABLED" : "DISABLED"); 521 printf("probe value: %d\n", s->probeVal); 522 printf("probe Tripped: %d\n", s->probeTripped); 523 printf("probing: %d\n", s->probing); 524 printf("probed pos: \t%f\t%f\t%f\n", 525 s->probedPos.tran.x, s->probedPos.tran.y, s->probedPos.tran.z); 526 break; 527 528 case 2: 529 /* print motion and axis flags */ 530 /*! \todo Another #if 0 */ 531 #if 0 /*! \todo FIXME - change to work with joint 532 structures */ 533 htostr(m, s->motionFlag); 534 printf("motion: %s\n", m); 535 printf("axes: "); 536 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 537 htostr(m, s->axisFlag[t]); 538 printf("%s ", m); 539 } 540 printf("\n"); 541 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 542 printf("%d\t", ((s->axisFlag[t] & EMCMOT_JOINT_ENABLE_BIT) != 0)); 543 } 544 printf("enable\n"); 545 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 546 printf("%d\t", ((s->axisFlag[t] & EMCMOT_JOINT_ACTIVE_BIT) != 0)); 547 } 548 printf("active\n"); 549 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 550 printf("%d\t", ((s->axisFlag[t] & EMCMOT_JOINT_INPOS_BIT) != 0)); 551 } 552 printf("inpos\n"); 553 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 554 printf("%d\t", ((s->axisFlag[t] & EMCMOT_JOINT_ERROR_BIT) != 0)); 555 } 556 printf("error\n"); 557 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 558 printf("%d\t", 559 ((s->axisFlag[t] & EMCMOT_JOINT_MAX_SOFT_LIMIT_BIT) != 0)); 560 } 561 printf("max_soft_limit\n"); 562 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 563 printf("%d\t", 564 ((s->axisFlag[t] & EMCMOT_JOINT_MIN_SOFT_LIMIT_BIT) != 0)); 565 } 566 printf("min_soft_limit\n"); 567 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 568 printf("%d\t", 569 ((s->axisFlag[t] & EMCMOT_JOINT_MAX_HARD_LIMIT_BIT) != 0)); 570 } 571 printf("max_hard_limit\n"); 572 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 573 printf("%d\t", 574 ((s->axisFlag[t] & EMCMOT_JOINT_MIN_HARD_LIMIT_BIT) != 0)); 575 } 576 printf("min_hard_limit\n"); 577 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 578 printf("%d\t", 579 ((s->axisFlag[t] & EMCMOT_JOINT_HOME_SWITCH_BIT) != 0)); 580 } 581 printf("home_switch\n"); 582 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 583 printf("%d\t", ((s->axisFlag[t] & EMCMOT_JOINT_HOMING_BIT) != 0)); 584 } 585 printf("homing\n"); 586 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 587 printf("%d\t", ((s->axisFlag[t] & EMCMOT_JOINT_HOMED_BIT) != 0)); 588 } 589 printf("homed\n"); 590 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 591 printf("%d\t", ((s->axisFlag[t] & EMCMOT_JOINT_FERROR_BIT) != 0)); 592 } 593 printf("ferror\n"); 594 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 595 printf("%d\t", ((s->axisFlag[t] & EMCMOT_JOINT_FAULT_BIT) != 0)); 596 } 597 #endif 598 printf("fault\n"); 599 printf("\npolarity: "); 600 printf("limit override mask: %08x\n", s->overrideLimitMask); 601 break; 602 603 case 4: 604 printf("scales handled in HAL now!\n"); 605 /*! \todo Another #if 0 */ 606 #if 0 607 printf("output scales: "); 608 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 609 printf("\t%f", s->outputScale[t]); 610 } 611 612 printf("\noutput offsets:"); 613 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 614 printf("\t%f", s->outputOffset[t]); 615 } 616 617 printf("\ninput scales: "); 618 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 619 printf("\t%f", s->inputScale[t]); 620 } 621 622 printf("\ninput offsets: "); 623 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 624 printf("\t%f", s->inputOffset[t]); 625 } 626 627 printf("\n"); 628 #endif 629 break; 630 631 default: 632 break; 633 } 634 } 635 636 static int module_id; 637 static int shmem_id; 638 639 int usrmotInit(const char *modname) 640 { 641 int retval; 642 643 module_id = rtapi_init(modname); 644 if (module_id < 0) { 645 fprintf(stderr, 646 "usrmotintf: ERROR: rtapi init failed\n"); 647 return -1; 648 } 649 /* get shared memory block from RTAPI */ 650 shmem_id = rtapi_shmem_new(SHMEM_KEY, module_id, sizeof(emcmot_struct_t)); 651 if (shmem_id < 0) { 652 fprintf(stderr, 653 "usrmotintf: ERROR: could not open shared memory\n"); 654 rtapi_exit(module_id); 655 return -1; 656 } 657 /* get address of shared memory area */ 658 retval = rtapi_shmem_getptr(shmem_id, (void **) &emcmotStruct); 659 if (retval < 0) { 660 rtapi_print_msg(RTAPI_MSG_ERR, 661 "usrmotintf: ERROR: could not access shared memory\n"); 662 rtapi_exit(module_id); 663 return -1; 664 } 665 /* got it */ 666 emcmotCommand = &(emcmotStruct->command); 667 emcmotStatus = &(emcmotStruct->status); 668 emcmotDebug = &(emcmotStruct->debug); 669 emcmotConfig = &(emcmotStruct->config); 670 emcmotError = &(emcmotStruct->error); 671 672 inited = 1; 673 674 return 0; 675 } 676 677 int usrmotExit(void) 678 { 679 if (NULL != emcmotStruct) { 680 rtapi_shmem_delete(shmem_id, module_id); 681 rtapi_exit(module_id); 682 } 683 684 emcmotStruct = 0; 685 emcmotCommand = 0; 686 emcmotStatus = 0; 687 emcmotError = 0; 688 /*! \todo Another #if 0 */ 689 #if 0 690 /*! \todo FIXME - comp structs no longer in shmem */ 691 for (axis = 0; axis < EMCMOT_MAX_JOINTS; axis++) { 692 emcmotComp[axis] = 0; 693 } 694 #endif 695 696 inited = 0; 697 return 0; 698 } 699 700 /* Loads pairs of comp from the compensation file. 701 The default way is to specify nominal, forward & reverse triplets in the file 702 However if type != 0, it expects nominal, forward_trim & reverse_trim 703 (where forward_trim = nominal - forward 704 reverse_trim = nominal - reverse) 705 */ 706 int usrmotLoadComp(int joint, const char *file, int type) 707 { 708 FILE *fp; 709 char buffer[LINELEN]; 710 double nom, fwd, rev; 711 int ret = 0; 712 emcmot_command_t emcmotCommand; 713 714 /* check joint range */ 715 if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) { 716 fprintf(stderr, "joint out of range for compensation\n"); 717 return -1; 718 } 719 720 /* open input comp file */ 721 if (NULL == (fp = fopen(file, "r"))) { 722 fprintf(stderr, "can't open compensation file %s\n", file); 723 return -1; 724 } 725 726 while (!feof(fp)) { 727 if (NULL == fgets(buffer, LINELEN, fp)) { 728 break; 729 } 730 if (3 != sscanf(buffer, "%lf %lf %lf", &nom, &fwd, &rev)) { 731 break; 732 } else { 733 // got a triplet 734 if (type == 0) { 735 /* expecting nominal-forward-reverse triplets, e.g., 736 0.000000 0.000000 -0.001279 737 0.100000 0.098742 0.051632 738 0.200000 0.171529 0.194216 */ 739 emcmotCommand.comp_nominal = nom; 740 emcmotCommand.comp_forward = nom - fwd; //convert to diffs 741 emcmotCommand.comp_reverse = nom - rev; //convert to diffs 742 } else { 743 /* expecting nominal-forw_trim-rev_trim triplets */ 744 emcmotCommand.comp_nominal = nom; 745 emcmotCommand.comp_forward = fwd; 746 emcmotCommand.comp_reverse = rev; 747 } 748 emcmotCommand.joint = joint; 749 emcmotCommand.command = EMCMOT_SET_JOINT_COMP; 750 ret |= usrmotWriteEmcmotCommand(&emcmotCommand); 751 } 752 } 753 fclose(fp); 754 755 return ret; 756 } 757 758 759 int usrmotPrintComp(int joint) 760 { 761 /* FIXME-AJ: comp isn't in shmem atm 762 it's in the joint struct, which is only in shmem when STRUCTS_IN_SHM is defined, 763 currently only usrmot uses usrmotPrintComp - might go away */ 764 return -1; 765 /* currently disabled */ 766 #if 0 767 int t; 768 769 /* check axis range */ 770 if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) { 771 fprintf(stderr, "joint out of range for compensation\n"); 772 return -1; 773 } 774 775 /* first check if comp pointer is valid */ 776 if (emcmotComp[joint] == 0) { 777 fprintf(stderr, "compensation data structure not present\n"); 778 return -1; 779 } 780 781 printf("total: %d\n", emcmotComp[joint]->total); 782 printf("avgint: %f\n", emcmotComp[joint]->avgint); 783 for (t = 0; t < emcmotComp[joint]->total; t++) { 784 printf("%f\t%f\t%f\n", 785 emcmotComp[joint]->nominal[t], 786 emcmotComp[joint]->forward[t], emcmotComp[joint]->reverse[t]); 787 } 788 789 return 0; 790 #endif 791 }