From 83af8f1d92aae6a057ceea3a3515fa3c2d67cfd6 Mon Sep 17 00:00:00 2001 From: Ron Sluiter Date: Thu, 11 Oct 2012 16:47:16 +0000 Subject: [PATCH] phase 3 asyn motor support for OMS MAXnet and MAXv from Jens Eden. --- motorApp/Makefile | 3 + motorApp/MotorSimSrc/drvMotorSim.c | 69 +- motorApp/OmsAsynSrc/Makefile | 26 + motorApp/OmsAsynSrc/README_MAXnet | 24 + motorApp/OmsAsynSrc/omsAsynSupport.dbd | 9 + motorApp/OmsAsynSrc/omsBaseAxis.cpp | 200 ++++ motorApp/OmsAsynSrc/omsBaseAxis.h | 55 ++ motorApp/OmsAsynSrc/omsBaseController.cpp | 1099 +++++++++++++++++++++ motorApp/OmsAsynSrc/omsBaseController.h | 105 ++ motorApp/OmsAsynSrc/omsMAXnet.cpp | 369 +++++++ motorApp/OmsAsynSrc/omsMAXnet.h | 42 + motorApp/OmsAsynSrc/omsMAXv.cpp | 649 ++++++++++++ motorApp/OmsAsynSrc/omsMAXv.h | 176 ++++ motorApp/OmsAsynSrc/omsMAXvEncFunc.cpp | 223 +++++ motorApp/OmsAsynSrc/omsMAXvEncFunc.h | 51 + 15 files changed, 3078 insertions(+), 22 deletions(-) create mode 100644 motorApp/OmsAsynSrc/Makefile create mode 100644 motorApp/OmsAsynSrc/README_MAXnet create mode 100644 motorApp/OmsAsynSrc/omsAsynSupport.dbd create mode 100644 motorApp/OmsAsynSrc/omsBaseAxis.cpp create mode 100644 motorApp/OmsAsynSrc/omsBaseAxis.h create mode 100644 motorApp/OmsAsynSrc/omsBaseController.cpp create mode 100644 motorApp/OmsAsynSrc/omsBaseController.h create mode 100644 motorApp/OmsAsynSrc/omsMAXnet.cpp create mode 100644 motorApp/OmsAsynSrc/omsMAXnet.h create mode 100644 motorApp/OmsAsynSrc/omsMAXv.cpp create mode 100644 motorApp/OmsAsynSrc/omsMAXv.h create mode 100644 motorApp/OmsAsynSrc/omsMAXvEncFunc.cpp create mode 100644 motorApp/OmsAsynSrc/omsMAXvEncFunc.h diff --git a/motorApp/Makefile b/motorApp/Makefile index 4c02f0aa..5e0f1303 100644 --- a/motorApp/Makefile +++ b/motorApp/Makefile @@ -21,6 +21,9 @@ SoftMotorSrc_DEPEND_DIRS = MotorSrc # All the following modules require ASYN. ifdef ASYN +DIRS += OmsAsynSrc +OmsAsynSrc_DEPEND_DIRS = MotorSrc + DIRS += MotorSimSrc MotorSimSrc_DEPEND_DIRS = MotorSrc diff --git a/motorApp/MotorSimSrc/drvMotorSim.c b/motorApp/MotorSimSrc/drvMotorSim.c index 8a80e71d..0535181d 100644 --- a/motorApp/MotorSimSrc/drvMotorSim.c +++ b/motorApp/MotorSimSrc/drvMotorSim.c @@ -21,6 +21,8 @@ Last Modified: $Date: 2009-06-18 19:38:20 $ * force a status update with a call to callCallback(). * - Matthew Pearson added deferred move support. * 2010-10-05 rls - MP's fix for deferred moves broken in drvMotorSim. + * 2012-10-09 rls - Added motorAxisforceCallback to support motor record + * GET_INFO commands. * */ @@ -63,7 +65,10 @@ motorAxisDrvSET_t motorSim = motorAxisHome, /**< Pointer to function to execute a more to reference or home */ motorAxisMove, /**< Pointer to function to execute a position move */ motorAxisVelocityMove, /**< Pointer to function to execute a velocity mode move */ - motorAxisStop /**< Pointer to function to stop motion */ + motorAxisStop, /**< Pointer to function to stop motion */ + motorAxisforceCallback, /**< Pointer to function to request a poller status update */ + motorAxisProfileMove, /**< Pointer to function to execute a profile move */ + motorAxisTriggerProfile /**< Pointer to function to trigger a profile move */ }; epicsExportAddress(drvet, motorSim); @@ -120,10 +125,13 @@ static motorSim_t drv={ NULL, NULL, motorSimLogMsg, NULL, { 0, 0 } }; #define MAX(a,b) ((a)>(b)? (a): (b)) #define MIN(a,b) ((a)<(b)? (a): (b)) +#define DELTA 0.1 /*Deferred moves functions.*/ static int processDeferredMoves(const motorSim_t * pDrv); +static void motorProcTask(motorSim_t *); + static void motorAxisReportAxis( AXIS_HDL pAxis, int level ) { printf( "Found driver for motorSim card %d, axis %d, mutex %p\n", pAxis->card, pAxis->axis, pAxis->axisMutex ); @@ -562,6 +570,18 @@ static int motorAxisStop( AXIS_HDL pAxis, double acceleration ) return MOTOR_AXIS_OK; } +static int motorAxisforceCallback(AXIS_HDL pAxis) +{ + if (pAxis == NULL) + return(MOTOR_AXIS_ERROR); + + /* Force a status update. */ + motorParam->forceCallback(pAxis->params); + motorProcTask(&drv); + return(MOTOR_AXIS_OK); +} + + /**\defgroup motorSimTask Routines to implement the motor axis simulation task @{ */ @@ -636,34 +656,39 @@ static void motorSimProcess( AXIS_HDL pAxis, double delta ) motorParam->setInteger( pAxis->params, motorAxisLowHardLimit, (pAxis->nextpoint.axis[0].p <= pAxis->lowHardLimit) ); } -#define DELTA 0.1 -static void motorSimTask( motorSim_t * pDrv ) + +static void motorProcTask( motorSim_t *pDrv) { epicsTimeStamp now; double delta; AXIS_HDL pAxis; + /* Get a new timestamp */ + epicsTimeGetCurrent( &now ); + delta = epicsTimeDiffInSeconds( &now, &(pDrv->now) ); + pDrv->now = now; + + if ( delta > (DELTA/4.0) && delta <= (4.0*DELTA) ) + { + /* A reasonable time has elapsed, it's not a time step in the clock */ + + for (pAxis = pDrv->pFirst; pAxis != NULL; pAxis = pAxis->pNext ) + { + if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) + { + motorSimProcess( pAxis, delta ); + motorParam->callCallback( pAxis->params ); + epicsMutexUnlock( pAxis->axisMutex ); + } + } + } +} + +static void motorSimTask( motorSim_t * pDrv ) +{ while ( 1 ) { - /* Get a new timestamp */ - epicsTimeGetCurrent( &now ); - delta = epicsTimeDiffInSeconds( &now, &(pDrv->now) ); - pDrv->now = now; - - if ( delta > (DELTA/4.0) && delta <= (4.0*DELTA) ) - { - /* A reasonable time has elapsed, it's not a time step in the clock */ - - for (pAxis = pDrv->pFirst; pAxis != NULL; pAxis = pAxis->pNext ) - { - if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) - { - motorSimProcess( pAxis, delta ); - motorParam->callCallback( pAxis->params ); - epicsMutexUnlock( pAxis->axisMutex ); - } - } - } + motorProcTask(pDrv); epicsThreadSleep( DELTA ); } } diff --git a/motorApp/OmsAsynSrc/Makefile b/motorApp/OmsAsynSrc/Makefile new file mode 100644 index 00000000..06541a51 --- /dev/null +++ b/motorApp/OmsAsynSrc/Makefile @@ -0,0 +1,26 @@ +# Makefile +TOP = ../.. +include $(TOP)/configure/CONFIG +#---------------------------------------- +# ADD MACRO DEFINITIONS AFTER THIS LINE + +# +#USR_CXXFLAGS += -DDEBUG + +DBD += omsAsynSupport.dbd + +LIBRARY_IOC_DEFAULT += omsAsyn + +SRCS += omsBaseAxis.cpp +SRCS += omsBaseController.cpp +SRCS += omsMAXnet.cpp +SRCS += omsMAXv.cpp +#SRCS += omsMAXvEncFunc.cpp + +oms_LIBS += motor +oms_LIBS += asyn +oms_LIBS += $(EPICS_BASE_IOC_LIBS) + +include $(TOP)/configure/RULES +#---------------------------------------- +# ADD RULES AFTER THIS LINE diff --git a/motorApp/OmsAsynSrc/README_MAXnet b/motorApp/OmsAsynSrc/README_MAXnet new file mode 100644 index 00000000..169e9f2d --- /dev/null +++ b/motorApp/OmsAsynSrc/README_MAXnet @@ -0,0 +1,24 @@ + +Please note: + +- The MAXnet IP-stack is very basic. You cannot ping the card and it doesn't know + anything about routing, which means you can only use it from hosts in the same + subnet. +- MAXnet cards with firmware versions older than 1.33.4 shouldn't be used + with the ethernet port. They abort communication, if they detect a tcp packet + with checksum 0xffff. +- MAXnet cards with firmware versions below 1.33 use "\n\r" as input EOS + on Ethernet ports. Version 1.33 and higher use "\n" on ethernet and serial ports. + Comment the setInputEos call and set input EOS in st.cmd if using firmware + versions below 1.33 + +add this to st.cmd if using the ethernet port: + drvAsynIPPortConfigure("MAXNET","maxnet-ip-address:23",0,0,0) + +add this to st.cmd if using the serial port: + drvAsynSerialPortConfigure("MAXNET","/your/serial/device",0,0,0) + asynSetOption("MAXNET",0,"baud","115200") + asynSetOption("MAXNET",0,"bits","8") + asynSetOption("MAXNET",0,"parity","none") + asynSetOption("MAXNET",0,"crtscts","Y") + diff --git a/motorApp/OmsAsynSrc/omsAsynSupport.dbd b/motorApp/OmsAsynSrc/omsAsynSupport.dbd new file mode 100644 index 00000000..d857d303 --- /dev/null +++ b/motorApp/OmsAsynSrc/omsAsynSupport.dbd @@ -0,0 +1,9 @@ +registrar(OmsMAXnetAsynRegister) +registrar(OmsMAXvAsynRegister) +#registrar(omsMAXvEncFuncAsynRegister) +#variable(motorMAXvdebug) +#variable(motorOMSBASEdebug) +#variable(motorMAXvEncFuncdebug) +#variable(motorMAXnetdebug) + + diff --git a/motorApp/OmsAsynSrc/omsBaseAxis.cpp b/motorApp/OmsAsynSrc/omsBaseAxis.cpp new file mode 100644 index 00000000..a1aa6433 --- /dev/null +++ b/motorApp/OmsAsynSrc/omsBaseAxis.cpp @@ -0,0 +1,200 @@ +/* +FILENAME... omsBaseAxis.cpp +USAGE... Pro-Dex OMS asyn motor base axes support + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + + +/* + * Created on: 6/2012 + * Author: eden + * + */ + +#include +#include +#include +#include + +#include "omsBaseAxis.h" +#include "omsBaseController.h" + +static const char *driverName = "omsBaseAxisDriver"; + +omsBaseAxis::omsBaseAxis(omsBaseController *pController, int axis, char axisChar) + : asynMotorAxis(pController, axis), axisChar(axisChar) +{ + pC_ = pController; + stepper = 1; + invertLimit = 0; +} + +asynStatus omsBaseAxis::move(double position, int relative, double min_velocity, double max_velocity, double acceleration) +{ +// omsBaseAxis *pAxis = this->getAxis(pasynUser); + static const char *functionName = "moveAxis"; + + asynStatus status = asynError; + epicsInt32 minvelo, velo, acc, rela, pos; + char *relabs[2] = {(char *) "MA", (char *) "MR"}; + char buff[100]; + + if (relative) + rela = 1; + else + rela = 0; + + pos = (epicsInt32) (position + 0.5); + if (abs(pos) > 67000000){ + asynPrint(pasynUser_, ASYN_TRACE_ERROR, + "%s:%s:%s axis %d position out of range %f\n", + driverName, functionName, pC_->portName, axisNo_, position); + return status; + } + + velo = (epicsInt32) (max_velocity + 0.5); + if (velo < 1) velo = 1; + else if (velo > 4000000) velo = 4000000; + + minvelo = (epicsInt32) (min_velocity + 0.5); + if (minvelo < 0) minvelo = 0; + else if (minvelo >= velo) minvelo = velo - 1; + + acc = abs((epicsInt32) acceleration); + if (acc > 8000000) + acc = 8000000; + else if (acc < 1) + acc = 1; + + /* move to the specified position */ + sprintf(buff, "A%1c AC%d; VB%d; VL%d; %s%d; GO ID", axisChar, acc, minvelo, velo, relabs[rela], pos); + status = pC_->sendOnlyLock(buff); + + asynPrint(pasynUser_, ASYN_TRACE_FLOW, + "%s:%s: Set driver %s, axis %d move to %f, min vel=%f, max_vel=%f, accel=%f", + driverName, functionName, pC_->portName, axisNo_, position, min_velocity, max_velocity, acceleration ); + + return status; +} + +asynStatus omsBaseAxis::home(double min_velocity, double max_velocity, double acceleration, int forwards ) +{ + static const char *functionName = "homeAxis"; + + asynStatus status = asynError; + char buff[60]; + char *direction[2] = {(char*) "HR", (char*) "HM"}; + epicsInt32 velo, acc, fw = 0; + + if (forwards) fw = 1; + + velo = (epicsInt32) max_velocity; + if (velo < 1) velo = 1; + else if (velo > 1000000) velo = 1000000; + + acc = abs((epicsInt32) acceleration); + if (acc > 8000000) + acc = 8000000; + else if (acc < 1) + acc = 1; + + /* do a home run and move to the home position */ + sprintf(buff, "A%1c AC%d; VL%d; %s; MA0 GO ID", axisChar, acc, velo, direction[forwards]); + status = pC_->sendOnlyLock(buff); + + homing = 1; + + asynPrint(pasynUser_, ASYN_TRACE_FLOW, + "%s:%s: Set driver %s, axis %d to home %s, min vel=%f, max_vel=%f, accel=%f", + driverName, functionName, pC_->portName, axisNo_, (forwards?"FORWARDS":"REVERSE"), min_velocity, max_velocity, acceleration ); + + return status; +} + +asynStatus omsBaseAxis::doMoveToHome() +{ + static const char *functionName = "doMoveToHome"; + asynPrint(pasynUser_, ASYN_TRACE_ERROR, + "%s:%s: This function is not yet implemented for axis %d\n", + driverName, functionName, axisNo_); + return asynError; +} + +asynStatus omsBaseAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration ) +{ + asynStatus status = asynError; + static const char *functionName = "moveVelocityAxis"; + + char buff[100]; + epicsInt32 velo, acc; + + acc = (epicsInt32) acceleration; + if (acc < 1) acc = 1; + else if (acc > 8000000) acc = 8000000; + + velo = (epicsInt32) maxVelocity; + if (velo > 4000000) velo = 4000000; + else if (velo < -4000000) velo = -4000000; + + sprintf(buff, "A%1c AC%d; JG%d;", axisChar, acc, velo); + status = pC_->sendOnlyLock(buff); + + asynPrint(pasynUser_, ASYN_TRACE_FLOW, + "%s:%s: Set port %s, axis %d move with velocity of %f, acceleration=%f", + driverName, functionName, pC_->portName, axisNo_, maxVelocity, acceleration ); + + /* Send a signal to the poller task which will make it do a poll, and switch to the moving poll rate */ + return status; +} + +asynStatus omsBaseAxis::stop(double acceleration ) +{ + asynStatus status = asynError; + static const char *functionName = "stopAxis"; + int acc; + char buff[50]; + + acc = (int)(fabs(acceleration)+0.5); + if (acc > 8000000) acc=8000000; + if (acc < 1) acc = 200000; + + sprintf(buff, "A%1c AC%d; ST ID;", axisChar, acc); + status = pC_->sendOnlyLock(buff); + + asynPrint(pasynUser_, ASYN_TRACE_FLOW, + "%s:%s: port %s, set axis %d to stop with accel=%f\n", + driverName, functionName, pC_->portName, axisNo_, axisNo_, acceleration ); + + return status; +} + +/** Set the current position of the motor. + * \param[in] position The new absolute motor position that should be set in the hardware. Units=steps. + */ +asynStatus omsBaseAxis::setPosition(double position) +{ + static const char *functionName = "setPosition"; + asynStatus status = asynError; + char buff[20]; + asynPrint(pasynUser_, ASYN_TRACE_FLOW, + "%s:%s:%s axis %d set position to %f\n", + driverName, functionName, pC_->portName, axisNo_, position); + sprintf(buff,"A%1c LP%d;", axisChar, (int)(position)); + status = pC_->sendOnlyLock(buff); + + return status; +} + +/** we need to implement this, because we need to use the motorUpdateStatus_ function + * in asynMotorController, because we cannot access statusChanged_ (shouldn't be private) + * ignore moving flag, since we have our own poller. + */ +asynStatus omsBaseAxis::poll(bool *moving) +{ + epicsEventSignal(pC_->pollEventId_); + return asynSuccess; +} diff --git a/motorApp/OmsAsynSrc/omsBaseAxis.h b/motorApp/OmsAsynSrc/omsBaseAxis.h new file mode 100644 index 00000000..c8936abc --- /dev/null +++ b/motorApp/OmsAsynSrc/omsBaseAxis.h @@ -0,0 +1,55 @@ +/* +FILENAME... omsBaseAxis.h +USAGE... Pro-Dex OMS asyn motor base axes support + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + +/* + * Created on: 06/2012 + * Author: eden + */ + +#ifndef OMSBASEAXIS_H_ +#define OMSBASEAXIS_H_ + +#include "asynMotorController.h" +#include "asynMotorAxis.h" + +class omsBaseController; + +class omsBaseAxis : public asynMotorAxis +{ +public: + omsBaseAxis(class omsBaseController *, int, char ); + virtual asynStatus move(double position, int relative, double minVelocity, double maxVelocity, double acceleration); + virtual asynStatus moveVelocity(double minVelocity, double maxVelocity, double acceleration); + virtual asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards); + virtual asynStatus stop(double acceleration); + virtual asynStatus doMoveToHome(); + virtual asynStatus setPosition(double position); + virtual asynStatus poll(bool *moving); + + int getAxis(){return axisNo_;}; + int isStepper(){return stepper;}; + void setStepper(int val){stepper=val;}; + int getLimitInvert(){return invertLimit;}; + void setLimitInvert(int val){invertLimit=val;}; + int card; + int moveDelay; + char axisChar; + bool homing; + + +private: + omsBaseController *pC_; + int stepper; + int invertLimit; + +friend class omsBaseController; +}; + +#endif /* OMSBASEAXIS_H_ */ diff --git a/motorApp/OmsAsynSrc/omsBaseController.cpp b/motorApp/OmsAsynSrc/omsBaseController.cpp new file mode 100644 index 00000000..b19f8aa9 --- /dev/null +++ b/motorApp/OmsAsynSrc/omsBaseController.cpp @@ -0,0 +1,1099 @@ +/* +FILENAME... omsBaseController.cpp +USAGE... Pro-Dex OMS asyn motor base controller support + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + +/* + * Created on: 06/2012 + * Author: eden + * + */ + +#include +#include +#include +#include +#include + +#include "omsBaseController.h" + +#define MIN(a,b) ((a)<(b)? (a): (b)) + +#define motorOmsStringSendString "OMS_STRING_SEND" +#define motorOmsStringSendRecvString "OMS_STRING_SENDRECV" +#define motorOmsStringRecvString "OMS_STRING_RECV" +#define motorOmsPollString "OMS_POLL" +#define MOTOR_OMS_PARAMS_COUNT 4 + +ELLLIST omsBaseController::omsControllerList; +int omsBaseController::omsTotalControllerNumber = 0; + +static const char *driverName = "omsBaseDriver"; + +#ifdef __GNUG__ + #ifdef DEBUG + #define Debug(l, f, args...) {if (l <= motorOMSBASEdebug) \ + errlogPrintf(f, ## args);} + #else + #define Debug(l, f, args...) + #endif +#else + #define Debug +#endif +volatile int motorOMSBASEdebug = 0; +extern "C" {epicsExportAddress(int, motorOMSBASEdebug);} + + + +omsBaseController::omsBaseController(const char *portName, int maxAxes, int prio, int stackSz, int extMotorParams=0) + : asynMotorController(portName, maxAxes, extMotorParams + MOTOR_OMS_PARAMS_COUNT, + asynInt32Mask | asynFloat64Mask | asynOctetMask, + asynInt32Mask | asynFloat64Mask | asynOctetMask, + ASYN_CANBLOCK | ASYN_MULTIDEVICE, + 1, // autoconnect + prio, stackSz) +{ + + if (omsTotalControllerNumber == 0) { + ellInit(&omsControllerList); + } + + // check if portName is in use */ + if (findController(portName) != NULL) + errlogPrintf("omsBaseController: ERROR: asynPort %s already in use\n", portName); + + omsBaseNode* pNode = new omsBaseNode; + pNode->portName = epicsStrDup(portName); + pNode->pController = this; + ellAdd(&omsControllerList, (ELLNODE *)pNode); + + this->portName = epicsStrDup(portName); + controllerNumber = omsTotalControllerNumber; + ++omsTotalControllerNumber; + + sanityCounter = 0; + useWatchdog = false; + enabled = true; + numAxes = maxAxes; + controllerType = NULL; + baseMutex = new epicsMutex; + + if (prio == 0) + priority = epicsThreadPriorityLow; + else + priority = prio; + + if (stackSz == 0) + stackSize = epicsThreadGetStackSize(epicsThreadStackMedium); + else + stackSize = stackSz; + + createParam(0, motorOmsPollString, asynParamInt32, &pollIndex); + createParam(0, motorOmsStringSendString, asynParamOctet, &sendIndex); + createParam(0, motorOmsStringSendRecvString, asynParamOctet, &sendReceiveIndex); + createParam(0, motorOmsStringRecvString, asynParamOctet, &receiveIndex); + setStringParam(0, sendIndex, (char *) ""); + setStringParam(0, sendReceiveIndex, (char *) ""); + setStringParam(0, receiveIndex, (char *) ""); + + /* Set an EPICS exit handler */ + epicsAtExit(omsBaseController::callShutdown, this); + +} + +asynStatus omsBaseController::startPoller(double movingPollPeriod, double idlePollPeriod, int forcedFastPolls) +{ + char threadName[20]; + + movingPollPeriod_ = movingPollPeriod/1000.; + idlePollPeriod_ = idlePollPeriod/1000.; + forcedFastPolls_ = forcedFastPolls; + + epicsSnprintf(threadName, sizeof(threadName), "OMSPoller-%d", controllerNumber); + this->motorThread = epicsThreadCreate(threadName, + priority, stackSize, + (EPICSTHREADFUNC) &omsBaseController::callPoller, (void *) this); + return asynSuccess; +} + + +void omsBaseController::shutdown(){ + lock(); + shuttingDown_ = 1; + unlock(); +} + +void omsBaseController::report(FILE *fp, int level) +{ + int axis; + double velocity, position, encoderPosition; + int haveEncoder=0; + + fprintf(fp, "Oms %s motor driver %s, numAxes=%d\n", + controllerType, portName, numAxes); + + for (axis=0; axis < numAxes; axis++) { + omsBaseAxis *pAxis = pAxes[axis]; + fprintf(fp, " axis %d\n", pAxis->axisNo_); + + if (level > 0) + { + lock(); + getDoubleParam(pAxis->axisNo_, motorVelocity_, &velocity); + getDoubleParam(pAxis->axisNo_, motorPosition_, &position); + getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder); + if (haveEncoder) getDoubleParam(pAxis->axisNo_, motorEncoderPosition_, &encoderPosition); + unlock(); + fprintf(fp, " Current position = %f, velocity = %f\n", + position, + velocity); + + if (haveEncoder) fprintf(fp, " Encoder position %f\n", encoderPosition ); + if (pAxis->homing) fprintf(fp, " Currently homing axis\n" ); + } + } + // Call the base class method + asynMotorController::report(fp, level); +} + +omsBaseAxis * omsBaseController::getAxis(asynUser *pasynUser) +{ + int axisNo; + + getAddress(pasynUser, &axisNo); + return pAxes[axisNo]; +} + +/** Returns a pointer to an omsBaseAxis object. + * Returns NULL if the axis number is invalid. + * \param[in] axisNo Axis index number. */ +omsBaseAxis* omsBaseController::getAxis(int axisNo) +{ + if ((axisNo < 0) || (axisNo >= numAxes)) return NULL; + return pAxes[axisNo]; +} + +asynStatus omsBaseController::writeOctet(asynUser *pasynUser, const char *value, + size_t nChars, size_t *nActual) +{ + int function = pasynUser->reason; + asynStatus status = asynSuccess; + const char *functionName = "writeOctet"; + char inBuffer[MAX_STRING_SIZE]; + if (strlen(value) > nChars) return asynError; + + omsBaseAxis *pAxis = getAxis(pasynUser); + if (!pAxis) return asynError; + + if (function == sendReceiveIndex) + { + status = sendReceiveLock(value, inBuffer, sizeof(inBuffer)); + if (status == asynSuccess){ + /* Set the parameter in the parameter library. */ + status = (asynStatus)setStringParam(pAxis->axisNo_, receiveIndex, (char *)inBuffer); + status = (asynStatus)callParamCallbacks(pAxis->axisNo_); + asynPrint(pasynUser, ASYN_TRACE_ERROR, "%s:%s:%s: answer is %s\n", + driverName, functionName, portName, inBuffer); + *nActual = nChars; + } + else { + *nActual = 0; + asynPrint(pasynUser, ASYN_TRACE_ERROR, "%s:%s:%s: sendReceive Error\n", + driverName, functionName, portName); + + } + } + else if (function == sendIndex) + { + status = sendOnlyLock(value); + if (status == asynSuccess){ + *nActual = nChars; + } + else { + *nActual = 0; + asynPrint(pasynUser, ASYN_TRACE_ERROR, "%s:%s:%s: send Error\n", + driverName, functionName, portName); + } + } + return status; +} + +asynStatus omsBaseController::readInt32(asynUser *pasynUser, epicsInt32 *value) +{ + int function = pasynUser->reason; + asynStatus status = asynSuccess; + omsBaseAxis *pAxis = getAxis(pasynUser); + static const char *functionName = "readInt32"; + static char outputBuffer[8]; + + if (!pAxis) return asynError; + + if (function == motorPosition_) { + strcpy(outputBuffer,"A? RP"); + sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer)); + *value = strtol(inputBuffer, NULL, 10); + } else if (function == motorEncoderPosition_) { + int haveEncoder; + getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder); + if (haveEncoder){ + strcpy(outputBuffer,"A? RE"); + sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer)); + *value = strtol(inputBuffer, NULL, 10); + } + } else { + // Call base class + status = asynMotorController::readInt32(pasynUser, value); + } + + + return status; +} + +asynStatus omsBaseController::writeInt32(asynUser *pasynUser, epicsInt32 value) +{ + int function = pasynUser->reason; + asynStatus status = asynSuccess; + static const char *functionName = "writeInt32"; + omsBaseAxis *pAxis = getAxis(pasynUser); + + if (!pAxis) return asynError; + + status = pAxis->setIntegerParam(function, value); + + if (function == motorDeferMoves_) + { + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "%s:%s:%s Deferred Move: not yet implemented %s\n", + driverName, functionName, portName); + } + else if (function == motorClosedLoop_) + { + if (value) { + asynPrint(pasynUser, ASYN_TRACE_FLOW, "%s:%s:%s axis %d closed loop enable\n", + driverName, functionName, portName, pAxis->axisNo_); + if (firmwareMin(1,30,0)) + status = sendReplace(pAxis, (char*) "A? CL1"); + else + status = sendReplace(pAxis, (char*) "A? HN"); + } else { + asynPrint(pasynUser, ASYN_TRACE_FLOW, "%s:%s:%s SetInteger axis %d closed loop disable\n", + driverName, functionName, portName, pAxis->axisNo_); + if (firmwareMin(1,30,0)) + status = sendReplace(pAxis, (char*) "A? CL0"); + else + status = sendReplace(pAxis, (char*) "A? HF"); + } + } + else if (function == motorMoveToHome_) { + /* avoid asynMotorController::writeInt32 to handle this*/ + } + else if (function == pollIndex) + { + if (value) { + wakeupPoller(); + } + } + else { + return asynMotorController::writeInt32(pasynUser, value); + } + + pAxis->callParamCallbacks(); + + if (status) + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "%s:%s: error, status=%d function=%d, value=%d\n", + driverName, functionName, status, function, value); + else + asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, + "%s:%s: function=%d, value=%d\n", + driverName, functionName, function, value); + return status; +} + +asynStatus omsBaseController::writeFloat64(asynUser *pasynUser, epicsFloat64 value) +{ + int function = pasynUser->reason; + asynStatus status = asynSuccess; + static const char *functionName = "writeFloat64"; + omsBaseAxis *pAxis = getAxis(pasynUser); + + if (!pAxis) return asynError; + + + /* Set the parameter and readback in the parameter library. This may be overwritten when we read back the + * status at the end, but that's OK */ + status = pAxis->setDoubleParam(function, value); + + if (function == motorEncoderPosition_){ + int haveEncoder; + getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder); + if (haveEncoder){ + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s:%s axis %d set encoder position to %f\n", + driverName, functionName, portName, pAxis->axisNo_, value); + sprintf(inputBuffer,"A? LPE%d;",(int)(value)); + status = sendReplace(pAxis, inputBuffer); + } + } + else if (function == motorEncoderRatio_) { + int haveEncoder; + getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder); + if (haveEncoder){ + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s:%s axis %d set encoder ratio to %f\n", + driverName, functionName, portName, pAxis->axisNo_, value); + sprintf(inputBuffer,"A? ER%d,100000;",(int)(value*100000)); + status = sendReplace(pAxis, inputBuffer); + } + } + else if ((function == motorResolution_) || (function == motorLowLimit_) || (function == motorHighLimit_)) { + // we do nothing + status = asynSuccess; + } + else if (function == motorPGain_) { + if ((0.00 >= value) && (value < 32768.00)){ + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s:%s axis %d set proportional gain to %f\n", + driverName, functionName, portName, pAxis->axisNo_, value); + sprintf(inputBuffer,"A? KP%f;", value); + status = sendReplace(pAxis, inputBuffer); + } + else { + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "%s:%s:%s axis %d proportional gain %f not in range 0.0 -> 32768.0\n", + driverName, functionName, portName, pAxis->axisNo_, value); + } + } + else if (function == motorIGain_) { + if ((0.00 >= value) && (value < 32768.00)){ + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s:%s axis %d set integral gain to %f\n", + driverName, functionName, portName, pAxis->axisNo_, value); + sprintf(inputBuffer,"A? KI%f;", value); + status = sendReplace(pAxis, inputBuffer); + } + else { + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "%s:%s:%s axis %d integral gain %f not in range 0.0 -> 32768.0\n", + driverName, functionName, portName, pAxis->axisNo_, value); + } + } + else if (function == motorDGain_) + { + if ((0.00 >= value) && (value < 32768.00)){ + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s:%s axis %d set derivative gain to %f\n", + driverName, functionName, portName, pAxis->axisNo_, value); + sprintf(inputBuffer,"A? KD%f;", value); + status = sendReplace(pAxis, inputBuffer); + } + else { + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "%s:%s:%s axis %d derivative gain %f not in range 0.0 -> 32768.0\n", + driverName, functionName, portName, pAxis->axisNo_, value); + } + } + else { + /* Call base classes method (if we have our parameters check this here) */ + status = asynMotorController::writeFloat64(pasynUser, value); + } + + pAxis->callParamCallbacks(); + if (status) + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "%s:%s: error, status=%d function=%d, value=%f\n", + driverName, functionName, status, function, value); + else + asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, + "%s:%s: function=%d, value=%f\n", + driverName, functionName, function, value); + return status; +} + + + +asynStatus omsBaseController::getFirmwareVersion() +{ + /* get FirmwareVersion, try 3 times */ + int count = 0; + asynStatus status = asynError; + char *p; + + while (status != asynSuccess && count < 3){ + epicsThreadSleep(1.0); + status = sendReceiveLock((char*) "WY", inputBuffer, sizeof(inputBuffer)); + count++; + errlogPrintf("OMS Firmware Version: %s\n", inputBuffer); + } + // + if ((p = strstr(inputBuffer, "ver:"))){ + count = sscanf(p, "ver:%d.%d.%d,", &fwMajor, &fwMinor, &fwRevision); + } + if ((p == NULL) || (count < 2)) { + errlogPrintf("omsBaseController::getFirmwareVersion: unable to retrieve Firmware version\n"); + status = asynError; + } + + return status; +} + +asynStatus omsBaseController::Init(const char* initString, int multiple){ + + char *p, *tokSave; + int totalAxes; + char axisChrArr[OMS_MAX_AXES] = {'X','Y','Z','T','U','V','R','S'}; + char outputBuffer[10]; + + /* Interrupt clear */ + sendOnlyLock("IC;"); + + /* Stop all axes */ + sendOnlyLock("AM SA;"); + + /* send InitString */ + if ((initString != NULL) && (strlen(initString) > 0)) { + if (multiple){ + char* inittmp = epicsStrDup(initString); + for (tokSave = NULL, p = epicsStrtok_r(inittmp, ";", &tokSave); + p != NULL; p = epicsStrtok_r(NULL, ";", &tokSave)) { + sendOnlyLock(p); + } + free(inittmp); + } + else { + sendOnlyLock(initString); + } + } + + /* get Positions of all axes */ + inputBuffer[0] = '\0'; + sendReceiveLock((char*) "AA RP;", inputBuffer, sizeof(inputBuffer)); + + if (numAxes > OMS_MAX_AXES) { + errlogPrintf("omsBaseController:Init: number of axes=%d exceeds allowed maximum\n", numAxes); + return asynError; + } + + /* The return string will tell us how many axes this controller has */ + for (totalAxes = 0, tokSave = NULL, p = epicsStrtok_r(inputBuffer, ",", &tokSave); + p != 0; p = epicsStrtok_r(NULL, ",", &tokSave), totalAxes++); + if ((totalAxes > numAxes) || (totalAxes > OMS_MAX_AXES)) { + errlogPrintf("omsBaseController:Init: actual number of axes=%d > MIN(numAxes=%d, OMS_MAX_AXES)\n", totalAxes, numAxes); + return asynError; + } + numAxes = totalAxes; + + pAxes = new omsBaseAxis*[numAxes]; + lock(); + for (int axis=0; axis < numAxes; axis++) { + + omsBaseAxis* pAxis = new omsBaseAxis(this, axis, axisChrArr[axis]); + pAxes[axis] = pAxis; + + pAxis->setIntegerParam(motorStatusDone_, 1); + pAxis->setIntegerParam(motorStatusMoving_, 0); + pAxis->setIntegerParam(motorStatusHomed_, 0); + pAxis->setIntegerParam(motorStatusHome_, 0); + pAxis->setIntegerParam(motorStatusAtHome_, 0); + pAxis->setIntegerParam(motorStatusProblem_, 0); + pAxis->setIntegerParam(motorStatusCommsError_, 0); + + /* Determine if encoder is present and if mode is stepper or servo. */ + if (firmwareMin(1,1,30)) + strcpy(outputBuffer,"A? PS?"); + else + strcpy(outputBuffer,"A? ?PS"); + sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer)); + /*we expect any of "=O","=E","=M" */ + if (inputBuffer[1] == 'O'){ + pAxis->setStepper(1); + pAxis->setIntegerParam(motorStatusHasEncoder_, 0); + pAxis->setDoubleParam(motorEncoderPosition_, 0.0); // reset if not available + } + else if (inputBuffer[1] == 'M'){ + pAxis->setStepper(0); + pAxis->setIntegerParam(motorStatusHasEncoder_, 1); + } + else if (inputBuffer[1] == 'E'){ + pAxis->setStepper(1); + pAxis->setIntegerParam(motorStatusHasEncoder_, 1); + } + else + errlogPrintf("omsBaseController:Init: error: unknown axis type! (%s)\n", inputBuffer); + + /* Determine limit true state high or low */ + /* CAUTION you need firmware version 1.30 or higher to do this */ + if (firmwareMin(1,1,30)) + strcpy(outputBuffer,"A? LT?"); + else + strcpy(outputBuffer,"A? ?LS"); + sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer)); + /*we expect any of "=l" or "=h" */ + if (inputBuffer[1] == 'l'){ + pAxis->setLimitInvert(1); + } + else if (inputBuffer[1] == 'h'){ + pAxis->setLimitInvert(0); + } + else + errlogPrintf("omsBaseController:Init: error: unknown limit true state!\n"); + + } + unlock(); + return asynSuccess; +} + +void omsBaseController::callPoller(void *drvPvt) +{ + omsBaseController *pController = (omsBaseController*)drvPvt; + pController->omsPoller(); +} + + +#define DELTA 0.1 +void omsBaseController::omsPoller() +{ + + static const char *functionName = "BasePoller"; + +#define STATUSSTRINGLEN 5 + /* This is the task that polls the MAX card */ + double timeout, timeToWait; + double movingPollPeriod; + double idlePollPeriod; + omsBaseAxis* pAxis; + int retry_count, loopBreakCount=0, haveEncoder; + // asynStatus status; + int axisMoving, anyMoving=0; + int forcedFastPolls, fastPolls=0; + epicsInt32 axisPosArr[OMS_MAX_AXES], encPosArr[OMS_MAX_AXES], veloArr[OMS_MAX_AXES]; + char statusBuffer[OMS_MAX_AXES*STATUSSTRINGLEN+2]; + char encStatusBuffer[OMS_MAX_AXES*6+2]; + int closedLoopStatus[OMS_MAX_AXES]; + char encBuffer[7]; + unsigned int limitFlags; + epicsTimeStamp now, loopStart; + bool haveCLStatus, haveVeloArray, haveEncStatus, haveLimits, useEncoder=false, moveDone; + + lock(); + movingPollPeriod = movingPollPeriod_; + idlePollPeriod = idlePollPeriod_; + forcedFastPolls = forcedFastPolls_; + for (int i=0; (i < numAxes); i++) { + getIntegerParam(i, motorStatusHasEncoder_, &haveEncoder); + if (haveEncoder) useEncoder = true; + } + unlock(); + + + while(1) { + + lock(); + if (shuttingDown_) { + unlock(); + break; + } + unlock(); + + if (loopBreakCount > 10){ + errlogPrintf("%s:%s:%s: Error: %d consecutive unsuccessful attempts to read from motor card\n" + , driverName, functionName, this->portName, loopBreakCount); + loopBreakCount = 0; + } + + epicsTimeGetCurrent(&loopStart); + + retry_count = 0; + while ((getAxesPositions(axisPosArr) != asynSuccess) && (retry_count < 5)){ + epicsThreadSleepQuantum(); + ++retry_count; + } + if (retry_count > 4){ + errlogPrintf("%s:%s:%s: error reading axis position after %d attempts\n", + driverName, functionName, this->portName, retry_count); + ++loopBreakCount; + continue; + } + + if (useEncoder && (getEncoderPositions(encPosArr) != asynSuccess)){ + Debug(1, "%s:%s:%s: error executing get Encoder Positions\n", driverName, functionName, this->portName); + ++loopBreakCount; + continue; + } + + /* read all axis status values and reset done-field + * MDNN,MDNN,PNLN,PNNN,PNLN,PNNN,PNNN,PNNN*/ + if (getAxesStatus(statusBuffer, sizeof(statusBuffer), &moveDone) != asynSuccess){ + Debug(1, "%s:%s:%s: error reading axes status\n", driverName, functionName, this->portName); + ++loopBreakCount; + continue; + } + loopBreakCount = 0; +/* + if (sanityCheck() != asynSuccess){ + errlogPrintf("%s:%s:%s: error during sanity check\n", driverName, functionName, this->portName); + } +*/ + + if (anyMoving) + haveCLStatus = false; + else + haveCLStatus = true; + if (haveCLStatus && getClosedLoopStatus(closedLoopStatus) != asynSuccess){ + haveCLStatus = false; + Debug(1, "%s:%s:%s: error executing get Closed Loop Status\n", driverName, functionName, this->portName); + } + + haveVeloArray = true; + if (getAxesArray((char*) "AM;RV;", veloArr) != asynSuccess){ + haveVeloArray = false; + Debug(1,"%s:%s:%s: Error executing command Report Velocity (RV)\n", driverName, functionName, this->portName); + } + haveEncStatus = true; + if (sendReceiveLock((char*) "AM;EA;", encStatusBuffer, sizeof(encStatusBuffer)) != asynSuccess){ + haveEncStatus = false; + Debug(1,"%s:%s:%s: Error reading encoder status buffer >%s<\n", driverName, functionName, this->portName, encStatusBuffer); + } + + /* read all limits */ + if (moveDone || (anyMoving == 0)) + haveLimits = true; + else + haveLimits = false; + limitFlags =0; + if (haveLimits){ + if ((sendReceiveLock((char*) "AM;QL;", pollInputBuffer, sizeof(pollInputBuffer)) == asynSuccess)){ + if (1 != sscanf(pollInputBuffer, "%x", &limitFlags)){ + Debug(1,"%s:%s:%s: error converting limits: %s\n", driverName, functionName, this->portName, pollInputBuffer); + haveLimits = false; + } + } + else { + haveLimits = false; + Debug(1,"%s:%s:%s: error reading limits %s\n", driverName, functionName, this->portName, pollInputBuffer); + } + } + if (enabled) watchdogOK(); + + anyMoving = 0; + lock(); + for (int i=0; (i < numAxes) && enabled && (shuttingDown_ == 0); i++) { + pAxis = pAxes[i]; + + if (useEncoder){ + /* if this axis has an encoder, read encoder status */ + getIntegerParam(i, motorStatusHasEncoder_, &haveEncoder); + if (haveEncoder){ + if (haveEncStatus){ + if (getSubstring(i, encStatusBuffer, encBuffer, sizeof(encBuffer)) == asynSuccess){ + if (encBuffer[2] == 'S') + pAxis->setIntegerParam(motorStatusFollowingError_, 1); + else + pAxis->setIntegerParam(motorStatusFollowingError_, 0); + } + else{ + errlogPrintf("%s:%s:%s: error parsing encoder status string %s\n", + driverName, functionName, this->portName, encStatusBuffer); + } + } + pAxis->setDoubleParam(motorEncoderPosition_, (double) encPosArr[i]); + } + } + + /* check the done flag or current velocity */ + if (statusBuffer[i*STATUSSTRINGLEN + 1] == 'D'){ + Debug(3, "%s:%s:%s: found Done Flag axis %d\n", driverName, functionName, portName, i); + pAxis->setIntegerParam(motorStatusProblem_, 0); + pAxis->moveDelay=0; + pAxis->setIntegerParam(motorStatusDone_, 1); + pAxis->setIntegerParam(motorStatusMoving_, 0); + if (pAxis->homing) pAxis->homing = 0; + Debug(1, "%s:%s:%s: done axis %d \n", driverName, functionName, + this->portName, i ); + } + else if (haveVeloArray && (veloArr[i] == 0)){ + /* keep a small delay here, to make sure that a motorAxisDone cycle is performed + even if the motor is on hard limits */ + getIntegerParam(pAxis->axisNo_, motorStatusMoving_, &axisMoving); + if (axisMoving){ + pAxis->moveDelay++ ; + if (pAxis->moveDelay >= 5) { + Debug(4, "%s:%s:%s: setting Problem Flag axis %d\n", driverName, functionName, portName, i); + pAxis->setIntegerParam(motorStatusDone_, 1); + pAxis->setIntegerParam(motorStatusMoving_, 0); + pAxis->setIntegerParam(motorStatusProblem_, 1); + pAxis->moveDelay = 0; + if (pAxis->homing) pAxis->homing = 0; + Debug(1, "%s:%s:%s: stop axis %d, moveDelay count %d\n", driverName, functionName, + this->portName, i, pAxis->moveDelay ); + } + Debug(2, "%s:%s:%s: moveDelay axis %d, count %d\n", driverName, functionName, + this->portName, i, pAxis->moveDelay ); + } + } + else { + Debug(4, "%s:%s:%s: poller loop: axis %d still moving\n", driverName, functionName, this->portName, i); + pAxis->moveDelay = 0; + pAxis->setIntegerParam(motorStatusProblem_, 0); + pAxis->setIntegerParam(motorStatusDone_, 0); + pAxis->setIntegerParam(motorStatusMoving_, 1); + } + getIntegerParam(pAxis->axisNo_, motorStatusMoving_, &axisMoving); + anyMoving += axisMoving; + + /* check limits */ + if (haveLimits){ + if (((limitFlags & (1 << i)) > 0) ^ (pAxis->getLimitInvert())) + pAxis->setIntegerParam(motorStatusLowLimit_, 1); + else + pAxis->setIntegerParam(motorStatusLowLimit_, 0); + if (((limitFlags & (1 << (i+8))) > 0) ^ (pAxis->getLimitInvert())) + pAxis->setIntegerParam(motorStatusHighLimit_, 1); + else + pAxis->setIntegerParam(motorStatusHighLimit_, 0); + } + + /* check home switch */ + if (statusBuffer[i*STATUSSTRINGLEN + 3] == 'H') + pAxis->setIntegerParam(motorStatusAtHome_, 1); + else + pAxis->setIntegerParam(motorStatusAtHome_, 0); + + /* check direction */ + if (statusBuffer[i*STATUSSTRINGLEN] == 'P') + pAxis->setIntegerParam(motorStatusDirection_, 1); + else + pAxis->setIntegerParam(motorStatusDirection_, 0); + + /* set positions */ + pAxis->setDoubleParam(motorPosition_, (double) axisPosArr[i]); + + /* set closed loop status */ + if (haveCLStatus) pAxis->setIntegerParam(motorStatusGainSupport_, closedLoopStatus[i]); + + // callParamCallbacks(pAxis->axisNo_, pAxis->axisNo_); + pAxis->callParamCallbacks(); + + } /* Next axis */ + + if (shuttingDown_) { + unlock(); + break; + } + unlock(); + + if (fastPolls > 0) { + timeout = movingPollPeriod; + fastPolls--; + } + else if (anyMoving) { + timeout = movingPollPeriod; + } + else { + timeout = idlePollPeriod; + } + + /* wait here for the next poll + waiting may be interrupted by pollEvent or interrupt messages*/ + epicsTimeGetCurrent(&now); + timeToWait = timeout - epicsTimeDiffInSeconds(&now, &loopStart); + Debug(5, "%s:%s:%s: poller loop: waiting %f s\n", driverName, functionName, this->portName, timeToWait); + if (waitInterruptible(timeToWait) == epicsEventWaitOK) { + fastPolls = forcedFastPolls; + } + } /* End while */ + Debug(1, "%s:%s:%s: omsPoller shutdown\n", driverName, functionName, portName); +} + +asynStatus omsBaseController::getEncoderPositions(epicsInt32 encPosArr[OMS_MAX_AXES]) +{ + return getAxesArray((char*) "AM PE;", encPosArr); +} + +asynStatus omsBaseController::getClosedLoopStatus(int clstatus[OMS_MAX_AXES]) +{ + asynStatus status = asynSuccess; + char clBuffer[9]; + + if (firmwareMin(1,30,0)){ + pollInputBuffer[0] = '\0'; + status = sendReceiveLock((char*) "AM;CL?;", pollInputBuffer, sizeof(pollInputBuffer)); + if (status == asynSuccess) { + for (int i=0; i < numAxes; ++i) { + status = getSubstring(i, pollInputBuffer, clBuffer, sizeof(clBuffer)); + if ( status == asynSuccess){ + if (strncmp(clBuffer, "on", 2)) + clstatus[i] = 1; + else + clstatus[i] = 0; + } + } + } + } + else { + for (int i=0; i < numAxes; ++i) { + strcpy(clBuffer,"A? ?PM"); + status = sendReceiveReplace(pAxes[i], clBuffer, pollInputBuffer, sizeof(pollInputBuffer)); + if (status != asynSuccess){ + Debug(1, "%s:getClosedLoopStatus:%s: Error getting closed loop status %s\n", + driverName, portName, pollInputBuffer); + } + else { + if (strncmp(pollInputBuffer, "=on", 3)) + clstatus[i] = 1; + else + clstatus[i] = 0; + } + } + } + return status; +} + +asynStatus omsBaseController::sanityCheck() +{ + const char* functionName="sanityCheck"; + asynStatus status = asynSuccess; + + ++ sanityCounter; + + if (sanityCounter > 100){ + int commandBufferSize[OMS_MAX_AXES]; + char outputBuffer[10]; + // check the remaining command buffer size, which must not overflow + sanityCounter = 0; + if (getAxesArray((char*) "AM;RQC", commandBufferSize) != asynSuccess){ + errlogPrintf("%s:%s:%s: Error executing command: Report Command Queue (RCQ)\n", driverName, functionName, this->portName); + } + for (int i=0; i < numAxes; i++) { + omsBaseAxis* pAxis = pAxes[i]; + // flush command buffer if size is below 100 + if (commandBufferSize[i] < 100) { + errlogPrintf("%s:%s:%s: Caution: flushing command queue axis %d,remaining size %d\n", + driverName, functionName, portName, pAxis->axisNo_, commandBufferSize[i]); + strcpy(outputBuffer,"A?;FL;"); + sendReplace(pAxis, (char*) outputBuffer); + status = asynError; + } + } + } + return status; +} + +bool omsBaseController::firmwareMin(int major, int minor, int revision){ + if (major < fwMajor) return true; + if (major == fwMajor){ + if (minor < fwMinor) return true; + if (minor == fwMinor){ + if (revision <= fwRevision) return true; + } + } + return false; +} + +omsBaseController* omsBaseController::findController(const char* asynPort){ + + omsBaseNode* pNode = (omsBaseNode*) ellFirst(&omsControllerList); + while (pNode) { + if (strcmp(pNode->portName, asynPort) == 0){ + return (pNode->pController); + } + pNode = (omsBaseNode *)ellNext(&pNode->node); + } + return (omsBaseController *) NULL; +} + +epicsEventWaitStatus omsBaseController::waitInterruptible(double timeout) +{ + return epicsEventWaitWithTimeout(pollEventId_, timeout); +} + +/* + insert axis character and send to Controller + If insertChar != 0 and 2nd Character is a "?", then replace it with axisChar +*/ +asynStatus omsBaseController::sendReplace(omsBaseAxis* pAxis, char *outputBuff) +{ + asynStatus status; + + if (strncmp(outputBuff,"A? ",3) == 0 ) + outputBuff[1] = pAxis->axisChar; + status = sendOnlyLock(outputBuff); + return status; +} + +/* + insert axis character, send to Controller and wait for the answer + If insertChar != 0 and 2nd Character is a "?", then replace it with axisChar +*/ +asynStatus omsBaseController::sendReceiveReplace(omsBaseAxis* pAxis, char *outputBuff, char *inputBuff, int inputSize) +{ + asynStatus status; + if (strncmp(outputBuff,"A? ",3) == 0 ) + outputBuff[1] = pAxis->axisChar; + status = sendReceiveLock(outputBuff, inputBuff, inputSize); + return status; +} + +asynStatus omsBaseController::sendOnlyLock(const char *outputBuff) +{ + asynStatus status; + baseMutex->lock(); + status = sendOnly(outputBuff); + baseMutex->unlock(); + return status; +} + +asynStatus omsBaseController::sendReceiveLock(const char *outputBuff, char *inputBuff, unsigned int inputSize) +{ + asynStatus status; + baseMutex->lock(); + status = sendReceive(outputBuff, inputBuff, inputSize); + baseMutex->unlock(); + return status; +} + +asynStatus omsBaseController::getAxesStatus(char *inputBuff, int inputSize, bool *done) +{ + char *outputBuff = (char*) "AM;RI;"; + asynStatus status; + + *done=false; + + status = sendReceiveLock(outputBuff, inputBuff, inputSize); + + if (status == asynSuccess){ + if (strchr(inputBuff, 'D') != NULL) *done=true; + if (!((inputBuff[0] == 'P') || (inputBuff[0] == 'M'))) + status = asynError; + if (strlen(inputBuff) < (unsigned int)(numAxes * 5 -1)) + status = asynError; + if (status == asynError) Debug(1, "%s:getAxesStatus:%s: corrupted status string %s\n", + driverName, portName, inputBuff); + } + return status; +} + +asynStatus omsBaseController::getAxesPositions(int positions[OMS_MAX_AXES] ) +{ + return getAxesArray((char*) "AM PP;", positions); +} + +asynStatus omsBaseController::getAxesArray(char* cmd, int positions[OMS_MAX_AXES] ) +{ + // maximum length of the ascii position / velocity values is 11 chars + comma + // -2147483648 <-> 2147483647 + // we expect numAxes values separated with commas + // possible answers are "0,5000,0" ",,,," "0" ",,," (3 commas for 4 axes) + + const char* functionName="getAxesArray"; + asynStatus status = asynSuccess; + char inputBuff[OMSINPUTBUFFERLEN] = ""; + char *start, *end, *stop; + int i, intVal, again = 1; + int count =0; + + status = sendReceiveLock(cmd, inputBuff, sizeof(inputBuff)); + if ((status == asynSuccess) && (strlen(inputBuff) >= (unsigned int)numAxes -1)) { + start = inputBuff; + stop = start + MIN(strlen(inputBuff), sizeof(inputBuff)); + for (i = 0; ((i < OMS_MAX_AXES) && again); ++i){ + if (*start == ','){ + positions[i] = 0; + ++count; + start += 1; + if (*start == '\0'){ + /* comma was last character */ + ++count; + again=0; + if (i < OMS_MAX_AXES-1) positions[i+1] = 0; + } + continue; + } + intVal = strtol(start, &end, 10); + if ((intVal == 0) && (start == end)) { + again = 0; + } + else { + positions[i] = intVal; + ++count; + } + if (end >= stop) again = 0; + start = end + 1; + } + if (count != numAxes) { + errlogPrintf("%s:%s:%s: array string conversion error, count: %d, axes: %d, input: >%s<\n", + driverName, functionName, portName, count, numAxes, inputBuff); + return asynError; + } + } + else { + if (status == asynSuccess){ + errlogPrintf("%s:%s:%s: read string too short %d\n", + driverName, functionName, portName, (int)strlen(inputBuff)); + return asynError; + } + } + return status; +} + +asynStatus omsBaseController::getSubstring(unsigned int number, char* inputBuffer, char *outBuffer, unsigned int outBufferLen) +{ + /* get the numbered substring from a comma-separated string */ + /* numbering starts with 0 */ + /* other as strtok accept a sequence of commas ,,, as empty substrings */ + char *start, *end, *pos, *tmpBuffer; + unsigned int i, count; + int doloop = 1; + asynStatus status = asynError; + + + if (strlen(inputBuffer) < number) return status; + + tmpBuffer = epicsStrDup(inputBuffer); + start = tmpBuffer; + end = start + strlen(start); + for (i=0; i<=number && doloop==1; ++i){ + pos = strchr(start, ','); + if (pos == NULL) { + doloop = 0; + count = MIN(strlen(start), outBufferLen-1); + } + else { + count = MIN((unsigned int)(pos-start), outBufferLen-1); + *pos = '\0'; + } + if (i==number){ + strncpy(outBuffer, start, count); + outBuffer[count]='\0'; + status = asynSuccess; + break; + } + start = pos+1; + if (start > end) doloop =0; + } + free (tmpBuffer); + return status; +} + +bool omsBaseController::watchdogOK() +{ + char inputBuff[10] = ""; + const char* functionName = "watchdogOK"; + + if (useWatchdog && (fwMinor >= 33)) { + sendReceiveLock((char*) "#WS", (char*) inputBuff, sizeof(inputBuff)); + if ((inputBuff[0] == '=') && (inputBuff[1] != '0')) { + errlogPrintf("%s:%s:%s: *** CAUTION watchdog not running, disabling card ***\n", + driverName, functionName, portName); + enabled = false; + return false; + } + } + return true; +} + diff --git a/motorApp/OmsAsynSrc/omsBaseController.h b/motorApp/OmsAsynSrc/omsBaseController.h new file mode 100644 index 00000000..d0166c1e --- /dev/null +++ b/motorApp/OmsAsynSrc/omsBaseController.h @@ -0,0 +1,105 @@ +/* +FILENAME... omsBaseController.h +USAGE... Pro-Dex OMS asyn motor base controller support + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + +/* + * Created on: 06/2012 + * Author: eden + */ + +#ifndef OMSBASECONTROLLER_H_ +#define OMSBASECONTROLLER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "asynMotorController.h" +#include "omsBaseAxis.h" + +#define OMS_MAX_AXES 8 +#define OMSBASE_MAXNUMBERLEN 12 +#define OMSINPUTBUFFERLEN OMSBASE_MAXNUMBERLEN * OMS_MAX_AXES + 2 + +class omsBaseController : public asynMotorController { +public: + omsBaseController(const char *portName, int numAxes, int priority, int stackSize, int extMotorParams); + virtual asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value); + virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + virtual asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value); + virtual void report(FILE *fp, int level); + virtual asynStatus sendReceive(const char*, char*, unsigned int ) = 0; + virtual asynStatus sendOnly(const char *outputBuff) = 0; + void omsPoller(); + virtual asynStatus startPoller(double movingPollPeriod, double idlePollPeriod, int forcedFastPolls); + static void callPoller(void*); + static void callShutdown(void *ptr){((omsBaseController*)ptr)->shutdown();}; + void shutdown(); + +protected: + virtual asynStatus writeOctet(asynUser *, const char *, size_t, size_t *); + virtual asynStatus getFirmwareVersion(); + virtual asynStatus Init(const char*, int); + virtual asynStatus sanityCheck(); + asynStatus sendReceiveLock(const char*, char*, unsigned int ); + asynStatus sendOnlyLock(const char *); + virtual omsBaseAxis* getAxis(asynUser *pasynUser); + virtual omsBaseAxis* getAxis(int); + asynStatus getAxesArray(char*, int positions[OMS_MAX_AXES]); + virtual asynStatus getAxesPositions(int positions[OMS_MAX_AXES]); + virtual asynStatus getAxesStatus(char *, int, bool *); + virtual asynStatus getEncoderPositions(epicsInt32 encPosArr[OMS_MAX_AXES]); + virtual asynStatus getClosedLoopStatus(int clstatus[OMS_MAX_AXES]); + virtual epicsEventWaitStatus waitInterruptible(double timeout); + virtual bool watchdogOK(); + char* getPortName(){return portName;}; + bool firmwareMin(int, int, int); + static omsBaseController* findController(const char*); + static ELLLIST omsControllerList; + static int omsTotalControllerNumber; + char* controllerType; + int fwMajor, fwMinor, fwRevision; + epicsTimeStamp now; + char* portName; + bool useWatchdog; + bool enabled; + int numAxes; + +private: + asynStatus sendReplace(omsBaseAxis*, char*); + asynStatus sendReceiveReplace(omsBaseAxis*, char *, char *, int); + asynStatus getSubstring(unsigned int , char* , char *, unsigned int); + int sanityCounter; + epicsThreadId motorThread; + char inputBuffer[OMSINPUTBUFFERLEN]; + char pollInputBuffer[OMSINPUTBUFFERLEN]; + omsBaseAxis** pAxes; + int controllerNumber; + epicsMutex *baseMutex; + int sendReceiveIndex; + int sendIndex; + int receiveIndex; + int pollIndex; + int priority, stackSize; + + friend class omsBaseAxis; +}; + +typedef struct omsBaseNode { + ELLNODE node; + const char *portName; + omsBaseController* pController; +} omsBaseNode; + +#endif /* OMSBASE_H_ */ diff --git a/motorApp/OmsAsynSrc/omsMAXnet.cpp b/motorApp/OmsAsynSrc/omsMAXnet.cpp new file mode 100644 index 00000000..a89fa1b1 --- /dev/null +++ b/motorApp/OmsAsynSrc/omsMAXnet.cpp @@ -0,0 +1,369 @@ +/* +FILENAME... omsMAXnet.cpp +USAGE... Pro-Dex OMS MAXnet asyn motor controller support + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + +/* + * Created on: 10/2010 + * Author: eden + */ + +#include + +#include "omsMAXnet.h" +#include "asynOctetSyncIO.h" + +#ifdef __GNUG__ + #ifdef DEBUG + #define Debug(l, f, args...) {if (l <= motorMAXnetdebug) \ + errlogPrintf(f, ## args);} + #else + #define Debug(l, f, args...) + #endif +#else + #define Debug +#endif +static const char *driverName = "omsMAXnetDriver"; +volatile int motorMAXnetdebug = 0; +extern "C" {epicsExportAddress(int, motorMAXnetdebug);} + +#define MAXnet_MAX_BUFFERLENGTH 80 + +static void connectCallback(asynUser *pasynUser, asynException exception) +{ + asynStatus status; + int connected = 0; + omsMAXnet* pController = (omsMAXnet*)pasynUser->userPvt; + + if (exception == asynExceptionConnect) { + status = pasynManager->isConnected(pasynUser, &connected); + if (connected){ + if (motorMAXnetdebug > 4) asynPrint(pasynUser, ASYN_TRACE_FLOW, + "MAXnet connectCallback: TCP-Port connected\n"); + pController->portConnected = 1; + } + else { + if (motorMAXnetdebug > 3) asynPrint(pasynUser, ASYN_TRACE_FLOW, + "MAXnet connectCallback: TCP-Port disconnected\n"); + pController->portConnected = 0; + } + } +} + +void omsMAXnet::asynCallback(void *drvPvt, asynUser *pasynUser, char *data, size_t len, int eomReason) +{ + omsMAXnet* pController = (omsMAXnet*)drvPvt; + +/* If the string has a "%", it is a notification, increment counter and + * send a signal to the poller task which will trigger a poll */ + + if ((len >= 1) && (strchr(data, '%') != NULL)){ + char* pos = strchr(data, '%'); + epicsEventSignal(pController->pollEventId_); + while (pos != NULL){ + Debug(2, "omsMAXnet::asynCallback: %s (%d)\n", data, len); + pController->notificationMutex->lock(); + ++pController->notificationCounter; + pController->notificationMutex->unlock(); + ++pos; + pos = strchr(pos, '%'); + } + } +} + +omsMAXnet::omsMAXnet(const char* portName, int numAxes, const char* serialPortName, const char* initString, int priority, int stackSize) + : omsBaseController(portName, numAxes, priority, stackSize, 0){ + + asynStatus status; + asynInterface *pasynInterface; + + controllerType = epicsStrDup("MAXnet"); + + notificationMutex = new epicsMutex(); + notificationCounter = 0; + useWatchdog = true; + + serialPortName = epicsStrDup(serialPortName); + + pasynUserSerial = pasynManager->createAsynUser(0,0); + pasynUserSerial->userPvt = this; + + Debug(9, "omsMAXnet connect to %s \n", serialPortName); + status = pasynManager->connectDevice(pasynUserSerial,serialPortName,0); + if(status != asynSuccess){ + printf("MAXnetConfig: can't connect to port %s: %s\n",serialPortName,pasynUserSerial->errorMessage); + return; + } + + Debug(9, "omsMAXnet add Callback \n"); + status = pasynManager->exceptionCallbackAdd(pasynUserSerial, connectCallback); + if(status != asynSuccess){ + printf("MAXnetConfig: can't set exceptionCallback for %s: %s\n",serialPortName,pasynUserSerial->errorMessage); + return; + } + /* set the connect flag */ + pasynManager->isConnected(pasynUserSerial, &portConnected); + + Debug(9, "omsMAXnet findInterface \n"); + pasynInterface = pasynManager->findInterface(pasynUserSerial,asynOctetType,1); + if( pasynInterface == NULL) { + printf("MAXnetConfig: %s driver not supported\n", asynOctetType); + return; + } + pasynOctetSerial = (asynOctet*)pasynInterface->pinterface; + octetPvtSerial = pasynInterface->drvPvt; + + Debug(9, "omsMAXnet SyncIO->connect \n"); + status = pasynOctetSyncIO->connect(serialPortName, 0, &pasynUserSyncIOSerial, NULL); + if(status != asynSuccess){ + printf("MAXnetConfig: can't connect pasynOctetSyncIO %s: %s\n",serialPortName,pasynUserSyncIOSerial->errorMessage); + return; + } + + /* flush any junk at input port - should be no data available */ + Debug(9, "omsMAXnet flush \n"); + pasynOctetSyncIO->flush(pasynUserSyncIOSerial); + + timeout = 2.0; + pasynUserSerial->timeout = 0.0; + + Debug(9, "omsMAXnet setInputEos \n"); + // CAUTION firmware versions before 1.33.4 use '\n' for serial port and '\n\r' for IP port as InputEOS + // Set inputEOS in st.cmd for old firmware versions + status = pasynOctetSyncIO->setInputEos(pasynUserSyncIOSerial, "\n\r", 2); + if(status != asynSuccess){ + printf("MAXnetConfig: unable to set InputEOS %s: %s\n", serialPortName, pasynUserSyncIOSerial->errorMessage); + return ; + } + + Debug(9, "omsMAXnet setOutputEos \n"); + status = pasynOctetSyncIO->setOutputEos(pasynUserSyncIOSerial, "\n", 1); + if(status != asynSuccess){ + printf("MAXnetConfig: unable to set OutputEOS %s: %s\n",serialPortName,pasynUserSyncIOSerial->errorMessage); + return ; + } + + Debug(9, "omsMAXnet registerInterruptUser \n"); + void* registrarPvt= NULL; + status = pasynOctetSerial->registerInterruptUser(octetPvtSerial, pasynUserSerial, omsMAXnet::asynCallback, this, ®istrarPvt); + if(status != asynSuccess) { + printf("MAXnetConfig: registerInterruptUser failed - %s: %s\n",serialPortName,pasynUserSerial->errorMessage); + return; + } + + Debug(9, "omsMAXnet get FirmwareVersion \n"); + /* get FirmwareVersion */ + if(getFirmwareVersion() != asynSuccess) { + printf("MAXnetConfig: unable to talk to controller at %s: %s\n",serialPortName,pasynUserSyncIOSerial->errorMessage); + return; + } + if (fwMinor < 30 ){ + printf("This Controllers Firmware Version %d.%d is not supported, version 1.30 or higher is mandatory\n", fwMajor, fwMinor); + } + + if( Init(initString, 0) != asynSuccess) { + printf("MAXnetConfig: unable to talk to controller at %s: %s\n",serialPortName,pasynUserSyncIOSerial->errorMessage); + return; + } +} + +// poll the serial port for notification messages while waiting +epicsEventWaitStatus omsMAXnet::waitInterruptible(double timeout) +{ + double pollWait, timeToWait = timeout; + asynStatus status; + size_t nRead; + char inputBuff[1]; + int eomReason = 0; + epicsTimeStamp starttime; + epicsEventWaitStatus waitStatus = epicsEventWaitTimeout; + epicsTimeGetCurrent(&starttime); + + // TODO use local poll Periods or lock() + if (timeout == idlePollPeriod_) + pollWait = idlePollPeriod_ / 5.0; + else + pollWait = movingPollPeriod_ / 20.0; + + pasynManager->lockPort(pasynUserSerial); + pasynOctetSerial->flush(octetPvtSerial, pasynUserSerial); + pasynManager->unlockPort(pasynUserSerial); + while ( timeToWait > 0){ + /* reading with bufferlength 0 and timeout 0.0 triggers a callback and + * poll event, if a notification is outstanding */ + if (enabled) { + pasynManager->lockPort(pasynUserSerial); + status = pasynOctetSerial->read(octetPvtSerial, pasynUserSerial, inputBuff, + 0, &nRead, &eomReason); + pasynManager->unlockPort(pasynUserSerial); + } + if (epicsEventWaitWithTimeout(pollEventId_, pollWait) == epicsEventWaitOK) { + waitStatus = epicsEventWaitOK; + break; + } + epicsTimeGetCurrent(&now); + timeToWait = timeout - epicsTimeDiffInSeconds(&now, &starttime); + } + return waitStatus; +} + +asynStatus omsMAXnet::sendOnly(const char *outputBuff) +{ + size_t nActual = 0; + asynStatus status; + + if (!enabled) return asynError; + Debug(9, "omsMAXnet::sendOnly: write: %s \n", outputBuff); + + status = pasynOctetSyncIO->write(pasynUserSyncIOSerial, outputBuff, + strlen(outputBuff), timeout, &nActual); + + if (status != asynSuccess) { + asynPrint(pasynUserSyncIOSerial, ASYN_TRACE_ERROR, + "drvMAXnetAsyn:sendOnly: error sending command %d, sent=%d, status=%d\n", + outputBuff, nActual, status); + } + Debug(4, "omsMAXnet::sendOnly: wrote: %s \n", outputBuff); + return(status); +} + +asynStatus omsMAXnet::sendReceive(const char *outputBuff, char *inputBuff, unsigned int inputSize) +{ + char localBuffer[MAXnet_MAX_BUFFERLENGTH + 1] = ""; + size_t nRead, nWrite, bufferSize = inputSize; + int eomReason = 0; + asynStatus status = asynSuccess; + char *outString; + int errorCount = 10; + + if (!enabled) return asynError; + + Debug(4, "omsMAXnet::sendReceive: write: %s \n", outputBuff); + + if (bufferSize > MAXnet_MAX_BUFFERLENGTH) bufferSize = MAXnet_MAX_BUFFERLENGTH; + + Debug(9, "omsMAXnet::sendReceive: read the notification \n"); + /* + * read the notification from input buffer + */ + while ((notificationCounter > 0) && errorCount){ + status = pasynOctetSyncIO->read(pasynUserSyncIOSerial, localBuffer, sizeof(localBuffer), 0.1, &nRead, &eomReason); + if (status == asynSuccess) { + localBuffer[nRead] = '\0'; + if (isNotification(localBuffer) && (notificationCounter > 0)) { + Debug(2, "omsMAXnet::sendReceive: decrement notificationCounter: %s, len: %d, reason: %d\n", localBuffer, nRead, eomReason); + --notificationCounter; + } + } + else if (status == asynTimeout) { + notificationCounter = 0; + } + else { + --errorCount; + } + } + + Debug(9, "omsMAXnet::sendReceive: write \n"); + status = pasynOctetSyncIO->writeRead(pasynUserSyncIOSerial, outputBuff, strlen(outputBuff), localBuffer, + bufferSize, timeout, &nWrite, &nRead, &eomReason); + if (status == asynSuccess) localBuffer[nRead] = '\0'; + + // if input data is a notification read until expected data arrived + while ((status == asynSuccess) && isNotification(localBuffer)) { + Debug(2, "omsMAXnet::sendReceive: notification while reading: %s\n", localBuffer); + status = pasynOctetSyncIO->read(pasynUserSyncIOSerial, localBuffer, + bufferSize, timeout, &nRead, &eomReason); + if (status == asynSuccess) localBuffer[nRead] = '\0'; + } + + if ((status == asynSuccess) && (eomReason == ASYN_EOM_EOS)) { + // cut off a leading /006 + outString = strrchr(localBuffer, 6); + if (outString == NULL) { + outString = localBuffer; + } + else { + ++outString; + } + + // copy into inputBuffer + strncpy(inputBuff, outString, inputSize); + inputBuff[inputSize-1] = '\0'; + } + + Debug(4, "omsMAXnet::sendReceive: read: %s \n", inputBuff); + + return status; +} + +/* + * check if buffer is a notification messages with 13 chars ("%000 SSSSSSSS") + */ +int omsMAXnet::isNotification (char *buffer) { + + const char* functionName="isNotification"; + char *inString; + if ((inString = strstr(buffer, "000 0")) != NULL){ + if ((inString = strstr(buffer, "000 01")) != NULL){ + printf("%s:%s:%s: CMD_ERR_FLAG received\n", driverName, functionName, portName); + } + else { + if (motorMAXnetdebug > 1) printf("%s:%s:%s: Interrupt notification: %s\n", + driverName, functionName, portName, buffer); + epicsEventSignal(pollEventId_); + } + return 1; + } + else + return 0; +} + +extern "C" int omsMAXnetConfig( + const char *portName, /* MAXnet Motor Asyn Port name */ + int numAxes, /* Number of axes this controller supports */ + const char *serialPortName,/* MAXnet Serial Asyn Port name */ + 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 */ +{ + // for now priority and stacksize are hardcoded here, should they be configurable in omsMAXnetConfig? + int priority = epicsThreadPriorityMedium; + int stackSize = epicsThreadGetStackSize(epicsThreadStackMedium); + omsMAXnet *pController = new omsMAXnet(portName, numAxes, serialPortName, initString, priority, stackSize); + pController->startPoller((double)movingPollPeriod, (double)idlePollPeriod, 10); + return(asynSuccess); +} + +/* Code for iocsh registration */ + +/* omsMAXnetConfig */ +static const iocshArg omsMAXnetConfigArg0 = {"asyn motor port name", iocshArgString}; +static const iocshArg omsMAXnetConfigArg1 = {"number of axes", iocshArgInt}; +static const iocshArg omsMAXnetConfigArg2 = {"asyn serial/tcp port name", iocshArgString}; +static const iocshArg omsMAXnetConfigArg3 = {"moving poll rate", iocshArgInt}; +static const iocshArg omsMAXnetConfigArg4 = {"idle poll rate", iocshArgInt}; +static const iocshArg omsMAXnetConfigArg5 = {"initstring", iocshArgString}; +static const iocshArg * const omsMAXnetConfigArgs[6] = {&omsMAXnetConfigArg0, + &omsMAXnetConfigArg1, + &omsMAXnetConfigArg2, + &omsMAXnetConfigArg3, + &omsMAXnetConfigArg4, + &omsMAXnetConfigArg5 }; +static const iocshFuncDef configOmsMAXnet = {"omsMAXnetConfig", 6, omsMAXnetConfigArgs}; +static void configOmsMAXnetCallFunc(const iocshArgBuf *args) +{ + omsMAXnetConfig(args[0].sval, args[1].ival, args[2].sval, args[3].ival, args[4].ival, args[5].sval); +} + +static void OmsMAXnetAsynRegister(void) +{ + iocshRegister(&configOmsMAXnet, configOmsMAXnetCallFunc); +} + +epicsExportRegistrar(OmsMAXnetAsynRegister); + diff --git a/motorApp/OmsAsynSrc/omsMAXnet.h b/motorApp/OmsAsynSrc/omsMAXnet.h new file mode 100644 index 00000000..fad4c03e --- /dev/null +++ b/motorApp/OmsAsynSrc/omsMAXnet.h @@ -0,0 +1,42 @@ +/* +FILENAME... omsMAXnet.h +USAGE... Pro-Dex OMS MAXnet asyn motor controller support + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + +/* + * Created on: 10/2010 + * Author: eden + */ + +#ifndef OMSMAXNET_H_ +#define OMSMAXNET_H_ + +#include "omsBaseController.h" + +class omsMAXnet : public omsBaseController { +public: + omsMAXnet(const char* , int , const char*, const char*, int , int ); + static void asynCallback(void*, asynUser*, char *, size_t, int); + int portConnected; + int notificationCounter; + epicsMutex* notificationMutex; + epicsEventWaitStatus waitInterruptible(double); + asynStatus sendReceive(const char *, char *, unsigned int ); + asynStatus sendOnly(const char *); + +private: + int isNotification (char *); + asynUser* pasynUserSerial; + asynUser* pasynUserSyncIOSerial; + asynOctet *pasynOctetSerial; + void* octetPvtSerial; + char* serialPortName; + double timeout; +}; + +#endif /* OMSMAXNET_H_ */ diff --git a/motorApp/OmsAsynSrc/omsMAXv.cpp b/motorApp/OmsAsynSrc/omsMAXv.cpp new file mode 100644 index 00000000..ada5a584 --- /dev/null +++ b/motorApp/OmsAsynSrc/omsMAXv.cpp @@ -0,0 +1,649 @@ +/* +FILENAME... omsMAXv.cpp +USAGE... Pro-Dex OMS MAXv asyn motor controller support + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + +/* + * based on drvMAXv.cc written by Ron Sluiter + * + * Created on: 10/2010 + * Author: eden + */ + +#include +#include + +#include "omsMAXv.h" +#include "asynOctetSyncIO.h" +#include +#include + +static const char *driverName = "omsMAXvDriver"; + +#define MIN(a,b) ((a)<(b)? (a): (b)) + +#ifdef __GNUG__ + #ifdef DEBUG + #define Debug(l, f, args...) {if (l & motorMAXvdebug) \ + errlogPrintf(f, ## args);} + #else + #define Debug(l, f, args...) + #endif +#else + #define Debug +#endif +volatile int motorMAXvdebug = 0; +extern "C" {epicsExportAddress(int, motorMAXvdebug);} + +char* omsMAXv::baseAddress = 0x0; +int omsMAXv::numCards = 0; +epicsUInt32 omsMAXv::baseInterruptVector = OMS_INT_VECTOR; +epicsUInt8 omsMAXv::interruptLevel = OMS_INT_LEVEL; +epicsAddressType omsMAXv::addrType = atVMEA16; + +void omsMAXv::InterruptHandler( void * param ) +{ + omsMAXv* pController = (omsMAXv*) param; + volatile struct MAXv_motor *pmotor = (MAXv_motor*) pController->getCardAddress();; + STATUS1 status1_flag; + static char errmsg[65]; + + status1_flag.All = pmotor->status1_flag.All; + + /* Motion done handling */ + if (status1_flag.Bits.done != 0) epicsEventSignal(pController->pollEventId_); + + if (status1_flag.Bits.cmndError) + { + strcpy(errmsg, "\nomsMAXv::InterruptHandler: command error - Port: "); + strncat(errmsg, pController->getPortName(), sizeof(errmsg)-strlen(errmsg)-2); + strcat(errmsg,"\n"); + epicsInterruptContextMessage(errmsg); + } + + // not clearing this may corrupt the next read/write operation + if (status1_flag.Bits.text_response != 0) status1_flag.Bits.text_response = 0; + + pmotor->status1_flag.All = status1_flag.All; /* Release IRQ's. */ + + /* do a dummy read to ensure that all previous writes, which may + * have been queued in the VME bridge chip get processed + */ + status1_flag.All = pmotor->status1_flag.All; + +} +omsMAXv::omsMAXv(const char* portName, int numAxes, int cardNo, const char* initString, + int priority, int stackSize, int addParams) + : omsBaseController(portName, numAxes, priority, stackSize, addParams) +{ + int vector = 0; + + if (baseInterruptVector != 0) + vector = baseInterruptVector + cardNo; + + initialize(portName, numAxes, cardNo, initString, priority, stackSize, 1, vector, interruptLevel, addrType, addParams); +} +omsMAXv::omsMAXv(const char* portName, int numAxes, int slotNo, const char* initString, int priority, + int stackSize, unsigned int vmeAddr, int vector, int intlevel, const char* addressType, int addParams) + : omsBaseController(portName, numAxes, priority, stackSize, addParams) +{ + const char* functionName = "omsMAXv"; + epicsAddressType vmeAddrType = atVMEA16; + + if (vmeAddr < 0) { + errlogPrintf("%s: invalid VME address: 0\n", functionName); + return; + } + + if (!strncmp(addressType, "A16",3)){ + vmeAddrType = atVMEA16; + if (vmeAddr & 0xFFFF0FFF) { + errlogPrintf("%s: invalid %s address: 0x%X.\n", functionName, addressType, vmeAddr); + return; + } + } + else if (!strncmp(addressType, "A24",3)){ + vmeAddrType = atVMEA24; + if (vmeAddr & 0xFF00FFFF) { + errlogPrintf("%s: invalid %s address: 0x%X.\n", functionName, addressType, vmeAddr); + return; + } + } + else if (!strncmp(addressType, "A32",3)){ + vmeAddrType = atVMEA32; + if (vmeAddr & 0x00FFFFFF) { + errlogPrintf("%s: invalid %s address: 0x%X.\n", functionName, addressType, vmeAddr); + return; + } + } + else if (strncmp(addressType, "CSR",3)){ + errlogPrintf("%s: VME CSR not supported\n", functionName); + return; + } + else { + errlogPrintf("%s: invalid address type, Please specify one of A16,A24,A32\n", functionName); + } + if (intlevel < 1 || intlevel > 6) { + errlogPrintf("%s: invalid interrupt level %d, Please specify a value between 1 and 6\n", functionName, intlevel); + return; + } + initialize(portName, numAxes, 0, initString, priority, stackSize, vmeAddr, vector, intlevel, vmeAddrType, addParams); +} + +void omsMAXv::initialize(const char* portName, int numAxes, int cardNo, const char* initString, int prio, + int stackSz, unsigned int vmeAddr, int intrVector, int level, epicsAddressType vmeAddrType, int paramCount) +{ + + const char* functionName = "initialize"; + long status; + void* probeAddr; + + Debug(32, "omsMAXv::initialize: start initialize\n" ); + + controllerType = epicsStrDup("MAXv"); + + // TODO check if cardNo has already been used + this->cardNo = cardNo; + if(cardNo < 0 || cardNo >= MAXv_NUM_CARDS){ + printf("invalid cardNo: %d", cardNo); + return; + } + + epicsUInt8 *startAddr; + epicsUInt8 *endAddr; + epicsUInt32 boardAddrSize = 0; + + if (vmeAddrType == atVMEA16) + boardAddrSize = 0x1000; + else if (vmeAddrType == atVMEA24) + boardAddrSize = 0x10000; + else if (vmeAddrType == atVMEA32) + boardAddrSize = 0x1000000; + + // if vmeAddr == 1 Setup/Config is used and not Config2 + if (vmeAddr == 1) + probeAddr = baseAddress + (cardNo * boardAddrSize); + else + probeAddr = (void*) vmeAddr; + + startAddr = (epicsUInt8 *) probeAddr; + endAddr = startAddr + boardAddrSize; + + Debug(64, "motor_init: devNoResponseProbe() on addr %p\n", probeAddr); + + /* Scan memory space to assure card id */ + while (startAddr < endAddr) { + status = devNoResponseProbe(vmeAddrType, (size_t) startAddr, 2); + if (status != S_dev_addressOverlap) { + errlogPrintf("%s:%s:%s: Card NOT found in specified address range! \n", + driverName, functionName, portName); + enabled = false; + return; + } + startAddr += (boardAddrSize / 10); + } + + status = devRegisterAddress(controllerType, vmeAddrType, + (size_t) probeAddr, boardAddrSize, + (volatile void **) &pmotor); + Debug(64, "motor_init: devRegisterAddress() status = %d\n", (int) status); + + if (status) { + errlogPrintf("%s:%s:%s: Can't register address 0x%lx \n", + driverName, functionName, portName, (long unsigned int) probeAddr); + return; + } + + Debug(64, "motor_init: pmotor = %p\n", pmotor); + + int loopCount=15; + while (loopCount && (pmotor->firmware_status.Bits.initializing == 1)){ + Debug(1, "MAXv port %s still initializing; status = 0x%x\n", + portName, (unsigned int) pmotor->firmware_status.All); + epicsThreadSleep(0.2); + --loopCount; + } + + Debug(64, "motor_init: check if card is ready\n"); + + if (pmotor->firmware_status.Bits.running == 0) + errlogPrintf("MAXv port %s is NOT running; status = 0x%x\n", + portName, (unsigned int) pmotor->firmware_status.All); + + Debug(64, "motor_init: init card\n"); + + FIRMWARE_STATUS fwStatus; + fwStatus.All = pmotor->firmware_status.All; + Debug(64, "motor_init: firmware status register: 0x%x\n", fwStatus.All); + + pmotor->IACK_vector = intrVector; + + pmotor->status1_flag.All = 0xFFFFFFFF; + pmotor->status2_flag = 0xFFFFFFFF; + /* Disable all interrupts */ + pmotor->status1_irq_enable.All = 0; + pmotor->status2_irq_enable = 0; + + Debug(64, "motor_init: clear all interrupt\n"); + //sendOnly("IC"); + + Debug(64, "motor_init: firmware version\n"); + + /* get FirmwareVersion */ + if(getFirmwareVersion() != asynSuccess) { + errlogPrintf("%s:%s:%s: unable to talk to controller card %d\n", + driverName, functionName, portName, cardNo); + return; + } + + if (fwMinor < 30 ){ + errlogPrintf("%s:%s:%s: This Controllers Firmware Version %d.%d is not supported, version 1.30 or higher is mandatory\n", + driverName, functionName, portName, fwMajor, fwMinor); + } + + Debug(64, "motor_init: send init string\n"); + + if( Init(initString, 1) != asynSuccess) { + errlogPrintf("%s:%s:%s: unable to send initstring to controller card %d\n", + driverName, functionName, portName, cardNo); + return; + } + + useWatchdog = true; + + if (watchdogOK()) { + Debug(64, "motor_init: enable interrupts ( vector=%d, level=%d) \n", intrVector, level); + /* Enable interrupt-when-done if selected */ + if (intrVector) motorIsrSetup((unsigned int)intrVector, level); + } + else + return; + + if (epicsAtExit(&omsMAXv::resetOnExit, this)) + errlogPrintf("%s:%s:%s: card %d, unable to register exit function\n", + driverName, functionName, portName, cardNo); + + return; +} + +void omsMAXv::resetIntr() +{ + enabled=false; + pmotor->status1_irq_enable.All = 0; +} + +asynStatus omsMAXv::sendOnly(const char *outputBuff) +{ + STATUS1 flag1; + const char* functionName = "sendOnly"; + int len = strlen(outputBuff); + epicsUInt16 getIndex, putIndex; + + if (!enabled) return asynError; + Debug(16, "omsMAXv::send: sending: %s \n", outputBuff); + + if (len > (BUFFER_SIZE-1)) + { + errlogPrintf("%s:%s:%s: message too long: %d character\n", + driverName, functionName, portName, len); + return asynError; + } + + /* see if junk at input port - should not be any data available */ + if ((epicsUInt16) pmotor->inGetIndex != (epicsUInt16) pmotor->inPutIndex) + { + // flush cards response Buffer +#ifdef DEBUG + int deltaIndex = ((epicsUInt16)pmotor->inPutIndex) - ((epicsUInt16)pmotor->inGetIndex); +#endif + Debug(32, "%s:%s:%s: flushing %d characters\n", + driverName, functionName, portName, (((deltaIndex < 0) ? BUFFER_SIZE + + deltaIndex : deltaIndex))); + putIndex = (epicsUInt16) pmotor->inPutIndex; + pmotor->inGetIndex = putIndex; + pmotor->status1_flag.Bits.text_response = 0; + flag1.All = pmotor->status1_flag.All; + pmotor->status1_flag.All = flag1.All; + pmotor->msg_semaphore=0; + + } + + putIndex = (epicsUInt16) pmotor->outPutIndex; + getIndex = (epicsUInt16) pmotor->outGetIndex; + + for (int i = 0; (i < len); i++) { + pmotor->outBuffer[putIndex++]= outputBuff[i]; + if (putIndex >= BUFFER_SIZE) putIndex = 0; + } + + pmotor->outPutIndex = putIndex; /* Message Sent */ + + int count=0, prevdeltaIndex =0; + int deltaIndex = ((epicsUInt16)pmotor->outPutIndex) - ((epicsUInt16)pmotor->outGetIndex); + int index = 0; + while (deltaIndex != 0) + { + deltaIndex = ((epicsUInt16)pmotor->outPutIndex) - ((epicsUInt16)pmotor->outGetIndex); + // do busy-waiting but not more than 100 times + while ((index < 100) && (deltaIndex != 0)){ + deltaIndex = ((epicsUInt16)pmotor->outPutIndex) - ((epicsUInt16)pmotor->outGetIndex); + ++index; + } + // epicsThreadSleepQuantum => 0.02s for RTEMS + if ((index >= 100) && (deltaIndex != 0)) epicsThreadSleep(epicsThreadSleepQuantum()); + if (deltaIndex == prevdeltaIndex) + ++count; + else + count =0; + if (count > 10) break; + prevdeltaIndex = deltaIndex; + }; + Debug(32, "%s:%s:%s: Waited %d loops\n", driverName, functionName, portName, index); + + if (deltaIndex != 0) { + Debug(1, "%s:%s:%s: Timeout\n", driverName, functionName, portName); + return asynTimeout; + } + + Debug(64, "omsMAXv::send: done\n"); + + return asynSuccess; +} +/** + * read just one line of input + */ +asynStatus omsMAXv::sendReceive(const char *outputBuff, char *inputBuff, unsigned int inputSize) +{ +#ifdef DEBUG + const char* functionName = "sendReceive"; +#endif + + STATUS1 flag1; + epicsUInt16 getIndex, putIndex; + size_t bufsize; + size_t usedSpace = 0; + char *start, *end; + int itera; + asynStatus status; + + if (!enabled) return asynError; + + status = sendOnly(outputBuff); + if (status != asynSuccess) return status; + + if (inputSize <= 0) return status; + + *inputBuff = '\0'; + + Debug(64, "omsMAXv::sendReceive: receiving\n"); + itera = 0; + double time = 0.0; + double timeout = epicsThreadSleepQuantum() + 0.001; + while ((pmotor->status1_flag.Bits.text_response == 0) && (time < timeout)){ + Debug(32, "%s:%s:%s: Waiting for reponse, itera:%d\n", + driverName, functionName, portName, itera); + // busy-waiting but not more than 2000 times + if (itera > 2000){ + time += epicsThreadSleepQuantum(); + epicsThreadSleep(epicsThreadSleepQuantum()); + } + itera++; + } + + if (pmotor->status1_flag.Bits.text_response == 0) + { + Debug(1, "Timeout occurred in recv_mess, %s\n", outputBuff); + return asynTimeout; + } + + getIndex = (epicsUInt16) pmotor->inGetIndex; + putIndex = (epicsUInt16) pmotor->inPutIndex; + bufsize = putIndex - getIndex; + start = (char *) &pmotor->inBuffer[getIndex]; + end = (char *) &pmotor->inBuffer[putIndex]; + + if (start < end) { /* Test for message wraparound in buffer. */ + usedSpace = MIN(bufsize, inputSize); + memcpy(inputBuff, start, usedSpace); + } + else + { + bufsize += BUFFER_SIZE; + size_t firstPart = ((char *) &pmotor->inBuffer[BUFFER_SIZE]) - start; + + usedSpace = MIN(firstPart, inputSize); + memcpy(inputBuff, start, usedSpace); + size_t copySize = MIN(bufsize - firstPart, inputSize - usedSpace); + memcpy((inputBuff + usedSpace), (const char *) &pmotor->inBuffer[0], copySize); + usedSpace += copySize; + } + + inputBuff[usedSpace - 1]= '\0'; + + getIndex += bufsize; + if (getIndex >= BUFFER_SIZE) + getIndex -= BUFFER_SIZE; + + while (getIndex != (epicsUInt16)pmotor->inPutIndex) + { + Debug(2, "readbuf(): flushed - %d\n", pmotor->inBuffer[getIndex]); + if (++getIndex > BUFFER_SIZE) + getIndex = 0; + } + pmotor->status1_flag.Bits.text_response = 0; + + pmotor->inGetIndex = (epicsUInt32) getIndex; + flag1.All = pmotor->status1_flag.All; + pmotor->status1_flag.All = flag1.All; + pmotor->msg_semaphore=0; + + Debug(16, "omsMAXv::sendReceive: received %s\n", inputBuff); + return asynSuccess; +} + + +void omsMAXv::motorIsrSetup(volatile unsigned int vector, volatile epicsUInt8 level) +{ + const char* functionName = "motorIsrSetup"; + STATUS1 status1_irq; + long status; + + Debug(64, "omsMAXv::isrSetup: start\n"); + + status = pdevLibVirtualOS->pDevConnectInterruptVME( vector, &omsMAXv::InterruptHandler, this); + + if (status) { + errlogPrintf("%s:%s:%s: Can't connect to interrupt vector %d\n", + driverName, functionName, portName, vector); + return; + } + + Debug(64, "omsMAXv::isrSetup: set level\n"); + status = devEnableInterruptLevel(intVME, level); + if (status) { + errlogPrintf("%s:%s:%s: Can't enable interrupt level %d\n", + driverName, functionName, portName, level); + return; + } + + /* Setup card for interrupt-on-done */ + status1_irq.All = 0; + status1_irq.Bits.done = 0xFF; + status1_irq.Bits.cmndError = 1; + + pmotor->status1_irq_enable.All = status1_irq.All; /* Enable interrupts. */ + pmotor->status2_irq_enable = 0x0; + + Debug(64, "omsMAXv::isrSetup: done\n"); + return; +} + +extern "C" int omsMAXvSetup( + int num_cards, /* maximum number of cards in rack */ + int addr_type, /* VME address type; 16 -> A16, 24 -> A24 or 32 -> A32. */ + unsigned int addrs, /* Base Address. */ + unsigned int vector, /* noninterrupting(0), valid vectors(64-255) */ + int int_level) /* interrupt level (1-6) */ + +{ + const char* functionName = "omsMAXvSetup"; + if (num_cards < 1 || num_cards > MAXv_NUM_CARDS) + { + errlogPrintf("\n%s: number of cards specified = %d but must be 1 <= number <= %d\n", + functionName, num_cards, MAXv_NUM_CARDS); + epicsThreadSleep(5.0); + return 1; + } + omsMAXv::numCards = num_cards; + omsMAXv::baseAddress = (char *) addrs; + + switch (addr_type) + { + case 16: + omsMAXv::addrType = atVMEA16; + if ((epicsUInt32) addrs & 0xFFFF0FFF) { + errlogPrintf("%s: invalid A%d address: 0x%X.\n", functionName, addr_type, (epicsUInt32) addrs); + return 1; + } + break; + case 24: + omsMAXv::addrType = atVMEA24; + if ((epicsUInt32) addrs & 0xFF00FFFF) { + errlogPrintf("%s: invalid A%d address: 0x%X.\n", functionName, addr_type, (epicsUInt32) addrs); + return 1; + } + break; + case 32: + omsMAXv::addrType = atVMEA32; + if ((epicsUInt32) addrs & 0x00FFFFFF) { + errlogPrintf("%s: invalid A%d address: 0x%X.\n", functionName, addr_type, (epicsUInt32) addrs); + return 1; + } + break; + default: + errlogPrintf("%s: invalid address type, Please specify one of 16/24/32 for VME A16/A24/A32\n", functionName); + return 1; + break; + } + + if ((vector != 0) && (vector < 64 || vector > 255)) { + errlogPrintf("%s: invalid address type, Please specify a value between 64 and 255\n", functionName); + epicsThreadSleep(5.0); + return 1; + } + omsMAXv::baseInterruptVector = vector; + + if (int_level < 1 || int_level > 6) { + errlogPrintf("%s: invalid interrupt level, Please specify a value between 1 and 6\n", functionName); + epicsThreadSleep(5.0); + return 1; + } + omsMAXv::interruptLevel = int_level; + + return 0; +} + +extern "C" int omsMAXvConfig( + 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 */ +{ + omsMAXv *pController = new omsMAXv(portName, numAxes, cardNo, initString, 0, 0, 0); + pController->startPoller((double)movingPollPeriod, (double)idlePollPeriod, 10); + return 0; +} + +/* + * extended MAXv configuration, which may be used instead of omsMAXvConfig, + * if more details need to be specified. + * omsMAXvConfig2 does not need and ignores omsMAXvSetup + */ +extern "C" int omsMAXvConfig2( + 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 */ +{ + omsMAXv *pController = new omsMAXv(portName, numAxes, slotNo, initString, priority, + stackSize, addrs, vector, int_level, addr_type, 0); + pController->startPoller((double)movingPollPeriod, (double)idlePollPeriod, 10); + return 0; +} + +/* Code for iocsh registration */ +/* omsMAXvSetup */ +static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt}; +static const iocshArg setupArg1 = {"VME address type", iocshArgInt}; +static const iocshArg setupArg2 = {"Base Address on 4K (0x1000) boundary", iocshArgInt}; +static const iocshArg setupArg3 = {"noninterrupting(0), valid vectors(64-255)", iocshArgInt}; +static const iocshArg setupArg4 = {"interrupt level (1-6)", iocshArgInt}; +static const iocshArg * const OmsSetupArgs[5] = { &setupArg0, &setupArg1, &setupArg2, + &setupArg3, &setupArg4}; +static const iocshFuncDef setupMAXv = {"omsMAXvSetup", 5, OmsSetupArgs}; +static void setupMAXvCallFunc(const iocshArgBuf *args) +{ + omsMAXvSetup(args[0].ival, args[1].ival, args[2].ival, args[3].ival, args[4].ival); +} + +/* omsMAXvConfig */ +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 = {"omsMAXvConfig", 6, configArgs}; +static void configMAXvCallFunc(const iocshArgBuf *args) +{ + omsMAXvConfig(args[0].ival, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].sval); +} + +/* omsMAXvConfig2 */ +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 = {"omsMAXvConfig2", 12, config2Args}; +static void config2MAXvCallFunc(const iocshArgBuf *args) +{ + omsMAXvConfig2(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 OmsMAXvAsynRegister(void) +{ + iocshRegister(&setupMAXv, setupMAXvCallFunc); + iocshRegister(&configMAXv, configMAXvCallFunc); + iocshRegister(&config2MAXv, config2MAXvCallFunc); +} + +epicsExportRegistrar(OmsMAXvAsynRegister); + diff --git a/motorApp/OmsAsynSrc/omsMAXv.h b/motorApp/OmsAsynSrc/omsMAXv.h new file mode 100644 index 00000000..eefb11cc --- /dev/null +++ b/motorApp/OmsAsynSrc/omsMAXv.h @@ -0,0 +1,176 @@ +/* +FILENAME... omsMAXv.h +USAGE... Pro-Dex OMS MAXv asyn motor controller support + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + +/* + * Created on: 10/2010 + * Author: eden + */ + +#ifndef OMSMAXV_H_ +#define OMSMAXV_H_ + +#include "omsBaseController.h" +#include + +#define BUFFER_SIZE 1024 + +#define MAXv_NUM_CARDS 15 /* maximum number of cards */ +#define OMS_INT_VECTOR 180 /* default interrupt vector (64-255) */ +#define OMS_INT_LEVEL 5 /* default interrupt level (1-6) */ + +/* Limit Switch Status - Offset = 0x40 */ +typedef union +{ + epicsUInt32 All; + struct + { + unsigned int s_minus :1; + unsigned int r_minus :1; + unsigned int v_minus :1; + unsigned int u_minus :1; + unsigned int t_minus :1; + unsigned int z_minus :1; + unsigned int y_minus :1; + unsigned int x_minus :1; + unsigned int s_plus :1; + unsigned int r_plus :1; + unsigned int v_plus :1; + unsigned int u_plus :1; + unsigned int t_plus :1; + unsigned int z_plus :1; + unsigned int y_plus :1; + unsigned int x_plus :1; + } Bits; +} LIMIT_SWITCH; + +/* Home Switch Status - Offset = 0x44 */ +typedef union +{ + epicsUInt32 All; + struct + { + unsigned int home_s :1; /* status of S axis */ + unsigned int home_r :1; /* status of R axis */ + unsigned int home_v :1; /* status of V axis */ + unsigned int home_u :1; /* status of U axis */ + unsigned int home_t :1; /* status of T axis */ + unsigned int home_z :1; /* status of Z axis */ + unsigned int home_y :1; /* status of Y axis */ + unsigned int home_x :1; /* status of X axis */ + } Bits; +} HOME_SWITCH; + +/* Firmware status - Offset = 0x48 */ +typedef union +{ + epicsUInt32 All; + struct + { + unsigned int na19 :13; /* N/A bits 19-31 */ + unsigned int factoryparm_loaded :1; + unsigned int altparm_loaded :1; + unsigned int defaultparm_loaded :1; + unsigned int altprgm_err :1; + unsigned int altparm_chksum_err :1; + unsigned int prgm_err :1; + unsigned int parm_chksum_err :1; + unsigned int na10 :2; /* N/A bits 10-11 */ + unsigned int program_error :1; + unsigned int flash_chksum_err :1; + unsigned int na3 :5; /* N/A bits 3-7 */ + unsigned int running :1; + unsigned int initializing :1; + unsigned int not_downloaded :1; + } Bits; +} FIRMWARE_STATUS; + + +/* Status#1 - Offset = 0xFC0 */ +typedef union +{ + epicsUInt32 All; + struct + { + unsigned int na3 :5; /* N/A bits 27-31. */ + unsigned int data_avail :1; + unsigned int text_response :1; + unsigned int cmndError :1; /* Command error dectect */ + unsigned int slip :8; + unsigned int overtravel :8; + unsigned int done :8; + } Bits; +} STATUS1; + +/* OMS MAXv VME dual port memory map */ +struct MAXv_motor +{ + epicsUInt32 cmndPos[8]; + epicsUInt32 encPos[8]; + LIMIT_SWITCH limit_switch; + HOME_SWITCH home_switch; + FIRMWARE_STATUS firmware_status; + epicsUInt32 direct_cmnd_mbox; + epicsUInt32 position_req_mbox; + epicsUInt32 coherent_cmndPos[8]; + epicsUInt32 coherent_encPos[8]; + epicsUInt32 msg_semaphore; + epicsUInt32 queue_flush_mbox; + epicsUInt32 gpio; + epicsUInt32 naA0[19]; /* N/A byte offset 0xA0 - 0xEB. */ + epicsUInt32 flash_pgm_ptr; + epicsUInt32 outPutIndex; + epicsUInt32 outGetIndex; + epicsUInt32 inPutIndex; + epicsUInt32 inGetIndex; + epicsUInt8 outBuffer[BUFFER_SIZE]; + epicsUInt8 inBuffer[BUFFER_SIZE]; + epicsUInt8 utility[BUFFER_SIZE]; + epicsUInt32 naD00[176]; /* N/A byte offset 0xD00 - 0xFBF. */ + STATUS1 status1_flag; + STATUS1 status1_irq_enable; + epicsUInt32 status2_flag; + epicsUInt32 status2_irq_enable; + epicsUInt32 IACK_vector; + epicsUInt32 config_switch; + epicsUInt32 AM_register; + epicsUInt32 naFDC[7]; + epicsUInt32 FIFO_status_cntrl; + epicsUInt32 FIFO_date; + +}; + +class omsMAXv : public omsBaseController { +public: + omsMAXv(const char*, int, int, const char*, int, int, unsigned int, int, int, const char*, int); + omsMAXv(const char*, int, int, const char*, int, int, int ); + asynStatus sendReceive(const char *, char *, unsigned int ); + asynStatus sendOnly(const char *); + static char* baseAddress; + static epicsAddressType addrType; + static int numCards; + static epicsUInt32 baseInterruptVector; + static epicsUInt8 interruptLevel; + static void InterruptHandler( void * param ); + void* getCardAddress(){return (void*) pmotor;}; + static void resetOnExit(void* param){((omsMAXv*)param)->resetIntr();}; + void resetIntr(); + int getCardNo(){return cardNo;}; + +protected: + virtual void initialize(const char*, int, int, const char*, int, int, unsigned int, int, int, epicsAddressType, int ); + +private: + void motorIsrSetup(volatile unsigned int, volatile epicsUInt8); + int cardNo; + volatile struct MAXv_motor *pmotor; + char readBuffer[BUFFER_SIZE]; +}; + +#endif /* OMSMAXV_H_ */ diff --git a/motorApp/OmsAsynSrc/omsMAXvEncFunc.cpp b/motorApp/OmsAsynSrc/omsMAXvEncFunc.cpp new file mode 100644 index 00000000..83224a8a --- /dev/null +++ b/motorApp/OmsAsynSrc/omsMAXvEncFunc.cpp @@ -0,0 +1,223 @@ +/* +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); + diff --git a/motorApp/OmsAsynSrc/omsMAXvEncFunc.h b/motorApp/OmsAsynSrc/omsMAXvEncFunc.h new file mode 100644 index 00000000..3b993db4 --- /dev/null +++ b/motorApp/OmsAsynSrc/omsMAXvEncFunc.h @@ -0,0 +1,51 @@ +/* +FILENAME... omsMAXvEncFunc.h +USAGE... Pro-Dex OMS MAXv encoder asyn motor support + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + +/* + * Created on: 10/2010 + * Author: eden + */ + +#ifndef OMSMAXVENCFUNC_H_ +#define OMSMAXVENCFUNC_H_ + +#include "omsMAXv.h" + + +#define ENCFUNCTION_FUNCTMASK 0xFF00 /* 8 function bits */ +#define ENCFUNCTION_ENC_MASK 0x00F0 /* 4 encoder bits (encoder 0 - 15) */ +#define ENCFUNCTION_AXISMASK 0x000F /* 4 axis bits (axis 0 - 15) */ + +#define ENCFUNCTION_NONE 0x00 /* no encoder functions */ +#define ENCFUNCTION_READ 0x01 /* read (auxiliary) encoder */ +#define ENCFUNCTION_REPLACE 0x02 /* replace encoder of axis axismask with encoder encodermask */ +#define ENCFUNCTION_AVERAGE 0x03 /* read (auxiliary) encoder */ + +#define ENCFUNCTION_AUXENC_0 0x0E /* aux encoder 0 is defined as encoder 14) */ +#define ENCFUNCTION_AUXENC_1 0x0F /* aux encoder 1 is defined as encoder 15) */ + +#define MAXENCFUNC 8 + +class omsMAXvEncFunc : public omsMAXv { +public: + omsMAXvEncFunc(const char*, int, int, const char*, int, int, unsigned int, int, int, const char* ); + omsMAXvEncFunc(const char*, int, int, const char*, int, int ); + virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + virtual asynStatus getEncoderPositions(epicsInt32 encPosArr[OMS_MAX_AXES]); + +private: + void initialize(); + int averageChannel[MAXENCFUNC]; + int encFuncIndex[MAXENCFUNC]; + int encRawPosIndex[MAXENCFUNC]; + int encPosIndex[2]; +}; + +#endif /* OMSMAXVENCFUNC_H_ */