}int emcAxisHalt(int axis){    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {	return 0;    }    /*! \todo FIXME-- refs global emcStatus; should make EMC_AXIS_STAT an arg here */    if (NULL != emcStatus && emcmotion_initialized	&& emcmotAxisInited[axis]) {	dumpAxis(axis, EMC_INIFILE, &emcStatus->motion.axis[axis]);    }    emcmotAxisInited[axis] = 0;    if (!AxisOrTrajInited()) {	usrmotExit();		// ours is final exit    }    return 0;}int emcAxisAbort(int axis){    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {	return 0;    }    emcmotCommand.command = EMCMOT_AXIS_ABORT;    emcmotCommand.axis = axis;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcAxisActivate(int axis){    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {	return 0;    }    emcmotCommand.command = EMCMOT_ACTIVATE_JOINT;    emcmotCommand.axis = axis;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcAxisDeactivate(int axis){    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {	return 0;    }    emcmotCommand.command = EMCMOT_DEACTIVATE_JOINT;    emcmotCommand.axis = axis;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcAxisOverrideLimits(int axis){    // can have axis < 0, for resuming normal limit checking    if (axis >= EMCMOT_MAX_AXIS) {	return 0;    }    emcmotCommand.command = EMCMOT_OVERRIDE_LIMITS;    emcmotCommand.axis = axis;    return usrmotWriteEmcmotCommand(&emcmotCommand);}/*! \todo Another #if 0 */#if 0int emcAxisSetOutput(int axis, double output){    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {	return 0;    }    emcmotCommand.command = EMCMOT_DAC_OUT;    emcmotCommand.axis = axis;    emcmotCommand.dacOut = output;    return usrmotWriteEmcmotCommand(&emcmotCommand);}#endifint emcAxisEnable(int axis){    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {	return 0;    }    emcmotCommand.command = EMCMOT_ENABLE_AMPLIFIER;    emcmotCommand.axis = axis;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcAxisDisable(int axis){    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {	return 0;    }    emcmotCommand.command = EMCMOT_DISABLE_AMPLIFIER;    emcmotCommand.axis = axis;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcAxisHome(int axis){    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {	return 0;    }    emcmotCommand.command = EMCMOT_HOME;    emcmotCommand.axis = axis;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcAxisJog(int axis, double vel){    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {	return 0;    }    if (vel > AXIS_MAX_VELOCITY[axis]) {	vel = AXIS_MAX_VELOCITY[axis];    } else if (vel < -AXIS_MAX_VELOCITY[axis]) {	vel = -AXIS_MAX_VELOCITY[axis];    }    emcmotCommand.command = EMCMOT_JOG_CONT;    emcmotCommand.axis = axis;    emcmotCommand.vel = vel;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcAxisIncrJog(int axis, double incr, double vel){    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {	return 0;    }    if (vel > AXIS_MAX_VELOCITY[axis]) {	vel = AXIS_MAX_VELOCITY[axis];    } else if (vel < -AXIS_MAX_VELOCITY[axis]) {	vel = -AXIS_MAX_VELOCITY[axis];    }    emcmotCommand.command = EMCMOT_JOG_INCR;    emcmotCommand.axis = axis;    emcmotCommand.vel = vel;    emcmotCommand.offset = incr;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcAxisAbsJog(int axis, double pos, double vel){    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {	return 0;    }    if (vel > AXIS_MAX_VELOCITY[axis]) {	vel = AXIS_MAX_VELOCITY[axis];    } else if (vel < -AXIS_MAX_VELOCITY[axis]) {	vel = -AXIS_MAX_VELOCITY[axis];    }    emcmotCommand.command = EMCMOT_JOG_ABS;    emcmotCommand.axis = axis;    emcmotCommand.vel = vel;    emcmotCommand.offset = pos;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcAxisLoadComp(int axis, const char *file){    return usrmotLoadComp(axis, file);}int emcAxisAlter(int axis, double alter){    return usrmotAlter(axis, alter);}static emcmot_config_t emcmotConfig;int get_emcmot_debug_info = 0;/*  these globals are set in emcMotionUpdate(), then referenced in  emcAxisUpdate(), emcTrajUpdate() to save calls to usrmotReadEmcmotStatus *//*! \todo FIXME - next line commented out and moved to top of file for debugging *///static emcmot_status_t emcmotStatus;*/static emcmot_debug_t emcmotDebug;static char errorString[EMCMOT_ERROR_LEN];static int new_config = 0;/*! \todo FIXME - debugging - uncomment the following line to log changes in   AXIS_FLAG */// #define WATCH_FLAGS 1int emcAxisUpdate(EMC_AXIS_STAT stat[], int numAxes){/*! \todo FIXME - this function accesses data that has been   moved.  Once I know what it is used for I'll fix it */    int axis;    emcmot_joint_status_t *joint;#ifdef WATCH_FLAGS    static int old_joint_flag[8];#endif    // check for valid range    if (numAxes <= 0 || numAxes > EMCMOT_MAX_AXIS) {	return -1;    }    for (axis = 0; axis < numAxes; axis++) {	/* point to joint data */	joint = &(emcmotStatus.joint_status[axis]);	stat[axis].axisType = localEmcAxisAxisType[axis];	stat[axis].units = localEmcAxisUnits[axis];/*! \todo Another #if 0 */#if 0	stat[axis].inputScale = emcmotStatus.inputScale[axis];	stat[axis].inputOffset = emcmotStatus.inputOffset[axis];	stat[axis].outputScale = emcmotStatus.outputScale[axis];	stat[axis].outputOffset = emcmotStatus.outputOffset[axis];#endif	if (new_config) {	    stat[axis].backlash = joint->backlash;	    stat[axis].minPositionLimit = joint->min_pos_limit;	    stat[axis].maxPositionLimit = joint->max_pos_limit;	    stat[axis].minFerror = joint->min_ferror;	    stat[axis].maxFerror = joint->max_ferror;/*! \todo FIXME - should all homing config params be included here? */	    stat[axis].homeOffset = joint->home_offset;	}	stat[axis].setpoint = joint->pos_cmd;	/*! \todo FIXME - output is the DAC output, now part of HAL */	stat[axis].output = 0.0;	stat[axis].input = joint->pos_fb;	if (get_emcmot_debug_info) {	    stat[axis].ferrorCurrent = joint->ferror;	    stat[axis].ferrorHighMark = joint->ferror_high_mark;	}	stat[axis].homing = (joint->flag & EMCMOT_AXIS_HOMING_BIT ? 1 : 0);	stat[axis].homed = (joint->flag & EMCMOT_AXIS_HOMED_BIT ? 1 : 0);	stat[axis].fault = (joint->flag & EMCMOT_AXIS_FAULT_BIT ? 1 : 0);	stat[axis].enabled =	    (joint->flag & EMCMOT_AXIS_ENABLE_BIT ? 1 : 0);	stat[axis].minSoftLimit =	    (joint->flag & EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT ? 1 : 0);	stat[axis].maxSoftLimit =	    (joint->flag & EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT ? 1 : 0);	stat[axis].minHardLimit =	    (joint->flag & EMCMOT_AXIS_MIN_HARD_LIMIT_BIT ? 1 : 0);	stat[axis].maxHardLimit =	    (joint->flag & EMCMOT_AXIS_MAX_HARD_LIMIT_BIT ? 1 : 0);	stat[axis].overrideLimits = (emcmotStatus.overrideLimits);	// one	// for	// all/*! \todo Another #if 0 */#if 0				/*! \todo FIXME - per-axis Vscale temporarily? removed */	stat[axis].scale = emcmotStatus.axVscale[axis];#endif	usrmotQueryAlter(axis, &stat[axis].alter);#ifdef WATCH_FLAGS	if (old_joint_flag[axis] != joint->flag) {	    printf("joint %d flag: %04X -> %04X\n", axis,		   old_joint_flag[axis], joint->flag);	    old_joint_flag[axis] = joint->flag;	}#endif	if (joint->flag & EMCMOT_AXIS_ERROR_BIT) {	    if (stat[axis].status != RCS_ERROR) {		rcs_print_error("Error on axis %d, command number %d\n",				axis, emcmotStatus.commandNumEcho);		stat[axis].status = RCS_ERROR;	    }	} else if (joint->flag & EMCMOT_AXIS_INPOS_BIT) {	    stat[axis].status = RCS_DONE;	} else {	    stat[axis].status = RCS_EXEC;	}    }    return 0;}// EMC_TRAJ functions// local status data, not provided by emcmotstatic int localEmcTrajAxes = 0;static double localEmcTrajLinearUnits = 1.0;static double localEmcTrajAngularUnits = 1.0;static int localEmcTrajMotionId = 0;int emcTrajSetAxes(int axes){    if (axes <= 0 || axes > EMCMOT_MAX_AXIS) {	return -1;    }    localEmcTrajAxes = axes;    emcmotCommand.command = EMCMOT_SET_NUM_AXES;    emcmotCommand.axis = axes;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajSetUnits(double linearUnits, double angularUnits){    if (linearUnits <= 0.0 || angularUnits <= 0.0) {	return -1;    }    localEmcTrajLinearUnits = linearUnits;    localEmcTrajAngularUnits = angularUnits;    return 0;}/*! \todo Another #if 0 */#if 0int emcTrajSetCycleTime(double cycleTime){    if (cycleTime <= 0.0) {	return -1;    }    emcmotCommand.command = EMCMOT_SET_TRAJ_CYCLE_TIME;    emcmotCommand.cycleTime = cycleTime;    return usrmotWriteEmcmotCommand(&emcmotCommand);}#endifint emcTrajSetMode(int mode){    switch (mode) {    case EMC_TRAJ_MODE_FREE:	emcmotCommand.command = EMCMOT_FREE;	return usrmotWriteEmcmotCommand(&emcmotCommand);    case EMC_TRAJ_MODE_COORD:	emcmotCommand.command = EMCMOT_COORD;	return usrmotWriteEmcmotCommand(&emcmotCommand);    case EMC_TRAJ_MODE_TELEOP:	emcmotCommand.command = EMCMOT_TELEOP;	return usrmotWriteEmcmotCommand(&emcmotCommand);    default:	return -1;    }}int emcTrajSetTeleopVector(EmcPose vel){    emcmotCommand.command = EMCMOT_SET_TELEOP_VECTOR;    emcmotCommand.pos = vel;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajSetVelocity(double vel, double ini_maxvel){    int retval;    if (vel < 0.0) {	vel = 0.0;    } else if (vel > TRAJ_MAX_VELOCITY) {	vel = TRAJ_MAX_VELOCITY;    }    if (ini_maxvel < 0.0) {	    ini_maxvel = 0.0;    } else if (vel > TRAJ_MAX_VELOCITY) {	    ini_maxvel = TRAJ_MAX_VELOCITY;    }/*! \todo Another #if 0 */#if 0    /*! \todo FIXME-- this fixes rapid rate reset problem */    if (vel == lastVel) {	// suppress it	return 0;    }#endif    emcmotCommand.command = EMCMOT_SET_VEL;    emcmotCommand.vel = vel;    emcmotCommand.ini_maxvel = ini_maxvel;    retval = usrmotWriteEmcmotCommand(&emcmotCommand);    if (0 == retval) {	lastVel = vel;	last_ini_maxvel = ini_maxvel;    }    return retval;}int emcTrajSetAcceleration(double acc){    if (acc < 0.0) {	acc = 0.0;    } else if (acc > localEmcMaxAcceleration) {	acc = localEmcMaxAcceleration;    }    emcmotCommand.command = EMCMOT_SET_ACC;    emcmotCommand.acc = acc;    return usrmotWriteEmcmotCommand(&emcmotCommand);}/*  emcmot has no limits on max velocity, acceleration so we'll save them  here and apply them in the functions above  */int emcTrajSetMaxVelocity(double vel){    if (vel < 0.0) {	vel = 0.0;    }    TRAJ_MAX_VELOCITY = vel;    emcmotCommand.command = EMCMOT_SET_VEL_LIMIT;    emcmotCommand.vel = vel;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajSetMaxAcceleration(double acc){    if (acc < 0.0) {	acc = 0.0;    }    localEmcMaxAcceleration = acc;    return 0;}int emcTrajSetHome(EmcPose home){    emcmotCommand.command = EMCMOT_SET_WORLD_HOME;    emcmotCommand.pos = home;#ifdef ISNAN_TRAP    if (isnan(emcmotCommand.pos.tran.x) ||	isnan(emcmotCommand.pos.tran.y)	|| isnan(emcmotCommand.pos.tran.z)) {	printf("isnan error in emcTrajSetHome\n");	return 0;		// ignore it for now, just don't send it    }#endif    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajSetScale(double scale){    if (scale < 0.0) {	scale = 0.0;    }    emcmotCommand.command = EMCMOT_SCALE;    emcmotCommand.scale = scale;    return usrmotWriteEmcmotCommand(&emcmotCommand);}int emcTrajSetMotionId(int id){    if (EMC_DEBUG_MOTION_TIME & EMC_DEBUG) {	if (id != localEmcTrajMotionId) {	    rcs_print("Outgoing motion id is %d.\n", id);	}    }    localEmcTrajMotionId = id;