usrmotintf.cc

Source code for an Numeric Cmputer

	printf("\n");
	printf("axis acc: \t");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%f ", c.axisLimitAcc[t]);
	}
	printf("\n");
#endif
/*! \todo Another #if 0 */
#if 0
	printf("probe index: %d\n", c.probeIndex);
	printf("probe polarity: %d\n", c.probePolarity);
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    htostr(m, c.axisPolarity[t]);
	    printf("%s ", m);
	}
#endif
	printf("\n");
	break;

    case 1:
	printf("pid stuff is obsolete\n");
/*! \todo Another #if 0 */
#if 0
	printf
	    ("pid:\tP\tI\tD\tFF0\tFF1\tFF2\tBCKLSH\tBIAS\tMAXI\tDEADBAND\tCYCLE TIME\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf
		("\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%f\t%f\n",
		c.pid[t].p, c.pid[t].i, c.pid[t].d, c.pid[t].ff0,
		c.pid[t].ff1, c.pid[t].ff2, c.pid[t].backlash, c.pid[t].bias,
		c.pid[t].maxError, c.pid[t].deadband, c.pid[t].cycleTime);
	}
	printf("\n");
#endif
	break;

    case 3:
/*! \todo Another #if 0 */
#if 0				/*! \todo FIXME - waiting for new structs */
	printf("pos limits:   ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", c.maxLimit[t]);
	}

	printf("\nneg limits:   ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", c.minLimit[t]);
	}

	printf("\nmax ferror:   ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", c.maxFerror[t]);
	}
	printf("\n");

	printf("\nmin ferror:   ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", c.minFerror[t]);
	}
	printf("\n");

	printf("\nhome offsets:  ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", c.homeOffset[t]);
	}
	printf("\n");
#endif
	break;

    default:
	break;
    }

}

/* status printing function */
void usrmotPrintEmcmotStatus(emcmot_status_t s, int which)
{
//    int t;
//    char m[32];

    switch (which) {
    case 0:
	printf("mode:         \t%s\n",
	    s.motionFlag & EMCMOT_MOTION_TELEOP_BIT ? "teleop" :
	    (s.motionFlag & EMCMOT_MOTION_COORD_BIT ? "coord" : "free")
	    );
	printf("cmd:          \t%d\n", s.commandEcho);
	printf("cmd num:      \t%d\n", s.commandNumEcho);
	printf("heartbeat:    \t%u\n", s.heartbeat);
	printf("compute time: \t%f\n", s.computeTime);
/*! \todo Another #if 0 */
#if 0				/*! \todo FIXME - change to work with joint
				   structures */
	printf("axes enabled: \t");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d", s.axisFlag[t] & EMCMOT_AXIS_ENABLE_BIT ? 1 : 0);
	}
	printf("\n");
#endif
	printf("cmd pos:      \t%f\t%f\t%f\t%f\t%f\t%f\n",
	    s.carte_pos_cmd.tran.x, s.carte_pos_cmd.tran.y,
	    s.carte_pos_cmd.tran.z, s.carte_pos_cmd.a, s.carte_pos_cmd.b,
	    s.carte_pos_cmd.c);
	printf("act pos:      \t%f\t%f\t%f\t%f\t%f\t%f\n",
	    s.carte_pos_fb.tran.x, s.carte_pos_fb.tran.y,
	    s.carte_pos_fb.tran.z, s.carte_pos_fb.a, s.carte_pos_fb.b,
	    s.carte_pos_fb.c);
	printf("joint data:\n");
/*! \todo Another #if 0 */
#if 0				/*! \todo FIXME - change to work with joint
				   structures */
	printf(" cmd: ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", s.joint_pos_cmd[t]);
	}
	printf("\n");
	printf(" fb:  ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", s.joint_pos_fb[t]);
	}
	printf("\n");
	printf(" vel: ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", s.joint_vel_cmd[t]);
	}
	printf("\n");
	printf(" ferr:");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", s.ferrorCurrent[t]);
	}
	printf("\n");
	printf(" lim:");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", s.ferrorLimit[t]);
	}
	printf("\n");
	printf(" max:");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", s.ferrorHighMark[t]);
	}
	printf("\n");
#endif
	printf("velocity:     \t%f\n", s.vel);
	printf("accel:        \t%f\n", s.acc);
	printf("id:           \t%d\n", s.id);
	printf("depth:        \t%d\n", s.depth);
	printf("active depth: \t%d\n", s.activeDepth);
	printf("inpos:        \t%d\n",
	    s.motionFlag & EMCMOT_MOTION_INPOS_BIT ? 1 : 0);
/*! \todo Another #if 0 */
#if 0				/*! \todo FIXME - change to work with joint
				   structures */
	printf("vscales:      \tQ: %.2f", s.qVscale);
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%d: %.2f", t, s.axVscale[t]);
	}
	printf("\n");
#endif
/*! \todo Another #if 0 */
#if 0				/*! \todo FIXME - change to work with joint
				   structures */
	printf("homing:       \t");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d", s.axisFlag[0] & EMCMOT_AXIS_HOMING_BIT ? 1 : 0);
	}
	printf("\n");
#endif
	printf("enabled:     \t%s\n",
	    s.motionFlag & EMCMOT_MOTION_ENABLE_BIT ? "ENABLED" : "DISABLED");
	printf("probe value: %d\n", s.probeVal);
	printf("probe Tripped: %d\n", s.probeTripped);
	printf("probing: %d\n", s.probing);
	printf("probed pos:      \t%f\t%f\t%f\n",
	    s.probedPos.tran.x, s.probedPos.tran.y, s.probedPos.tran.z);
	break;

    case 2:
	/* print motion and axis flags */
/*! \todo Another #if 0 */
#if 0				/*! \todo FIXME - change to work with joint
				   structures */
	htostr(m, s.motionFlag);
	printf("motion:   %s\n", m);
	printf("axes:     ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    htostr(m, s.axisFlag[t]);
	    printf("%s ", m);
	}
	printf("\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t", ((s.axisFlag[t] & EMCMOT_AXIS_ENABLE_BIT) != 0));
	}
	printf("enable\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t", ((s.axisFlag[t] & EMCMOT_AXIS_ACTIVE_BIT) != 0));
	}
	printf("active\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t", ((s.axisFlag[t] & EMCMOT_AXIS_INPOS_BIT) != 0));
	}
	printf("inpos\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t", ((s.axisFlag[t] & EMCMOT_AXIS_ERROR_BIT) != 0));
	}
	printf("error\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t",
		((s.axisFlag[t] & EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT) != 0));
	}
	printf("max_soft_limit\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t",
		((s.axisFlag[t] & EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT) != 0));
	}
	printf("min_soft_limit\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t",
		((s.axisFlag[t] & EMCMOT_AXIS_MAX_HARD_LIMIT_BIT) != 0));
	}
	printf("max_hard_limit\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t",
		((s.axisFlag[t] & EMCMOT_AXIS_MIN_HARD_LIMIT_BIT) != 0));
	}
	printf("min_hard_limit\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t",
		((s.axisFlag[t] & EMCMOT_AXIS_HOME_SWITCH_BIT) != 0));
	}
	printf("home_switch\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t", ((s.axisFlag[t] & EMCMOT_AXIS_HOMING_BIT) != 0));
	}
	printf("homing\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t", ((s.axisFlag[t] & EMCMOT_AXIS_HOMED_BIT) != 0));
	}
	printf("homed\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t", ((s.axisFlag[t] & EMCMOT_AXIS_FERROR_BIT) != 0));
	}
	printf("ferror\n");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("%d\t", ((s.axisFlag[t] & EMCMOT_AXIS_FAULT_BIT) != 0));
	}
#endif
	printf("fault\n");
	printf("\npolarity: ");
	printf("limit override: %d\n", s.overrideLimits);
	break;

    case 4:
	printf("scales handled in HAL now!\n");
/*! \todo Another #if 0 */
#if 0
	printf("output scales: ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", s.outputScale[t]);
	}

	printf("\noutput offsets:");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", s.outputOffset[t]);
	}

	printf("\ninput scales:  ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", s.inputScale[t]);
	}

	printf("\ninput offsets: ");
	for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
	    printf("\t%f", s.inputOffset[t]);
	}

	printf("\n");
#endif
	break;

    default:
	break;
    }
}

static int module_id;
static int shmem_id;

int usrmotInit(char *modname)
{
    int retval;

    module_id = rtapi_init(modname);
    if (module_id < 0) {
	fprintf(stderr,
	    "usrmotintf: ERROR: rtapi init failed\n");
	return -1;
    }
    /* get shared memory block from RTAPI */
    shmem_id = rtapi_shmem_new(SHMEM_KEY, module_id, sizeof(emcmot_struct_t));
    if (shmem_id < 0) {
	fprintf(stderr,
	    "usrmotintf: ERROR: could not open shared memory\n");
	rtapi_exit(module_id);
	return -1;
    }
    /* get address of shared memory area */
    retval = rtapi_shmem_getptr(shmem_id, (void **) &emcmotStruct);
    if (retval != RTAPI_SUCCESS) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "usrmotintf: ERROR: could not access shared memory\n");
	rtapi_exit(module_id);
	return -1;
    }
    /* got it */
    emcmotCommand = &(emcmotStruct->command);
    emcmotStatus = &(emcmotStruct->status);
    emcmotDebug = &(emcmotStruct->debug);
    emcmotConfig = &(emcmotStruct->config);
    emcmotError = &(emcmotStruct->error);

    emcmotshmem = emcmotStruct;

    inited = 1;

    return 0;
}

int usrmotExit(void)
{
    if (NULL != emcmotStruct) {
	rtapi_shmem_delete(shmem_id, module_id);
	rtapi_exit(module_id);
    }

    emcmotStruct = 0;
    emcmotCommand = 0;
    emcmotStatus = 0;
    emcmotError = 0;
/*! \todo Another #if 0 */
#if 0
/*! \todo FIXME - comp structs no longer in shmem */
    for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
	emcmotComp[axis] = 0;
    }
#endif
    emcmotshmem = 0;

    inited = 0;
    return 0;
}

int usrmotLoadComp(int axis, const char *file)
{
/*! \todo FIXME - this routine currently assumes that the comp structs
   reside in shared memory, and writes directly to them.  The
   comp structs have been moved out of shmem, so this needs to
   be re-written to use a command to load the struct.  In the
   meantime, we return -1 to indicate failure.
*/
return -1;
/*! \todo Another #if 0 */
#if 0

    FILE *fp;
    char buffer[LINELEN];
    double nom, fwd, rev;
    int index = 0;
    int total = 0;

    /* check axis range */
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	fprintf(stderr, "axis out of range for compensation\n");
	return -1;
    }

    /* first check if comp pointer is valid */
    if (emcmotComp[axis] == 0) {
	fprintf(stderr, "compensation data structure not present\n");
	return -1;
    }

    /* open input comp file */
    if (NULL == (fp = fopen(file, "r"))) {
	fprintf(stderr, "can't open compensation file %s\n", file);
	return -1;
    }

    while (!feof(fp)) {
	if (NULL == fgets(buffer, LINELEN, fp)) {
	    break;
	}
	/*
	   expecting nominal-forward-reverse triplets, e.g., 0.000000
	   0.000000 -0.001279 0.100000 0.098742 0.051632 0.200000 0.171529
	   0.194216 */
	if (3 != sscanf(buffer, "%lf %lf %lf", &nom, &fwd, &rev)) {
	    break;
	}
	if (index >= EMCMOT_COMP_SIZE) {
	    break;
	}
	emcmotComp[axis]->nominal[index] = nom;
	emcmotComp[axis]->forward[index] = fwd;
	emcmotComp[axis]->reverse[index] = rev;
	index++;
	total++;
    }
    fclose(fp);

    if (total > 1) {
	emcmotComp[axis]->avgint = (emcmotComp[axis]->nominal[total - 1] -
	    emcmotComp[axis]->nominal[0]) / (total - 1);
    }

    /* ->total is the flag to emcmot that the comp table is valid, so only
       set this to be >1 if the data is really valid: total > 1 and avgint >
       0 */
    if (total > 1 && emcmotComp[axis]->avgint > DBL_MIN) {
	emcmotComp[axis]->total = total;
    } else {
	fprintf(stderr, "compensation file %s has too few distinct points\n",
	    file);
	return -1;
    }

    /* leave alter alone */

    return 0;
#endif
}

int usrmotAlter(int axis, double alter)
{
/*! \todo FIXME - the comp stuff is temporarily disabled, so "alter" must
   be disabled too.  BTW, what the heck is "alter" anyway?
*/
return -1;
/*! \todo Another #if 0 */
#if 0
    /* check axis range */
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	fprintf(stderr, "axis out of range for alter\n");
	return -1;
    }

    /* first check if comp pointer is valid */
    if (emcmotComp[axis] == 0) {
	fprintf(stderr, "compensation data structure not present\n");
	return -1;
    }

    /* set alter value */
    emcmotComp[axis]->alter = alter;

    return 0;
#endif
}

int usrmotQueryAlter(int axis, double *alter)
{
/*! \todo FIXME - the comp stuff is temporarily disabled, so "alter" must
   be disabled too.  BTW, what the heck is "alter" anyway?
*/
return -1;
/*! \todo Another #if 0 */
#if 0
    /* check axis range */
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	fprintf(stderr, "axis out of range for alter query\n");
	return -1;
    }

    /* first check if comp pointer is valid */
    if (emcmotComp[axis] == 0) {
	fprintf(stderr, "compensation data structure not present\n");
	return -1;
    }

    /* set alter value */
    *alter = emcmotComp[axis]->alter;

    return 0;
#endif
}

int usrmotPrintComp(int axis)
{
/*! \todo FIXME - the comp stuff is temporarily disabled */
return -1;
/*! \todo Another #if 0 */
#if 0
    int t;

    /* check axis range */
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	fprintf(stderr, "axis out of range for compensation\n");
	return -1;
    }

    /* first check if comp pointer is valid */
    if (emcmotComp[axis] == 0) {
	fprintf(stderr, "compensation data structure not present\n");
	return -1;
    }

    printf("total:  %d\n", emcmotComp[axis]->total);
    printf("avgint: %f\n", emcmotComp[axis]->avgint);
    printf("alter:  %f\n", emcmotComp[axis]->alter);
    for (t = 0; t < emcmotComp[axis]->total; t++) {
	printf("%f\t%f\t%f\n",
	    emcmotComp[axis]->nominal[t],
	    emcmotComp[axis]->forward[t], emcmotComp[axis]->reverse[t]);
    }

    return 0;
#endif
}