diff --git a/motorApp/MotorSrc/asynMotorAxis.cpp b/motorApp/MotorSrc/asynMotorAxis.cpp index cb73f5c8..78c64dce 100644 --- a/motorApp/MotorSrc/asynMotorAxis.cpp +++ b/motorApp/MotorSrc/asynMotorAxis.cpp @@ -8,6 +8,10 @@ #include #include +#include +using std::endl; +using std::cout; + #include #include @@ -49,6 +53,11 @@ asynMotorAxis::asynMotorAxis(class asynMotorController *pC, int axisNo) profileReadbacks_ = NULL; profileFollowingErrors_ = NULL; + /* Used to keep track of referencing mode in the driver.*/ + referencingMode_ = 0; + /* Used to enable/disable move to home, and to tell driver how far to move.*/ + referencingModeMove_ = 0; + // Create the asynUser, connect to this axis pasynUser_ = pasynManager->createAsynUser(NULL, NULL); pasynManager->connectDevice(pasynUser_, pC->portName, axisNo); @@ -113,6 +122,33 @@ asynStatus asynMotorAxis::poll(bool *moving) return asynError; } +/** + * Default implementation of doMoveToHome. + * Derived classes need to implement this to actually perform the + * axis move to the home position. + */ +asynStatus asynMotorAxis::doMoveToHome() +{ + cout << "Dummy implementation of asynMotorAxis::doMoveToHome. Axis: " << pC_->moveToHomeAxis_ << endl; + return asynError; +} + +/** + * Set method for referencingModeMove_ + */ +void asynMotorAxis::setReferencingModeMove(int distance) +{ + referencingModeMove_ = distance; +} + +/** + * Get method for referencingModeMove_ + */ +int asynMotorAxis::getReferencingModeMove() +{ + return referencingModeMove_; +} + /** Set the current position of the motor. diff --git a/motorApp/MotorSrc/asynMotorAxis.h b/motorApp/MotorSrc/asynMotorAxis.h index 32167582..ba862303 100644 --- a/motorApp/MotorSrc/asynMotorAxis.h +++ b/motorApp/MotorSrc/asynMotorAxis.h @@ -32,6 +32,7 @@ class epicsShareFunc asynMotorAxis { virtual asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards); virtual asynStatus stop(double acceleration); virtual asynStatus poll(bool *moving); + virtual asynStatus doMoveToHome(); virtual asynStatus setPosition(double position); virtual asynStatus initializeProfile(size_t maxPoints); @@ -41,6 +42,9 @@ class epicsShareFunc asynMotorAxis { virtual asynStatus abortProfile(); virtual asynStatus readbackProfile(); + void setReferencingModeMove(int distance); + int getReferencingModeMove(); + protected: class asynMotorController *pC_; /**< Pointer to the asynMotorController to which this axis belongs. * Abbreviated because it is used very frequently */ @@ -49,10 +53,12 @@ class epicsShareFunc asynMotorAxis { double *profilePositions_; /**< Array of target positions for profile moves */ double *profileReadbacks_; /**< Array of readback positions for profile moves */ double *profileFollowingErrors_; /**< Array of following errors for profile moves */ + int referencingMode_; private: MotorStatus status_; int statusChanged_; + int referencingModeMove_; friend class asynMotorController; }; diff --git a/motorApp/MotorSrc/asynMotorController.cpp b/motorApp/MotorSrc/asynMotorController.cpp index d1370b41..ac1126f0 100644 --- a/motorApp/MotorSrc/asynMotorController.cpp +++ b/motorApp/MotorSrc/asynMotorController.cpp @@ -8,7 +8,13 @@ #include #include +#include +using std::endl; +using std::cout; + #include +#include +#include #include #define epicsExportSharedSymbols @@ -18,6 +24,8 @@ static const char *driverName = "asynMotorController"; static void asynMotorPollerC(void *drvPvt); +static void asynMotorMoveToHomeC(void *drvPvt); + /** Creates a new asynMotorController object. * All of the arguments are simply passed to the constructor for the asynPortDriver base class. @@ -49,6 +57,7 @@ asynMotorController::asynMotorController(const char *portName, int numAxes, int createParam(motorPositionString, asynParamFloat64, &motorPosition_); createParam(motorEncoderPositionString, asynParamFloat64, &motorEncoderPosition_); createParam(motorDeferMovesString, asynParamInt32, &motorDeferMoves_); + createParam(motorMoveToHomeString, asynParamInt32, &motorMoveToHome_); createParam(motorResolutionString, asynParamFloat64, &motorResolution_); createParam(motorEncRatioString, asynParamFloat64, &motorEncRatio_); createParam(motorPgainString, asynParamFloat64, &motorPgain_); @@ -113,11 +122,14 @@ asynMotorController::asynMotorController(const char *portName, int numAxes, int pAxes_ = (asynMotorAxis**) calloc(numAxes, sizeof(asynMotorAxis*)); pollEventId_ = epicsEventMustCreate(epicsEventEmpty); + moveToHomeId_ = epicsEventMustCreate(epicsEventEmpty); maxProfilePoints_ = 0; profileTimes_ = NULL; setIntegerParam(profileExecuteState_, PROFILE_EXECUTE_DONE); + moveToHomeAxis_ = 0; + asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, "%s:%s: constructor complete\n", driverName, functionName); @@ -168,6 +180,13 @@ asynStatus asynMotorController::writeInt32(asynUser *pasynUser, epicsInt32 value status = abortProfile(); } else if (function == profileReadback_) { status = readbackProfile(); + } else if (function == motorMoveToHome_) { + if (value == 1) { + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s:: Starting a move to home for axis %d\n", driverName, functionName, axis); + moveToHomeAxis_ = axis; + epicsEventSignal(moveToHomeId_); + } } /* Do callbacks so higher layers see any changes */ @@ -418,6 +437,7 @@ asynStatus asynMotorController::startPoller(double movingPollPeriod, double idle return asynSuccess; } + /** Wakes up the poller thread to make it start polling at the movingPollingPeriod_. * This is typically called after an axis has been told to move, so the poller immediately * starts polling quickly. */ @@ -502,6 +522,55 @@ void asynMotorController::asynMotorPoller() } } +/** + * Start the thread which deals with moving axes to their home position. + * This is called by the derived concrete controller class at object instatiation, so + * that drivers that don't need this functionality don't have the overhead of the thread. + */ +asynStatus asynMotorController::startMoveToHomeThread() +{ + epicsThreadCreate("motorMoveToHome", + epicsThreadPriorityMedium, + epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC)asynMotorMoveToHomeC, (void *)this); + return asynSuccess; +} + +static void asynMotorMoveToHomeC(void *drvPvt) +{ + asynMotorController *pController = (asynMotorController*)drvPvt; + pController->asynMotorMoveToHome(); +} + + + +/** + * Default move to home thread. Not normally overridden. + */ +void asynMotorController::asynMotorMoveToHome() +{ + + asynMotorAxis *pAxis; + int status = 0; + static const char *functionName = "asynMotorMoveToHome"; + + while(1) { + status = epicsEventWait(moveToHomeId_); + if (status == epicsEventWaitOK) { + pAxis = getAxis(this->moveToHomeAxis_); + if (!pAxis) continue; + status = pAxis->doMoveToHome(); + if (status) { + asynPrint(pasynUserSelf, ASYN_TRACE_ERROR, + "%s:%s: move to home failed in asynMotorController::asynMotorMoveToHome. Axis number=%d\n", + driverName, functionName, this->moveToHomeAxis_); + } + } + } +} + + + /* These are the functions for profile moves */ /** Initialize a profile move of multiple axes. */ asynStatus asynMotorController::initializeProfile(size_t maxProfilePoints) @@ -589,3 +658,61 @@ asynStatus asynMotorController::readbackProfile() } return asynSuccess; } + + + +/** The following functions have C linkage, and can be called directly or from iocsh */ + +extern "C" { + + +asynStatus asynMotorEnableMoveToHome(const char *portName, int axis, int distance) +{ + asynMotorController *pC = NULL; + asynMotorAxis *pA = NULL; + static const char *functionName = "asynMotorEnableMoveToHome"; + + pC = (asynMotorController*) findAsynPortDriver(portName); + if (!pC) { + cout << driverName << "::" << functionName << " Error port " << portName << " not found." << endl; + return asynError; + } + + pA = pC->getAxis(axis); + if (!pA) { + cout << driverName << "::" << functionName << " Error axis " << axis << " not found." << endl; + return asynError; + } + + if (distance<=0) { + cout << "Error in asynMotorEnableMoveToHome. distance must be positive integer." << endl; + } else { + pA->setReferencingModeMove(distance); + } + + return asynSuccess; +} + + +/* asynMotorEnableMoveToHome */ +static const iocshArg asynMotorEnableMoveToHomeArg0 = {"Controller port name", iocshArgString}; +static const iocshArg asynMotorEnableMoveToHomeArg1 = {"Axis number", iocshArgInt}; +static const iocshArg asynMotorEnableMoveToHomeArg2 = {"Distance", iocshArgInt}; +static const iocshArg * const asynMotorEnableMoveToHomeArgs[] = {&asynMotorEnableMoveToHomeArg0, + &asynMotorEnableMoveToHomeArg1, + &asynMotorEnableMoveToHomeArg2}; +static const iocshFuncDef enableMoveToHome = {"asynMotorEnableMoveToHome", 3, asynMotorEnableMoveToHomeArgs}; + +static void enableMoveToHomeCallFunc(const iocshArgBuf *args) +{ + asynMotorEnableMoveToHome(args[0].sval, args[1].ival, args[2].ival); +} + + +static void asynMotorControllerRegister(void) +{ + iocshRegister(&enableMoveToHome, enableMoveToHomeCallFunc); +} +epicsExportRegistrar(asynMotorControllerRegister); + +} //extern C diff --git a/motorApp/MotorSrc/asynMotorController.h b/motorApp/MotorSrc/asynMotorController.h index 7b708e55..8e8e2ba0 100644 --- a/motorApp/MotorSrc/asynMotorController.h +++ b/motorApp/MotorSrc/asynMotorController.h @@ -158,6 +158,10 @@ class epicsShareFunc asynMotorController : public asynPortDriver { virtual asynStatus poll(); void asynMotorPoller(); // This should be private but is called from C function + /* Functions to deal with moveToHome.*/ + virtual asynStatus startMoveToHomeThread(); + void asynMotorMoveToHome(); + /* These are the functions for profile moves */ virtual asynStatus initializeProfile(size_t maxPoints); virtual asynStatus buildProfile(); @@ -183,6 +187,7 @@ class epicsShareFunc asynMotorController : public asynPortDriver { int motorPosition_; int motorEncoderPosition_; int motorDeferMoves_; + int motorMoveToHome_; int motorResolution_; int motorEncRatio_; int motorPgain_; @@ -251,6 +256,7 @@ class epicsShareFunc asynMotorController : public asynPortDriver { int numAxes_; /**< Number of axes this controller supports */ asynMotorAxis **pAxes_; /**< Array of pointers to axis objects */ epicsEventId pollEventId_; /**< Event ID to wake up poller */ + epicsEventId moveToHomeId_; /**< Event ID to wake up move to home thread */ double idlePollPeriod_; /**< The time between polls when no axes are moving */ double movingPollPeriod_; /**< The time between polls when any axis is moving */ int forcedFastPolls_; /**< The number of forced fast polls when the poller wakes up */ @@ -258,6 +264,8 @@ class epicsShareFunc asynMotorController : public asynPortDriver { size_t maxProfilePoints_; /**< Maximum number of profile points */ double *profileTimes_; /**< Array of times per profile point */ + int moveToHomeAxis_; + friend class asynMotorAxis; }; #define NUM_MOTOR_DRIVER_PARAMS (&LAST_MOTOR_PARAM - &FIRST_MOTOR_PARAM + 1) diff --git a/motorApp/MotorSrc/motorSupport.dbd b/motorApp/MotorSrc/motorSupport.dbd index b8bc7d3b..7cf40812 100644 --- a/motorApp/MotorSrc/motorSupport.dbd +++ b/motorApp/MotorSrc/motorSupport.dbd @@ -4,5 +4,6 @@ registrar(motorUtilRegister) #variable(motordrvComdebug) #variable(motorUtil_debug) registrar(motorRegister) +registrar(asynMotorControllerRegister) device(motor,INST_IO,devMotorAsyn,"asynMotor")