return 0;}int emcTrajInit(){    int retval = 0;    // init emcmot interface    if (!AxisOrTrajInited()) {	usrmotIniLoad(EMC_INIFILE);	if (0 != usrmotInit("emc2_task")) {	    return -1;	}    }    emcmotTrajInited = 1;    // initialize parameters from ini file    if (0 != iniTraj(EMC_INIFILE)) {	retval = -1;    }    return retval;}int emcTrajHalt(){    emcmotTrajInited = 0;    if (!AxisOrTrajInited()) {	usrmotExit();		// ours is final exit    }    return 0;}int emcTrajEnable(){    emcmotCommand.command = EMCMOT_ENABLE;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajDisable(){    emcmotCommand.command = EMCMOT_DISABLE;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajAbort(){    emcmotCommand.command = EMCMOT_ABORT;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajPause(){    emcmotCommand.command = EMCMOT_PAUSE;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajStep(){    emcmotCommand.command = EMCMOT_STEP;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajResume(){    emcmotCommand.command = EMCMOT_RESUME;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajDelay(double delay){    /* nothing need be done here - it's done in task controller */    return 0;}int emcTrajSetSpindleSync(double sync) {    emcmotCommand.command = EMCMOT_SET_SPINDLESYNC;    emcmotCommand.spindlesync = sync;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajSetTermCond(int cond, double tolerance){    emcmotCommand.command = EMCMOT_SET_TERM_COND;    emcmotCommand.termCond =	(cond ==	 EMC_TRAJ_TERM_COND_STOP ? EMCMOT_TERM_COND_STOP :	 EMCMOT_TERM_COND_BLEND);    emcmotCommand.tolerance = tolerance;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajLinearMove(EmcPose end, int type){    emcmotCommand.command = EMCMOT_SET_LINE;    emcmotCommand.pos = end;    emcmotCommand.id = localEmcTrajMotionId;    emcmotCommand.motion_type = type;#ifdef ISNAN_TRAP    if (isnan(emcmotCommand.pos.tran.x) ||	isnan(emcmotCommand.pos.tran.y)	|| isnan(emcmotCommand.pos.tran.z)) {	printf("isnan error in emcTrajLinearMove\n");	return 0;		// ignore it for now, just don't send it    }#endif    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajCircularMove(EmcPose end, PM_CARTESIAN center,			PM_CARTESIAN normal, int turn, int type){    emcmotCommand.command = EMCMOT_SET_CIRCLE;    emcmotCommand.pos = end;    emcmotCommand.motion_type = type;    emcmotCommand.center.x = center.x;    emcmotCommand.center.y = center.y;    emcmotCommand.center.z = center.z;    emcmotCommand.normal.x = normal.x;    emcmotCommand.normal.y = normal.y;    emcmotCommand.normal.z = normal.z;    emcmotCommand.turn = turn;    emcmotCommand.id = localEmcTrajMotionId;#ifdef ISNAN_TRAP    if (isnan(emcmotCommand.pos.tran.x) ||	isnan(emcmotCommand.pos.tran.y) ||	isnan(emcmotCommand.pos.tran.z) ||	isnan(emcmotCommand.pos.a) ||	isnan(emcmotCommand.pos.b) ||	isnan(emcmotCommand.pos.c) ||	isnan(emcmotCommand.center.x) ||	isnan(emcmotCommand.center.y) ||	isnan(emcmotCommand.center.z) ||	isnan(emcmotCommand.normal.x) ||	isnan(emcmotCommand.normal.y) || isnan(emcmotCommand.normal.z)) {	printf("isnan error in emcTrajCircularMove\n");	return 0;		// ignore it for now, just don't send it    }#endif    return usrmotWriteEmcmotCommand(&emcmotCommand);}/*! \todo Another #if 0 */#if 0int emcTrajSetProbeIndex(int index){    emcmotCommand.command = EMCMOT_SET_PROBE_INDEX;    emcmotCommand.probeIndex = index;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajSetProbePolarity(int polarity){    emcmotCommand.command = EMCMOT_SET_PROBE_POLARITY;    emcmotCommand.level = polarity;    return usrmotWriteEmcmotCommand(&emcmotCommand);}#endif				/* #if 0 */int emcTrajClearProbeTrippedFlag(){    emcmotCommand.command = EMCMOT_CLEAR_PROBE_FLAGS;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajProbe(EmcPose pos){    emcmotCommand.command = EMCMOT_PROBE;    emcmotCommand.pos.tran.x = pos.tran.x;    emcmotCommand.pos.tran.y = pos.tran.y;    emcmotCommand.pos.tran.z = pos.tran.z;    emcmotCommand.id = localEmcTrajMotionId;    return usrmotWriteEmcmotCommand(&emcmotCommand);}static int last_id = 0;static int last_id_printed = 0;static int last_status = 0;static double last_id_time;int emcTrajUpdate(EMC_TRAJ_STAT * stat){    int axis;    stat->axes = localEmcTrajAxes;    stat->linearUnits = localEmcTrajLinearUnits;    stat->angularUnits = localEmcTrajAngularUnits;    stat->mode =	emcmotStatus.	motionFlag & EMCMOT_MOTION_TELEOP_BIT ? EMC_TRAJ_MODE_TELEOP	: (emcmotStatus.	   motionFlag & EMCMOT_MOTION_COORD_BIT ? EMC_TRAJ_MODE_COORD :	   EMC_TRAJ_MODE_FREE);    /* enabled if motion enabled and all axes enabled */    stat->enabled = 0;		/* start at disabled */    if (emcmotStatus.motionFlag & EMCMOT_MOTION_ENABLE_BIT) {	for (axis = 0; axis < localEmcTrajAxes; axis++) {/*! \todo Another #if 0 */#if 0				/*! \todo FIXME - the axis flag has been moved to the joint struct */	    if (!emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_ENABLE_BIT) {		break;	    }#endif	    /* got here, then all are enabled */	    stat->enabled = 1;	}    }    stat->inpos =	emcmotStatus.motionFlag & EMCMOT_MOTION_INPOS_BIT ? 1 : 0;    stat->queue = emcmotStatus.depth;    stat->activeQueue = emcmotStatus.activeDepth;    stat->queueFull = emcmotStatus.queueFull;    stat->id = emcmotStatus.id;    stat->motion_type = emcmotStatus.motionType;    if (EMC_DEBUG_MOTION_TIME & EMC_DEBUG) {	if (stat->id != last_id) {	    if (last_id != last_id_printed) {		rcs_print("Motion id %d took %f seconds.\n", last_id,			  etime() - last_id_time);		last_id_printed = last_id;	    }	    last_id = stat->id;	    last_id_time = etime();	}    }    stat->paused = emcmotStatus.paused;    stat->scale = emcmotStatus.qVscale;    stat->position = emcmotStatus.carte_pos_cmd;    stat->actualPosition = emcmotStatus.carte_pos_fb;    stat->velocity = emcmotStatus.vel;    stat->acceleration = emcmotStatus.acc;    stat->maxAcceleration = localEmcMaxAcceleration;    if (emcmotStatus.motionFlag & EMCMOT_MOTION_ERROR_BIT) {	stat->status = RCS_ERROR;    } else if (stat->inpos && (stat->queue == 0)) {	stat->status = RCS_DONE;    } else {	stat->status = RCS_EXEC;    }    if (EMC_DEBUG_MOTION_TIME & EMC_DEBUG) {	if (stat->status == RCS_DONE && last_status != RCS_DONE	    && stat->id != last_id_printed) {	    rcs_print("Motion id %d took %f seconds.\n", last_id,		      etime() - last_id_time);	    last_id_printed = last_id = stat->id;	    last_id_time = etime();	}    }    stat->probedPosition.tran.x = emcmotStatus.probedPos.tran.x;    stat->probedPosition.tran.y = emcmotStatus.probedPos.tran.y;    stat->probedPosition.tran.z = emcmotStatus.probedPos.tran.z;    stat->probeval = emcmotStatus.probeVal;    stat->probe_tripped = emcmotStatus.probeTripped;    if (new_config) {	stat->cycleTime = emcmotConfig.trajCycleTime;/*! \todo Another #if 0 */#if 0	stat->probe_index = emcmotConfig.probeIndex;	stat->probe_polarity = emcmotConfig.probePolarity;#endif	stat->kinematics_type = emcmotConfig.kinematics_type;	stat->maxVelocity = emcmotConfig.limitVel;    }    return 0;}// EMC_MOTION functionsint emcMotionInit(){    int r1;    int r2;    int axis;    r1 = -1;    for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {	if (0 == emcAxisInit(axis)) {	    r1 = 0;		// at least one is okay	}    }    r2 = emcTrajInit();    if (r1 == 0 && r2 == 0) {	emcmotion_initialized = 1;    }    return (r1 == 0 && r2 == 0) ? 0 : -1;}int emcMotionHalt(){    int r1, r2, r3;    int t;    r1 = -1;    for (t = 0; t < EMCMOT_MAX_AXIS; t++) {	if (0 == emcAxisHalt(t)) {	    r1 = 0;		// at least one is okay	}    }    r2 = emcTrajDisable();    r3 = emcTrajHalt();    emcmotion_initialized = 0;    return (r1 == 0 && r2 == 0 && r3 == 0) ? 0 : -1;}int emcMotionAbort(){    int r1;    int r2;    int t;    r1 = -1;    for (t = 0; t < EMCMOT_MAX_AXIS; t++) {	if (0 == emcAxisAbort(t)) {	    r1 = 0;		// at least one is okay	}    }    r2 = emcTrajAbort();    // reset optimization flag which suppresses duplicate speed requests    lastVel = -1.0;    return (r1 == 0 && r2 == 0) ? 0 : -1;}int emcMotionSetDebug(int debug){    emcmotCommand.command = EMCMOT_SET_DEBUG;    emcmotCommand.debug = debug;    return usrmotWriteEmcmotCommand(&emcmotCommand);}/*! \function emcMotionSetAout()        This function sends a EMCMOT_SET_AOUT message to the motion controller.    That one plans a AOUT command when motion starts or right now.    @parameter	index	which output gets modified    @parameter	now	wheather change is imediate or synched with motion    @parameter	start	value set at start of motion    @parameter	end	value set at end of motion*/int emcMotionSetAout(unsigned char index, double start, double end, unsigned char now){    emcmotCommand.command = EMCMOT_SET_AOUT;    emcmotCommand.now = now;    emcmotCommand.out = index;  /*! \todo FIXME-- if this works, set up some dedicated cmd fields instead of     borrowing these */    emcmotCommand.minLimit = start;    emcmotCommand.maxLimit = end;    return usrmotWriteEmcmotCommand(&emcmotCommand);}/*! \function emcMotionSetDout()        This function sends a EMCMOT_SET_DOUT message to the motion controller.    That one plans a DOUT command when motion starts or right now.    @parameter	index	which output gets modified    @parameter	now	wheather change is imediate or synched with motion    @parameter	start	value set at start of motion    @parameter	end	value set at end of motion*/int emcMotionSetDout(unsigned char index, unsigned char start,		     unsigned char end, unsigned char now){    emcmotCommand.command = EMCMOT_SET_DOUT;    emcmotCommand.now = now;    emcmotCommand.out = index;    emcmotCommand.start = start;    emcmotCommand.end = end;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcMotionUpdate(EMC_MOTION_STAT * stat){    int r1;    int r2;    int axis;    int error;    int exec;    // read the emcmot status    if (0 != usrmotReadEmcmotStatus(&emcmotStatus)) {	return -1;    }    new_config = 0;    if (emcmotStatus.config_num != emcmotConfig.config_num) {	if (0 != usrmotReadEmcmotConfig(&emcmotConfig)) {	    return -1;	}	new_config = 1;    }    if (get_emcmot_debug_info) {	if (0 != usrmotReadEmcmotDebug(&emcmotDebug)) {	    return -1;	}    }    // read the emcmot error    if (0 != usrmotReadEmcmotError(errorString)) {	// no error, so ignore    } else {	// an error to report	emcOperatorError(0, errorString);    }    // save the heartbeat and command number locally,    // for use with emcMotionUpdate    localMotionHeartbeat = emcmotStatus.heartbeat;    localMotionCommandType = emcmotStatus.commandEcho;	/*! \todo FIXME-- not NML one! */    localMotionEchoSerialNumber = emcmotStatus.commandNumEcho;    r1 = emcAxisUpdate(&stat->axis[0], EMCMOT_MAX_AXIS);    r2 = emcTrajUpdate(&stat->traj);    stat->heartbeat = localMotionHeartbeat;    stat->command_type = localMotionCommandType;    stat->echo_serial_number = localMotionEchoSerialNumber;    stat->debug = emcmotConfig.debug;    // set the status flag    error = 0;    exec = 0;    for (axis = 0; axis < stat->traj.axes; axis++) {	if (stat->axis[axis].status == RCS_ERROR) {	    error = 1;	    break;	}	if (stat->axis[axis].status == RCS_EXEC) {	    exec = 1;	    break;	}    }    if (stat->traj.status == RCS_ERROR) {	error = 1;    } else if (stat->traj.status == RCS_EXEC) {	exec = 1;    }    if (error) {	stat->status = RCS_ERROR;    } else if (exec) {	stat->status = RCS_EXEC;    } else {	stat->status = RCS_DONE;    }    return (r1 == 0 && r2 == 0) ? 0 : -1;}