taskintf.cc

Source code for an Numeric Cmputer

}

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 0
int 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);
}
#endif

int 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 1

int 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 emcmot
static 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 0
int emcTrajSetCycleTime(double cycleTime)
{
    if (cycleTime <= 0.0) {
	return -1;
    }

    emcmotCommand.command = EMCMOT_SET_TRAJ_CYCLE_TIME;
    emcmotCommand.cycleTime = cycleTime;

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}
#endif

int 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;