/********************************************************************* 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, movedhere 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 initstatic int emcmotAxisInited[EMCMOT_MAX_AXIS] = { 0 };	// non-zero means axis called init__attribute__ ((unused))static int emcmotIoInited = 0;	// non-zero means io called initstatic 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 emcmotstatic 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 0int 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);}#endifint 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 0int 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);}#endifint 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);}#endifint 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;