/* FILENAME... omsMAXvEncFunc.cpp USAGE... Pro-Dex OMS MAXv encoder asyn motor support Version: $Revision$ Modified By: $Author$ Last Modified: $Date$ HeadURL: $URL$ */ /* * Created on: 10/2011 * Author: eden */ #include #include #include "omsMAXvEncFunc.h" static const char *driverName = "omsMAXvEncFuncDriver"; #ifdef __GNUG__ #ifdef DEBUG #define Debug(l, f, args...) {if (l <= motorMAXvEncFuncdebug) \ errlogPrintf(f, ## args);} #else #define Debug(l, f, args...) #endif #else #define Debug #endif volatile int motorMAXvEncFuncdebug = 0; extern "C" {epicsExportAddress(int, motorMAXvEncFuncdebug);} /* define additional Parameters to use special encoder functions and auxiliary encoders of the MAXv */ #define NUM_ADDITIONALPARAMS 18 #define motorEncoderFunctionString "ENCODER_FUNCTION" #define motorAuxEncoderPositionString "AUX_ENC_POSITION" #define motorEncoderRawPosString "RAW_ENC_POSITION" omsMAXvEncFunc::omsMAXvEncFunc(const char* portName, int numAxes, int cardNo, const char* initString, int priority, int stackSize) : omsMAXv(portName, numAxes, cardNo, initString, priority, stackSize, NUM_ADDITIONALPARAMS) { initialize(); } omsMAXvEncFunc::omsMAXvEncFunc(const char* portName, int numAxes, int cardNo, const char* initString, int priority, int stackSize, unsigned int vmeAddr, int vector, int intlevel, const char* addressType) : omsMAXv(portName, numAxes, cardNo, initString, priority, stackSize, vmeAddr, vector, intlevel, addressType, NUM_ADDITIONALPARAMS) { initialize(); } void omsMAXvEncFunc::initialize() { const char* functionName = "initialize"; Debug(5, "omsMAXvEncFunc::initialize: start initialize\n" ); int encIndex = numAxes; if (encIndex > MAXENCFUNC) encIndex = MAXENCFUNC; /* auxiliary encoders */ for (int i=0; i < encIndex; ++i){ if (createParam(i, motorEncoderFunctionString, asynParamInt32, &encFuncIndex[i]) != asynSuccess) errlogPrintf("%s:%s:%s: unable to create param motorEncoderFunctionString, index %d\n", driverName, functionName, portName, i); if (createParam(i, motorEncoderRawPosString, asynParamFloat64, &encRawPosIndex[i]) != asynSuccess) errlogPrintf("%s:%s:%s: unable to create param motorEncoderRawPosString, index %d\n", driverName, functionName, portName, i); } createParam(0, motorAuxEncoderPositionString, asynParamFloat64, &encPosIndex[0]); createParam(1, motorAuxEncoderPositionString, asynParamFloat64, &encPosIndex[1]); Debug(3, "omsMAXvEncFunc::initialize: auxiliary encoder 0 position index %d\n", encPosIndex[0] ); Debug(3, "omsMAXvEncFunc::initialize: auxiliary encoder 1 position index %d\n", encPosIndex[1] ); for (int i=0; i < encIndex; ++i) Debug(3, "omsMAXvEncFunc::initialize: encFuncIndex %d => %d\n", i, encFuncIndex[i] ); lock(); for (int i=0; i < encIndex; ++i){ setIntegerParam(i, encFuncIndex[i], 0); setDoubleParam(i, encRawPosIndex[i], 0.0); } setDoubleParam(0, encPosIndex[0], 0.0); setDoubleParam(1, encPosIndex[1], 0.0); for (int i=0; i < encIndex; ++i) { callParamCallbacks(i, i); } unlock(); } asynStatus omsMAXvEncFunc::writeInt32(asynUser *pasynUser, epicsInt32 value) { int function = pasynUser->reason; asynStatus status = asynSuccess; omsBaseAxis *pAxis = (omsBaseAxis *)this->getAxis(pasynUser); static const char *functionName = "writeInt32"; if ((pAxis->getAxis() < MAXENCFUNC) && (function == encFuncIndex[pAxis->getAxis()])) { Debug(5, "omsMAXvEncFunc::writeInt32: set average axis %d with axis: %d\n", pAxis->getAxis(), value ); if ((value >= 0) && (value < MAXENCFUNC)) averageChannel [pAxis->getAxis()] = value; asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, "%s:%s: function=%d, value=%d\n", driverName, functionName, function, value); } else { status=omsMAXv::writeInt32(pasynUser, value); } return status; } /** * overrides the base class to add encoder functions and auxiliary encoders * for now we assume the function is average and averageChannel contains the * number of the other encoder to compute the average. */ asynStatus omsMAXvEncFunc::getEncoderPositions(epicsInt32 encPosArr[OMS_MAX_AXES]) { // const char* functionName = "getEncoderPositions"; asynStatus status = asynSuccess; double position; omsBaseController::getEncoderPositions(encPosArr); for (int i=0; i < OMS_MAX_AXES; ++i) { if ((i < MAXENCFUNC) && (averageChannel[i] != i) && (averageChannel[i] > 0) && (averageChannel[i] < OMS_MAX_AXES)){ position = (double) encPosArr[i]; encPosArr[i] = (int) (((double)(encPosArr[averageChannel[i]]) + position)/2.0); lock(); setDoubleParam(i, encRawPosIndex[i], position); setDoubleParam(averageChannel[i], encRawPosIndex[averageChannel[i]], (double)encPosArr[averageChannel[i]]); unlock(); Debug(9, "omsMAXvEncFunc::getEncPos: axis %d other: %d, old: %g, new: %d, other: %d \n", i, averageChannel[i], position, encPosArr[i], encPosArr[averageChannel[i]] ); } } return status; } extern "C" int omsMAXvEncFuncConfig( int cardNo, /* card no, starts with 0*/ const char *portName, /* MAXv Motor Asyn Port name */ int numAxes, /* Number of axes this controller supports */ int movingPollPeriod, /* Time to poll (msec) when an axis is in motion */ int idlePollPeriod, /* Time to poll (msec) when an axis is idle. 0 for no polling */ const char *initString) /* Init String sent to card */ { omsMAXvEncFunc *pController = new omsMAXvEncFunc(portName, numAxes, cardNo, initString, 0, 0); pController->startPoller((double)movingPollPeriod, (double)idlePollPeriod, 10); return 0; } /* * extended MAXv configuration, which may be used instead of omsMAXvEncFuncConfig, * if more details need to be specified. * omsMAXvEncFuncConfig2 does not need and ignores omsMAXvSetup */ extern "C" int omsMAXvEncFuncConfig2( int slotNo, /* VME slot no of MAXv card*/ const char* addr_type, /* VME address type; "A16", "A24" or "A32" */ unsigned int addrs, /* Board Address */ unsigned int vector, /* Interrupt Vector: noninterrupting(0), (64-255) */ int int_level, /* interrupt level (1-6) */ const char *portName, /* MAXv Motor Asyn Port name */ int numAxes, /* Number of axes this controller supports */ int priority, /* priority of PollerTask (0 => epicsThreadPriorityMedium)*/ int stackSize, /* stackSize of PollerTask (0 => epicsThreadStackMedium) */ int movingPollPeriod, /* Time to poll (msec) when an axis is in motion */ int idlePollPeriod, /* Time to poll (msec) when an axis is idle. 0 for no polling */ const char *initString) /* Init String sent to card */ { omsMAXvEncFunc *pController = new omsMAXvEncFunc(portName, numAxes, slotNo, initString, priority, stackSize, addrs, vector, int_level, addr_type); pController->startPoller((double)movingPollPeriod, (double)idlePollPeriod, 10); return 0; } /* Code for iocsh registration */ /* omsMAXvEncFuncConfig */ static const iocshArg configArg0 = {"number of card", iocshArgInt}; static const iocshArg configArg1 = {"asyn motor port name", iocshArgString}; static const iocshArg configArg2 = {"number of axes", iocshArgInt}; static const iocshArg configArg3 = {"moving poll rate", iocshArgInt}; static const iocshArg configArg4 = {"idle poll rate", iocshArgInt}; static const iocshArg configArg5 = {"initstring", iocshArgString}; static const iocshArg * const configArgs[6] = {&configArg0, &configArg1, &configArg2, &configArg3, &configArg4, &configArg5 }; static const iocshFuncDef configMAXv = {"omsMAXvEncFuncConfig", 6, configArgs}; static void configMAXvCallFunc(const iocshArgBuf *args) { omsMAXvEncFuncConfig(args[0].ival, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].sval); } /* omsMAXvEncFuncConfig2 */ static const iocshArg config2Arg0 = {"Slot number", iocshArgInt}; static const iocshArg config2Arg1 = {"Address type: A16,A24,A32", iocshArgString}; static const iocshArg config2Arg2 = {"Board Address on 4K (0x1000) boundary", iocshArgInt}; static const iocshArg config2Arg3 = {"Interrupt Vector: noninterrupting(0), (64-255)", iocshArgInt}; static const iocshArg config2Arg4 = {"Interrupt level (1-6)", iocshArgInt}; static const iocshArg config2Arg5 = {"Asyn motor port name", iocshArgString}; static const iocshArg config2Arg6 = {"Number of axes", iocshArgInt}; static const iocshArg config2Arg7 = {"Task priority: 0 => medium", iocshArgInt}; static const iocshArg config2Arg8 = {"Stack size: 0 => medium", iocshArgInt}; static const iocshArg config2Arg9 = {"Moving poll rate", iocshArgInt}; static const iocshArg config2Arg10 = {"Idle poll rate", iocshArgInt}; static const iocshArg config2Arg11 = {"Initstring", iocshArgString}; static const iocshArg * const config2Args[12] = {&config2Arg0, &config2Arg1, &config2Arg2, &config2Arg3, &config2Arg4, &config2Arg5, &config2Arg6, &config2Arg7, &config2Arg8, &config2Arg9, &config2Arg10, &config2Arg11}; static const iocshFuncDef config2MAXv = {"omsMAXvEncFuncConfig2", 12, config2Args}; static void config2MAXvCallFunc(const iocshArgBuf *args) { omsMAXvEncFuncConfig2(args[0].ival, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].sval, args[6].ival, args[7].ival, args[8].ival, args[9].ival, args[10].ival, args[11].sval); } static void omsMAXvEncFuncAsynRegister(void) { iocshRegister(&configMAXv, configMAXvCallFunc); iocshRegister(&config2MAXv, config2MAXvCallFunc); } epicsExportRegistrar(omsMAXvEncFuncAsynRegister);