From c805385ad12a31455edd9c50a962251dd652c75b Mon Sep 17 00:00:00 2001 From: brambilla_m Date: Mon, 28 Feb 2022 15:33:47 +0100 Subject: [PATCH] Allow enable/disable (tentative) --- sinqEPICSApp/Db/motorSet.db | 7 ++ sinqEPICSApp/src/pmacAxis.cpp | 80 +++++++++-------- sinqEPICSApp/src/pmacAxis.h | 8 +- sinqEPICSApp/src/pmacController.cpp | 129 ++++++++++++++++++++++------ sinqEPICSApp/src/pmacController.h | 18 ++++ 5 files changed, 177 insertions(+), 65 deletions(-) diff --git a/sinqEPICSApp/Db/motorSet.db b/sinqEPICSApp/Db/motorSet.db index 91d7b80..f71ea13 100644 --- a/sinqEPICSApp/Db/motorSet.db +++ b/sinqEPICSApp/Db/motorSet.db @@ -4,3 +4,10 @@ record(ao, "$(P)$(M)-SetPosition") { field(OUT, "@asyn($(PORT),$(N),1) SET_MOTOR_POSITION") field(PINI, "YES") } + +# enable axis +record(longout, "$(P)$(M):Enable") { + field(DTYP, "asynInt32") + field(OUT, "@asyn($(PORT),$(N),1) ENABLE_AXIS") + field(PINI, "YES") +} diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index 4affc4a..7d230a9 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -368,34 +368,6 @@ static char *translateAxisError(int axErr) } } -asynStatus pmacV3Axis::getAxisStatus(bool *moving) -{ - char command[pC_->PMAC_MAXBUF_]; - char response[pC_->PMAC_MAXBUF_]; - int cmdStatus = 0; - int done = 0, posChanging = 0; - double position = 0; - int nvals = 0; - int axisProblemFlag = 0; - epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim = 0; - char message[132], *axMessage; - - /* read our status items one by one: our PMAC does not seem to give multiple - * responses ..*/ - sprintf(command, "P%2.2d01", axisNo_); - cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response); - nvals = sscanf(response, "%d", &axErr); - if (cmdStatus || nvals != 1) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "drvPmacAxisGetStatus: Failed to read axis Error Status: " - "%d\nCommand :%s\nResponse:%s\n", - cmdStatus, command, response); - updateMsgTxtFromDriver("Cannot read Axis Error Status"); - } -} - - - asynStatus pmacAxis::getAxisStatus(bool *moving) { char command[pC_->PMAC_MAXBUF_]; @@ -625,6 +597,16 @@ asynStatus pmacHRPTAxis::getAxisStatus(bool *moving) return result; } +asynStatus pmacAxis::enable(int on) { + static const char *functionName = "pmacAxis::enable"; + + pC_->debugFlow(functionName); + + // Cannot do this. + + return asynSuccess; +} + /*================================= SeleneAxis code ======================================================*/ SeleneAxis::SeleneAxis(SeleneController *pC, int axisNo, double limitTarget) : pmacAxis(pC, axisNo, false) @@ -890,13 +872,13 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { double position = 0; int nvals = 0; int axisProblemFlag = 0; - epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim = 0; + epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim = 0, axDone = 0; char message[132], *axMessage; - sprintf(command, "Q%2.2d10 P%2.2d00", axisNo_, axisNo_); + sprintf(command, "Q%2.2d10 P%2.2d00 P%2.2d23", axisNo_, axisNo_, axisNo_); cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response); - nvals = sscanf(response, "%lf %d", &position, &axStat); - if (cmdStatus || nvals != 2) { + nvals = sscanf(response, "%lf %d %d", &position, &axStat, &axDone); + if (cmdStatus || nvals != 3) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "drvPmacAxisGetStatus: Failed to read position and status, " "Status: %d\nCommand :%s\nResponse:%s\n", @@ -924,7 +906,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { // errlogPrintf("Polling, axStat = %d, position = %f\n", axStat, position); /* are we done? */ - if ((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0) { + if ( axDone == 0 && starting == 0) { done = 1; } else { starting = 0; @@ -992,7 +974,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { } sprintf(message, "poll results: axis %d, status %d, done = %d", - axisNo_, axStat, done); + axisNo_, axStat, axDone); pC_->debugFlow(message); if (*moving == false) { @@ -1064,4 +1046,32 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag); return asynSuccess; -} \ No newline at end of file +} + +// pmacV3Axis::enable(int on) { +// char command[pC_->PMAC_MAXBUF_]; +// char response[pC_->PMAC_MAXBUF_]; +// int cmdStatus = 0; +// static const char *functionName = "pmacV3Axis::enable"; + +// pC_->debugFlow(functionName); + +// // Enable the axis. After startup, the axis are disabled on the controller... +// sprintf(command, "M%2.2d14=%1.1d", axisNo_, on); +// cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response); +// if (cmdStatus) { +// if(on) { +// asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, +// "%s: Error: enabling axis %d failed.\n", functionName, axisNo_); +// } else { +// asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, +// "%s: Error: disabling axis %d failed.\n", functionName, axisNo_); +// } +// return asynError; +// } + +// callParamCallbacks(); + +// return asynSuccess; +// } + diff --git a/sinqEPICSApp/src/pmacAxis.h b/sinqEPICSApp/src/pmacAxis.h index eb9ebe6..c9d684a 100644 --- a/sinqEPICSApp/src/pmacAxis.h +++ b/sinqEPICSApp/src/pmacAxis.h @@ -24,6 +24,8 @@ class pmacController; class SeleneController; +#define PMAC_EnableAxis "PMAC_ENABLE_AXIS" + class pmacAxis : public SINQAxis { public: @@ -36,8 +38,9 @@ class pmacAxis : public SINQAxis asynStatus stop(double acceleration); asynStatus poll(bool *moving); asynStatus setPosition(double position); - - protected: + asynStatus enable(int on); + +protected: pmacController *pC_; asynStatus getAxisStatus(bool *moving); @@ -139,6 +142,7 @@ class LiftAxis : public pmacAxis class pmacV3Axis : public pmacAxis { public: + using pmacAxis::pmacAxis; friend class pmacController; }; diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index ead47fb..1f5b497 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -133,7 +133,10 @@ extern "C" { int numAxes, int movingPollPeriod, int idlePollPeriod); asynStatus SeleneCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, int numAxes, int movingPollPeriod, int idlePollPeriod); - + asynStatus pmacV3CreateController(const char *portName, + const char *lowLevelPortName, + int lowLevelPortAddress, int numAxes, + int movingPollPeriod, int idlePollPeriod); asynStatus pmacCreateAxis(const char *pmacName, int axis); asynStatus pmacCreateAxis(const char *pmacName, int numAxis); @@ -546,6 +549,19 @@ asynStatus SeleneCreateController(const char *portName, const char *lowLevelPort return asynSuccess; } +asynStatus pmacV3CreateController(const char *portName, + const char *lowLevelPortName, + int lowLevelPortAddress, int numAxes, + int movingPollPeriod, int idlePollPeriod) { + + pmacV3Controller *ppmacController = new pmacV3Controller( + portName, lowLevelPortName, lowLevelPortAddress, numAxes, + movingPollPeriod / 1000., idlePollPeriod / 1000.); + ppmacController = NULL; + + return asynSuccess; +} + /*---------------------------------------------------------------------------------------------------------------------*/ SeleneController::SeleneController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, int numAxes, double movingPollPeriod, double idlePollPeriod) : pmacController(portName, @@ -559,33 +575,46 @@ SeleneController::SeleneController(const char *portName, const char *lowLevelPor callParamCallbacks(); } - -/** - * C wrapper for the pmacAxis constructor. - * See pmacAxis::pmacAxis. - * - */ -asynStatus pmacCreateAxis(const char *pmacName, /* specify which controller by port name */ - int axis) /* axis number (start from 1). */ -{ - pmacController *pC; - pmacAxis *pAxis; - - static const char *functionName = "pmacCreateAxis"; - - pC = (pmacController*) findAsynPortDriver(pmacName); - if (!pC) { - printf("%s:%s: Error port %s not found\n", - driverName, functionName, pmacName); - return asynError; + pmacV3Controller::pmacV3Controller(const char *portName, + const char *lowLevelPortName, + int lowLevelPortAddress, int numAxes, + double movingPollPeriod, + double idlePollPeriod, + const int &extraParams){ + pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, + movingPollPeriod, idlePollPeriod, extraParams); + static const char *functionName = "pmacV3Controller::pmacV3Controller"; + createParam(EnableAxisString, asynParamInt32, &enableAxis_); + callParamCallbacks(); + } + + /** + * C wrapper for the pmacAxis constructor. + * See pmacAxis::pmacAxis. + * + */ + asynStatus pmacCreateAxis( + const char *pmacName, /* specify which controller by port name */ + int axis) /* axis number (start from 1). */ + { + pmacController *pC; + pmacAxis *pAxis; + + static const char *functionName = "pmacCreateAxis"; + + pC = (pmacController *)findAsynPortDriver(pmacName); + if (!pC) { + printf("%s:%s: Error port %s not found\n", driverName, functionName, + pmacName); + return asynError; + } + + pC->lock(); + pAxis = new pmacAxis(pC, axis); + pAxis = NULL; + pC->unlock(); + return asynSuccess; } - - pC->lock(); - pAxis = new pmacAxis(pC, axis); - pAxis = NULL; - pC->unlock(); - return asynSuccess; -} /** * C wrapper for the pmacHRPTAxis constructor. * See pmacHRPTAxis::pmacHRPTAxis. @@ -752,6 +781,8 @@ asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 valu status = lowLevelWriteRead(pAxis->axisNo_,command, response); } + // What if now status != asynSuccess + //Call base class method //This will handle callCallbacks even if the function was handled here. status = asynMotorController::writeFloat64(pasynUser, value); @@ -760,6 +791,42 @@ asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 valu } +asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { + int function = pasynUser->reason; + asynStatus status = asynError; + pmacAxis *pAxis = NULL; + static const char *functionName = "pmacV3Controller::writeInt32"; + + debugFlow(functionName); + + pAxis = this->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->setIntegerParam(function, value); + + if (function == enableAxis_) { + sprintf(command, "M%2.2d14=%d\n", pAxis->axisNo_, value); + asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Enable axis on controller %s, axis %d enable=%d\n", + functionName, portName, pAxis->axisNo_, value); + errlogPrintf("Enable axis %d: %d, command = %s\n", pAxis->axisNo_, value, command); + } + + // Execute the command. + if (command[0] != 0 && status == asynSuccess) { + status = lowLevelWriteRead(pAxis->axisNo_, command, response); + } + + // Call base class method + // This will handle callCallbacks even if the function was handled here. + status = asynMotorController::writeInt32(pasynUser, value); + + return status; +} /* Code for iocsh registration */ @@ -790,7 +857,13 @@ static void configSeleneCreateControllerCallFunc(const iocshArgBuf *args) { SeleneCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival); } - +static const iocshFuncDef configpmacV3CreateController = { + "pmacV3CreateController", 6, pmacCreateControllerArgs +}; +static void configpmacV3CreateControllerCallFunc(const iocshArgBuf *args) { + pmacV3CreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, + args[4].ival, args[5].ival); +} /* pmacCreateAxis */ static const iocshArg pmacCreateAxisArg0 = {"Controller port name", iocshArgString}; diff --git a/sinqEPICSApp/src/pmacController.h b/sinqEPICSApp/src/pmacController.h index 0898577..7bf1969 100644 --- a/sinqEPICSApp/src/pmacController.h +++ b/sinqEPICSApp/src/pmacController.h @@ -164,4 +164,22 @@ class SeleneController : public pmacController { }; +#define EnableAxisString "ENABLE_AXIS" + +class pmacV3Controller : public pmacController { +public: + pmacV3Controller(const char *portName, const char *lowLevelPortName, + int lowLevelPortAddress, int numAxes, double movingPollPeriod, + double idlePollPeriod, const int &extraParams = 2); + + // overloaded because we want to enable/disable the motor + asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + + friend class pmacV3Axis; + friend class pmacAxis; + +protected: + int enableAxis_; +}; + #endif /* pmacController_H */