taskintf.cc

Source code for an Numeric Cmputer

/********************************************************************
* Description: taskintf.cc
*   Interface functions for motion.
*
*   Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*    
* Copyright (c) 2004 All rights reserved.
*
* Last change:
* $Revision: 1.37 $
* $Author: cradek $
* $Date: 2006/03/21 02:42:31 $
********************************************************************/

#include <math.h>		// isnan()
#include <float.h>		// DBL_MAX
#include <string.h>		// memcpy() strncpy()

#include "usrmotintf.h"		// usrmotInit(), usrmotReadEmcmotStatus(),
				// etc.
#include "motion.h"		// emcmot_command_t,STATUS, etc.
#include "emcglb.h"		// EMC_INIFILE

#include "iniaxis.hh"
#include "initraj.hh"

/* define this to catch isnan errors, for rtlinux FPU register 
   problem testing */
#define ISNAN_TRAP

// MOTION INTERFACE

/*! \todo FIXME - this decl was originally much later in the file, moved
here temporarily for debugging */
static emcmot_status_t emcmotStatus;

/*
  Implementation notes:

  Initing:  the emcmot interface needs to be inited once, but nml_traj_init()
  and nml_servo_init() can be called in any order. Similarly, the emcmot
  interface needs to be exited once, but nml_traj_exit() and nml_servo_exit()
  can be called in any order. They can also be called multiple times. Flags
  are used to signify if initing has been done, or if the final exit has
  been called.
  */

static emcmot_command_t emcmotCommand;

static int emcmotTrajInited = 0;	// non-zero means traj called init
static int emcmotAxisInited[EMCMOT_MAX_AXIS] = { 0 };	// non-zero means axis called init

__attribute__ ((unused))
static int emcmotIoInited = 0;	// non-zero means io called init
static int emcmotion_initialized = 0;	// non-zero means both
						// emcMotionInit called.

// saved value of velocity last sent out, so we don't send redundant requests
// used by emcTrajSetVelocity(), emcMotionAbort()
static double lastVel = -1.0;
static double last_ini_maxvel = -1.0;

// EMC_AXIS functions

// local status data, not provided by emcmot
static unsigned long localMotionHeartbeat = 0;
static int localMotionCommandType = 0;
static int localMotionEchoSerialNumber = 0;
static unsigned char localEmcAxisAxisType[EMCMOT_MAX_AXIS];
static double localEmcAxisUnits[EMCMOT_MAX_AXIS];
static double localEmcMaxAcceleration = DBL_MAX;

// axes are numbered 0..NUM-1

/*
  In emcmot, we need to set the cycle time for traj, and the interpolation
  rate, in any order, but both need to be done. 
 */

int emcAxisSetAxis(int axis, unsigned char axisType)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    localEmcAxisAxisType[axis] = axisType;

    return 0;
}

int emcAxisSetUnits(int axis, double units)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    localEmcAxisUnits[axis] = units;

    return 0;
}

/*! \todo Another #if 0 */
#if 0
int emcAxisSetGains(int axis, double p, double i, double d,
		    double ff0, double ff1, double ff2,
		    double bias, double maxError, double deadband)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    emcmotCommand.command = EMCMOT_SET_PID;
    emcmotCommand.axis = axis;

    emcmotCommand.pid.p = p;
    emcmotCommand.pid.i = i;
    emcmotCommand.pid.d = d;
    emcmotCommand.pid.ff0 = ff0;
    emcmotCommand.pid.ff1 = ff1;
    emcmotCommand.pid.ff2 = ff2;
    emcmotCommand.pid.bias = bias;
    emcmotCommand.pid.maxError = maxError;
    emcmotCommand.pid.deadband = deadband;

#ifdef ISNAN_TRAP
    if (isnan(emcmotCommand.pid.p) ||
	isnan(emcmotCommand.pid.i) ||
	isnan(emcmotCommand.pid.d) ||
	isnan(emcmotCommand.pid.ff0) ||
	isnan(emcmotCommand.pid.ff1) ||
	isnan(emcmotCommand.pid.ff2) ||
	isnan(emcmotCommand.pid.bias) ||
	isnan(emcmotCommand.pid.maxError) ||
	isnan(emcmotCommand.pid.deadband)) {
	printf("isnan error in emcAxisSetGains\n");
	return -1;
    }
#endif

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}
#endif

int emcAxisSetBacklash(int axis, double backlash)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    emcmotCommand.command = EMCMOT_SET_BACKLASH;
    emcmotCommand.axis = axis;

    emcmotCommand.backlash = backlash;

#ifdef ISNAN_TRAP
    if (isnan(emcmotCommand.backlash)) {
	printf("isnan error in emcAxisSetBacklash\n");
	return -1;
    }
#endif

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

/*! \todo Another #if 0 */
#if 0
int emcAxisSetCycleTime(int axis, double cycleTime)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    if (cycleTime <= 0.0) {
	return -1;
    }

    emcmotCommand.command = EMCMOT_SET_SERVO_CYCLE_TIME;
    emcmotCommand.cycleTime = cycleTime;

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

int emcAxisSetInputScale(int axis, double scale, double offset)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    emcmotCommand.command = EMCMOT_SET_INPUT_SCALE;
    emcmotCommand.axis = axis;
    emcmotCommand.scale = scale;
    emcmotCommand.offset = offset;

#ifdef ISNAN_TRAP
    if (isnan(emcmotCommand.scale) || isnan(emcmotCommand.offset)) {
	printf("isnan eror in emcAxisSetInputScale\n");
	return -1;
    }
#endif

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

int emcAxisSetOutputScale(int axis, double scale, double offset)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    emcmotCommand.command = EMCMOT_SET_OUTPUT_SCALE;
    emcmotCommand.axis = axis;
    emcmotCommand.scale = scale;
    emcmotCommand.offset = offset;

#ifdef ISNAN_TRAP
    if (isnan(emcmotCommand.scale) || isnan(emcmotCommand.offset)) {
	printf("isnan eror in emcAxisSetOutputScale\n");
	return -1;
    }
#endif

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

#endif				/* #if 0 */

// saved values of limits, since emcmot expects them to be set in
// pairs and we set them individually.
static double saveMinLimit[EMCMOT_MAX_AXIS];
static double saveMaxLimit[EMCMOT_MAX_AXIS];

int emcAxisSetMinPositionLimit(int axis, double limit)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    emcmotCommand.command = EMCMOT_SET_POSITION_LIMITS;
    emcmotCommand.axis = axis;
    emcmotCommand.maxLimit = saveMaxLimit[axis];
    emcmotCommand.minLimit = limit;
    saveMinLimit[axis] = limit;

#ifdef ISNAN_TRAP
    if (isnan(emcmotCommand.maxLimit) || isnan(emcmotCommand.minLimit)) {
	printf("isnan error in emcAxisSetMinPosition\n");
	return -1;
    }
#endif

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

int emcAxisSetMaxPositionLimit(int axis, double limit)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    emcmotCommand.command = EMCMOT_SET_POSITION_LIMITS;
    emcmotCommand.axis = axis;
    emcmotCommand.minLimit = saveMinLimit[axis];
    emcmotCommand.maxLimit = limit;
    saveMaxLimit[axis] = limit;

#ifdef ISNAN_TRAP
    if (isnan(emcmotCommand.maxLimit) || isnan(emcmotCommand.minLimit)) {
	printf("isnan error in emcAxisSetMaxPosition\n");
	return -1;
    }
#endif

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

/*! \todo Another #if 0 */
#if 0

// saved values of limits, since emcmot expects them to be set in
// pairs and we set them individually.
static double saveMinOutput[EMCMOT_MAX_AXIS];
static double saveMaxOutput[EMCMOT_MAX_AXIS];

int emcAxisSetMinOutputLimit(int axis, double limit)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    emcmotCommand.command = EMCMOT_SET_OUTPUT_LIMITS;
    emcmotCommand.axis = axis;
    emcmotCommand.maxLimit = saveMaxOutput[axis];
    emcmotCommand.minLimit = limit;
    saveMinOutput[axis] = limit;

#ifdef ISNAN_TRAP
    if (isnan(emcmotCommand.maxLimit) || isnan(emcmotCommand.minLimit)) {
	printf("isnan error in emcAxisSetMinOutputLimit\n");
	return -1;
    }
#endif

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

int emcAxisSetMaxOutputLimit(int axis, double limit)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    emcmotCommand.command = EMCMOT_SET_OUTPUT_LIMITS;
    emcmotCommand.axis = axis;
    emcmotCommand.minLimit = saveMinOutput[axis];
    emcmotCommand.maxLimit = limit;
    saveMaxOutput[axis] = limit;

#ifdef ISNAN_TRAP
    if (isnan(emcmotCommand.maxLimit) || isnan(emcmotCommand.minLimit)) {
	printf("isnan error in emcAxisSetMaxOutputLimit\n");
	return -1;
    }
#endif

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}
#endif

int emcAxisSetFerror(int axis, double ferror)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    emcmotCommand.command = EMCMOT_SET_MAX_FERROR;
    emcmotCommand.axis = axis;
    emcmotCommand.maxFerror = ferror;

#ifdef ISNAN_TRAP
    if (isnan(emcmotCommand.maxFerror)) {
	printf("isnan error in emcAxisSetFerror\n");
	return -1;
    }
#endif

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

int emcAxisSetMinFerror(int axis, double ferror)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }
    emcmotCommand.command = EMCMOT_SET_MIN_FERROR;
    emcmotCommand.axis = axis;
    emcmotCommand.minFerror = ferror;

#ifdef ISNAN_TRAP
    if (isnan(emcmotCommand.minFerror)) {
	printf("isnan error in emcAxisSetMinFerror\n");
	return -1;
    }
#endif

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

int emcAxisSetHomingParams(int axis, double home, double offset,
			   double search_vel, double latch_vel,
			   int use_index, int ignore_limits)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    emcmotCommand.command = EMCMOT_SET_HOMING_PARAMS;
    emcmotCommand.axis = axis;
    emcmotCommand.home = home;
    emcmotCommand.offset = offset;
    emcmotCommand.search_vel = search_vel;
    emcmotCommand.latch_vel = latch_vel;
    emcmotCommand.flags = 0;
    if (use_index) {
	emcmotCommand.flags |= HOME_USE_INDEX;
    }
    if (ignore_limits) {
	emcmotCommand.flags |= HOME_IGNORE_LIMITS;
    }
#ifdef ISNAN_TRAP
    if (isnan(emcmotCommand.home) ||
	isnan(emcmotCommand.offset) ||
	isnan(emcmotCommand.search_vel) ||
	isnan(emcmotCommand.latch_vel)) {
	printf("isnan error in emcAxisSetHoming\n");
	return -1;
    }
#endif

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

/*! \todo Another #if 0 */
#if 0
/*! \todo FIXME - to be deleted eventually, these are configured thru HAL */
int emcAxisSetStepParams(int axis, double setup_time, double hold_time)
{
    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }

    emcmotCommand.command = EMCMOT_SET_STEP_PARAMS;
    emcmotCommand.axis = axis;
    emcmotCommand.setup_time = setup_time;
    emcmotCommand.hold_time = hold_time;

    return usrmotWriteEmcmotCommand(&emcmotCommand);
}
#endif

int emcAxisSetMaxVelocity(int axis, double vel)
{
    if (axis < 0 || axis >= EMC_AXIS_MAX) {
	return 0;
    }

    if (vel < 0.0) {
	vel = 0.0;
    }

    AXIS_MAX_VELOCITY[axis] = vel;

    emcmotCommand.command = EMCMOT_SET_JOINT_VEL_LIMIT;
    emcmotCommand.axis = axis;
    emcmotCommand.vel = vel;
    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

int emcAxisSetMaxAcceleration(int axis, double acc)
{

    if (axis < 0 || axis >= EMC_AXIS_MAX) {
	return 0;
    }
    if (acc < 0.0) {
	acc = 0.0;
    }
    AXIS_MAX_ACCELERATION[axis] = acc;
    emcmotCommand.command = EMCMOT_SET_JOINT_ACC_LIMIT;
    emcmotCommand.axis = axis;
    emcmotCommand.acc = acc;
    return usrmotWriteEmcmotCommand(&emcmotCommand);
}

/* This function checks to see if any axis or the traj has
   been inited already.  At startup, if none have been inited,
   usrmotIniLoad and usrmotInit must be called first.  At
   shutdown, after all have been halted, the usrmotExit must
   be called.
*/

static int AxisOrTrajInited(void)
{
    int axis;

    for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
	if (emcmotAxisInited[axis]) {
	    return 1;
	}
    }
    if (emcmotTrajInited) {
	return 1;
    }
    return 0;
}

int emcAxisInit(int axis)
{
    int retval = 0;

    if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
	return 0;
    }
    // init emcmot interface
    if (!AxisOrTrajInited()) {
	usrmotIniLoad(EMC_INIFILE);
	if (0 != usrmotInit("emc2_task")) {
	    return -1;
	}
    }
    emcmotAxisInited[axis] = 1;
    if (0 != iniAxis(axis, EMC_INIFILE)) {
	retval = -1;
    }
    return retval;