diff --git a/Makefile b/Makefile index 3553656..013243f 100644 --- a/Makefile +++ b/Makefile @@ -13,16 +13,18 @@ REQUIRED+=motorBase motorBase_VERSION=7.2.2 # Specify the version of sinqMotor we want to build against -sinqMotor_VERSION=mathis_s +sinqMotor_VERSION=0.8.0 # Specify the version of turboPmac we want to build against -turboPmac_VERSION=mathis_s +turboPmac_VERSION=0.7.0 # These headers allow to depend on this library for derived drivers. +HEADERS += src/offsetAxis.h HEADERS += src/detectorTowerAxis.h HEADERS += src/detectorTowerController.h # Source files to build +SOURCES += src/offsetAxis.cpp SOURCES += src/detectorTowerAxis.cpp SOURCES += src/detectorTowerController.cpp diff --git a/db/detectorTower.db b/db/detectorTower.db index 084994f..0a98bb2 100644 --- a/db/detectorTower.db +++ b/db/detectorTower.db @@ -6,7 +6,7 @@ record(longout, "$(INSTR)$(M):ResetError") { field(PINI, "NO") } -# Start the movement into the position defined by BeamAngle, LiftOffset and TiltOffset +# Start the movement into the position defined by DetectorTower, LiftOffset and TiltOffset record(longout, "$(INSTR)$(M):MoveToWorkingPosition") { field(DTYP, "asynInt32") field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) MOVE_TO_WORKING_POSITION") @@ -23,112 +23,4 @@ record(longout, "$(INSTR)$(M):PositionState") field(DTYP, "asynInt32") field(OUT, "@asyn($(CONTROLLER),$(AXIS)) POSITION_STATE") field(SCAN, "I/O Intr") -} - -# Start the movement into the position defined by BeamAngle, LiftOffset and TiltOffset -record(longout, "$(INSTR)$(M):Start") { - field(DTYP, "asynInt32") - field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) START") - field(PINI, "NO") -} - -# Set the target angle for the beam -record(ao, "$(INSTR)$(M):BeamAngle") { - field(DTYP, "asynFloat64") - field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) BEAM_ANGLE") - field(PINI, "NO") -} - -# Copy the high and low limits from the motor record -record(ao, "$(INSTR)$(M):DHLM2BeamAngle_RBV") { - field(DOL, "$(INSTR)$(M).DHLM CP") - field(OUT, "$(INSTR)$(M):BeamAngle.DRVH") - field(OMSL, "closed_loop") -} -record(ao, "$(INSTR)$(M):DLLM2BeamAngle_RBV") { - field(DOL, "$(INSTR)$(M).DLLM CP") - field(OUT, "$(INSTR)$(M):BeamAngle.DRVL") - field(OMSL, "closed_loop") -} - -# Set the target position for the offset of the lift (motor COZ) -# This record is coupled to the parameter library via liftOffset_ -> LIFT_OFFSET. -record(ao, "$(INSTR)$(M):LiftOffset") { - field(DTYP, "asynFloat64") - field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) LIFT_OFFSET") - field(PINI, "NO") - field(FLNK, "$(INSTR)$(M):LiftOffsetFwd") -} - -# Like $(INSTR)$(M):LiftOffset, but starts a movement immediately, if able -record(ao, "$(INSTR)$(M):LiftOffsetMove") { - field(DTYP, "asynFloat64") - field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) LIFT_OFFSET_MOVE") - field(PINI, "NO") -} - -# Read the lower and upper limits for the lift offset -record(ai, "$(INSTR)$(M):LiftOffsetLowLimitRBV") -{ - field(DTYP, "asynFloat64") - field(INP, "@asyn($(CONTROLLER),$(AXIS)) LIFT_OFFSET_LOW_LIMIT") - field(SCAN, "I/O Intr") -} -record(ao, "$(INSTR)$(M):LiftOffsetLowLimit2Field") { - field(DOL, "$(INSTR)$(M):LiftOffsetLowLimitRBV CP") - field(OUT, "$(INSTR)$(M):LiftOffset.DRVL") - field(OMSL, "closed_loop") -} - -record(ai, "$(INSTR)$(M):LiftOffsetHighLimitRBV") -{ - field(DTYP, "asynFloat64") - field(INP, "@asyn($(CONTROLLER),$(AXIS)) LIFT_OFFSET_HIGH_LIMIT") - field(SCAN, "I/O Intr") -} -record(ao, "$(INSTR)$(M):LiftOffsetHighLimit2Field") { - field(DOL, "$(INSTR)$(M):LiftOffsetHighLimitRBV CP") - field(OUT, "$(INSTR)$(M):LiftOffset.DRVH") - field(OMSL, "closed_loop") -} - -# Set the target position for the offset of the detector tilt (motor COX) -# This record is coupled to the parameter library via tiltOffset_ -> TILT_OFFSET. -record(ao, "$(INSTR)$(M):TiltOffset") { - field(DTYP, "asynFloat64") - field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) TILT_OFFSET") - field(PINI, "NO") -} - -# Like $(INSTR)$(M):TiltOffset, but starts a movement immediately, if able -record(ao, "$(INSTR)$(M):TiltOffsetMove") { - field(DTYP, "asynFloat64") - field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) TILT_OFFSET") - field(PINI, "NO") - field(FLNK, "$(INSTR)$(M):Start") -} - -# Read the lower and upper limits for the detector tilt offset -record(ai, "$(INSTR)$(M):TiltOffsetLowLimitRBV") -{ - field(DTYP, "asynFloat64") - field(INP, "@asyn($(CONTROLLER),$(AXIS)) TILT_OFFSET_LOW_LIMIT") - field(SCAN, "I/O Intr") -} -record(ao, "$(INSTR)$(M):TiltOffsetLowLimit2Field") { - field(DOL, "$(INSTR)$(M):TiltOffsetLowLimitRBV CP") - field(OUT, "$(INSTR)$(M):TiltOffset.DRVL") - field(OMSL, "closed_loop") -} - -record(ai, "$(INSTR)$(M):TiltOffsetHighLimitRBV") -{ - field(DTYP, "asynFloat64") - field(INP, "@asyn($(CONTROLLER),$(AXIS)) TILT_OFFSET_HIGH_LIMIT") - field(SCAN, "I/O Intr") -} -record(ao, "$(INSTR)$(M):TiltOffsetHighLimit2Field") { - field(DOL, "$(INSTR)$(M):TiltOffsetHighLimitRBV CP") - field(OUT, "$(INSTR)$(M):TiltOffset.DRVH") - field(OMSL, "closed_loop") -} +} \ No newline at end of file diff --git a/src/detectorTowerAxis.cpp b/src/detectorTowerAxis.cpp index 6a35b92..0e73e49 100644 --- a/src/detectorTowerAxis.cpp +++ b/src/detectorTowerAxis.cpp @@ -4,10 +4,11 @@ #include #include #include +#include /* -Contains all instances of turboPmacAxis which have been created and is used in -the initialization hook function. +Contains all instances of detectorTowerAxis which have been created and is used +in the initialization hook function. */ static std::vector axes; @@ -28,7 +29,24 @@ static void epicsInithookFunction(initHookState iState) { } } -detectorTowerAxis::detectorTowerAxis(detectorTowerController *pC, int axisNo) +static void deferredMovementCollectorLoop(void *drvPvt) { + detectorTowerAxis *axis = (detectorTowerAxis *)drvPvt; + while (1) { + if (axis->receivedTarget_) { + // Wait for 50 ms and then start the movement with the information + // available + epicsThreadSleep(0.05); + axis->startCombinedMove(); + + // After the movement command has been send, reset the flag + axis->receivedTarget_ = false; + } + } +} + +detectorTowerAxis::detectorTowerAxis(detectorTowerController *pC, int axisNo, + offsetAxis *liftOffsetAxis, + offsetAxis *tiltOffsetAxis) : turboPmacAxis(pC, axisNo, false), pC_(pC) { /* @@ -57,7 +75,12 @@ detectorTowerAxis::detectorTowerAxis(detectorTowerController *pC, int axisNo) // Initialize all member variables // Assumed to be not ready by default, this is overwritten in the next poll error_ = 0; - scheduleMoveFromParamLib_ = false; + targetPosition_ = 0.0; + receivedTarget_ = false; + liftOffsetAxis_ = liftOffsetAxis; + tiltOffsetAxis_ = tiltOffsetAxis; + liftOffsetAxis->dTA_ = this; + tiltOffsetAxis->dTA_ = this; // Register the hook function during construction of the first axis object if (axes.empty()) { @@ -67,6 +90,16 @@ detectorTowerAxis::detectorTowerAxis(detectorTowerController *pC, int axisNo) // Collect all axes into this list which will be used in the hook // function axes.push_back(this); + + // Create a thread which collects all movement commands send to components + // of the virtual axis. After a component received a new target position, + // it forwards this information to the thread. The thread then waits a short + // time to see if other components also received new targets and starts the + // movement with all targets afterwards. + epicsThreadCreate("deferredMovement", epicsThreadPriorityLow, + epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC)deferredMovementCollectorLoop, + (void *)this); } detectorTowerAxis::~detectorTowerAxis(void) { @@ -90,7 +123,8 @@ asynStatus detectorTowerAxis::init() { if (status == asynParamUndefined) { if (now + maxInitTime < time(NULL)) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line " + "Controller \"%s\", axis " + "%ddeferredMovementCollectorLoop => %s, line " "%d\nInitializing the parameter library failed.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); @@ -105,40 +139,6 @@ asynStatus detectorTowerAxis::init() { } } - // Initialize some values in the parameter library - status = pC_->setDoubleParam(axisNo_, pC_->liftOffset_, 0.0); - if (status != asynSuccess) { - asynPrint( - pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nFATAL ERROR " - "(setting a parameter value failed with %s)\n. Terminating IOC", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->stringifyAsynStatus(status)); - exit(-1); - } - status = pC_->setDoubleParam(axisNo_, pC_->tiltOffset_, 0.0); - if (status != asynSuccess) { - asynPrint( - pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nFATAL ERROR " - "(setting a parameter value failed with %s)\n. Terminating IOC", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->stringifyAsynStatus(status)); - exit(-1); - } - - // Initialize some values in the parameter library - status = pC_->setIntegerParam(axisNo_, pC_->motorCanSetSpeed_, 0); - if (status != asynSuccess) { - asynPrint( - pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nFATAL ERROR " - "(setting a parameter value failed with %s)\n. Terminating IOC", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->stringifyAsynStatus(status)); - exit(-1); - } - return status; } @@ -167,6 +167,8 @@ asynStatus detectorTowerAxis::doPoll(bool *moving) { double limitsOffset = 0.0; double highLimit = 0.0; double lowLimit = 0.0; + double detectorHeight = 0.0; + double detectorRadius = 0.0; double liftOffsetHighLimit = 0.0; double liftOffsetLowLimit = 0.0; double tiltOffsetHighLimit = 0.0; @@ -205,7 +207,7 @@ asynStatus detectorTowerAxis::doPoll(bool *moving) { - Beam lift offset limits - Detector tilt offset limits */ - char command[] = "P354 P358 P359 Q510 Q513 Q514 Q353 Q354 Q553 Q554"; + const char *command = "P354 P358 P359 Q510 Q513 Q514 Q353 Q354 Q553 Q554"; rw_status = pC_->writeRead(axisNo_, command, response, 10); if (rw_status != asynSuccess) { return rw_status; @@ -688,47 +690,66 @@ asynStatus detectorTowerAxis::doPoll(bool *moving) { pl_status = pC_->setDoubleParam(axisNo_, pC_->motorLowLimitFromDriver_, lowLimit); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); + return pC_->paramLibAccessFailed(pl_status, "motorLowLimitFromDriver_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); } - pl_status = pC_->setDoubleParam(axisNo_, pC_->liftOffsetHighLimit_, + // Set the values for the offset axes + int liftAxisNo = liftOffsetAxis_->axisNo_; + double liftOffsetPosition = + detectorHeight + detectorRadius * sin(beamTiltAngle); + + pl_status = pC_->setDoubleParam(liftAxisNo, pC_->motorPosition_, + liftOffsetPosition / motorRecResolution); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", + liftAxisNo, __PRETTY_FUNCTION__, + __LINE__); + } + pl_status = pC_->setDoubleParam(liftAxisNo, pC_->motorHighLimitFromDriver_, liftOffsetHighLimit); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "liftOffsetHighLimit_", - axisNo_, __PRETTY_FUNCTION__, + return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_", + liftAxisNo, __PRETTY_FUNCTION__, __LINE__); } - - pl_status = pC_->setDoubleParam(axisNo_, pC_->liftOffsetLowLimit_, + pl_status = pC_->setDoubleParam(liftAxisNo, pC_->motorLowLimitFromDriver_, liftOffsetLowLimit); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "liftOffsetLowLimit_", - axisNo_, __PRETTY_FUNCTION__, + return pC_->paramLibAccessFailed(pl_status, "motorLowLimitFromDriver_", + liftAxisNo, __PRETTY_FUNCTION__, __LINE__); } - pl_status = pC_->setDoubleParam(axisNo_, pC_->tiltOffsetHighLimit_, + int tiltAxisNo = tiltOffsetAxis_->axisNo_; + pl_status = pC_->setDoubleParam(tiltAxisNo, pC_->motorPosition_, + tiltOffsetAxis_->targetPosition_ / + motorRecResolution); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", + liftAxisNo, __PRETTY_FUNCTION__, + __LINE__); + } + pl_status = pC_->setDoubleParam(tiltAxisNo, pC_->motorHighLimitFromDriver_, tiltOffsetHighLimit); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "tiltOffsetHighLimit_", - axisNo_, __PRETTY_FUNCTION__, + return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_", + tiltAxisNo, __PRETTY_FUNCTION__, __LINE__); } - - pl_status = pC_->setDoubleParam(axisNo_, pC_->tiltOffsetLowLimit_, + pl_status = pC_->setDoubleParam(tiltAxisNo, pC_->motorLowLimitFromDriver_, tiltOffsetLowLimit); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "tiltOffsetLowLimit_", - axisNo_, __PRETTY_FUNCTION__, + return pC_->paramLibAccessFailed(pl_status, "motorLowLimitFromDriver_", + tiltAxisNo, __PRETTY_FUNCTION__, __LINE__); } // Transform from motor to EPICS coordinates (see comment in // turboPmacAxis::init()) - beamTiltAngle = beamTiltAngle / motorRecResolution; - - pl_status = setDoubleParam(pC_->motorPosition_, beamTiltAngle); + pl_status = + setDoubleParam(pC_->motorPosition_, beamTiltAngle / motorRecResolution); if (pl_status != asynSuccess) { return pC_->paramLibAccessFailed(pl_status, "motorPosition_", axisNo_, __PRETTY_FUNCTION__, __LINE__); @@ -740,53 +761,30 @@ asynStatus detectorTowerAxis::doPoll(bool *moving) { __PRETTY_FUNCTION__, __LINE__); } - // If a movement is scheduled and the axis is not moving anymore, start the - // scheduled movement. This replaces the original motor record movement - // control loop functionality. - if (scheduleMoveFromParamLib_ && !(*moving) && poll_status == asynSuccess) { - scheduleMoveFromParamLib_ = false; - - // Update the parameter library immediately - pl_status = callParamCallbacks(); - - if (pC_->msgPrintControl_.shouldBePrinted( - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pl_status != asynSuccess, pC_->pasynUserSelf)) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line " - "%d:\ncallParamCallbacks failed with %s.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->stringifyAsynStatus(poll_status), - pC_->msgPrintControl_.getSuffix()); - } - if (pl_status != asynSuccess) { - return poll_status; - } - - return moveFromParamLib(); - } else { - return poll_status; - } + return poll_status; } -asynStatus detectorTowerAxis::move(double position, int relative, - double min_velocity, double max_velocity, - double acceleration) { - - // Status of parameter library operations - asynStatus pl_status = asynSuccess; - - // Set the position as beam angle - pl_status = pC_->setDoubleParam(axisNo_, pC_->beamAngle_, position); +asynStatus detectorTowerAxis::doMove(double position, int relative, + double min_velocity, double max_velocity, + double acceleration) { + double motorRecResolution = 0.0; + asynStatus pl_status = pC_->getDoubleParam( + axisNo_, pC_->motorRecResolution_, &motorRecResolution); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "beamAngle_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); + return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); } - return moveFromParamLib(); + // Signal to the deferredMovementCollectorLoop that a movement should be + // started to the defined target position. + targetPosition_ = position * motorRecResolution; + receivedTarget_ = true; + + return asynSuccess; } -asynStatus detectorTowerAxis::moveFromParamLib() { +asynStatus detectorTowerAxis::startCombinedMove() { // Status of read-write-operations of ASCII commands to the controller asynStatus rw_status = asynSuccess; @@ -796,15 +794,23 @@ asynStatus detectorTowerAxis::moveFromParamLib() { char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; double motorCoordinatesPosition = 0.0; - double motorRecResolution = 0.0; - double beamAngle = 0.0; - double liftOffset = 0.0; - double tiltOffset = 0.0; - int writeOffset = 0; int positionState = 0; + int moving = 0; // ========================================================================= + pl_status = pC_->getIntegerParam(axisNo_, pC_->motorStatusMoving_, &moving); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + // Check if the axis is already moving. In this case, do nothing + if (moving) { + return asynSuccess; + } + pl_status = pC_->getIntegerParam(axisNo_, pC_->positionState_, &positionState); if (pl_status != asynSuccess) { @@ -830,38 +836,24 @@ asynStatus detectorTowerAxis::moveFromParamLib() { return asynError; } - // Read the target values from the parameter library - pl_status = pC_->getDoubleParam(axisNo_, pC_->beamAngle_, &beamAngle); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "beamAngle_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } - pl_status = pC_->getDoubleParam(axisNo_, pC_->liftOffset_, &liftOffset); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "liftOffset_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } - pl_status = pC_->getDoubleParam(axisNo_, pC_->tiltOffset_, &tiltOffset); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "tiltOffset_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } - pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, - &motorRecResolution); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - // Set the target positions for beam tilt, detector tilt offset and lift // offset - snprintf(&command[writeOffset], sizeof(command) - writeOffset, - "Q451=%lf Q454=%lf Q456=%lf P350=1", beamAngle, liftOffset, - tiltOffset); + snprintf(command, sizeof(command), "Q451=%lf Q454=%lf Q456=%lf P350=1", + targetPosition_, liftOffsetAxis_->targetPosition_, + tiltOffsetAxis_->targetPosition_); + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", command); + + // Lock the access to the controller since this function runs in another + // thread than the poll method. + pC_->lock(); // We don't expect an answer rw_status = pC_->writeRead(axisNo_, command, response, 0); + + // Free the controller again + pC_->unlock(); + if (rw_status != asynSuccess) { asynPrint( pC_->pasynUserSelf, ASYN_TRACE_ERROR, @@ -877,9 +869,6 @@ asynStatus detectorTowerAxis::moveFromParamLib() { } } - // Start polling immediately - pC_->wakeupPoller(); - return rw_status; } @@ -919,7 +908,6 @@ asynStatus detectorTowerAxis::stop(double acceleration) { // The detector tower axis uses absolute encoders asynStatus detectorTowerAxis::readEncoderType() { asynStatus status = setStringParam(pC_->encoderType_, AbsoluteEncoder); - if (status != asynSuccess) { return pC_->paramLibAccessFailed(status, "encoderType_", axisNo_, __PRETTY_FUNCTION__, __LINE__); @@ -1056,7 +1044,8 @@ extern "C" { C wrapper for the axis constructor. Please refer to the detectorTower constructor documentation. The controller is read from the portName. */ -asynStatus detectorTowerCreateAxis(const char *portName, int axis) { +asynStatus detectorTowerCreateAxis(const char *portName, int axisDetectorTower, + int liftOffsetaxisNo, int tiltOffsetaxisNo) { /* findAsynPortDriver is a asyn library FFI function which uses the C ABI. @@ -1074,8 +1063,7 @@ asynStatus detectorTowerCreateAxis(const char *portName, int axis) { /* We can't use asynPrint here since this macro would require us to get a lowLevelPortUser_ from a pointer to an asynPortDriver. - However, the given pointer is a nullptr and therefore doesn't - have a lowLevelPortUser_! printf is an EPICS alternative which + However, the given pointer is a nullptr and therefore doesn'taxis works w/o that, but doesn't offer the comfort provided by the asynTrace-facility */ @@ -1095,6 +1083,16 @@ asynStatus detectorTowerCreateAxis(const char *portName, int axis) { return asynError; } + // Assert that the three indices are different from each other + if (axisDetectorTower == liftOffsetaxisNo || + axisDetectorTower == tiltOffsetaxisNo || + tiltOffsetaxisNo == liftOffsetaxisNo) { + errlogPrintf("Controller \"%s\" => %s, line %d\nAll three axis indices " + "must be unique.", + portName, __PRETTY_FUNCTION__, __LINE__); + return asynError; + } + // Prevent manipulation of the controller from other threads while we // create the new axis. pC->lock(); @@ -1108,9 +1106,14 @@ asynStatus detectorTowerCreateAxis(const char *portName, int axis) { The created object is registered in EPICS in its constructor and can safely be "leaked" here. */ + + offsetAxis *liftOffsetAxis = new offsetAxis(pC, liftOffsetaxisNo); + offsetAxis *tiltOffsetAxis = new offsetAxis(pC, tiltOffsetaxisNo); + #pragma GCC diagnostic ignored "-Wunused-but-set-variable" #pragma GCC diagnostic ignored "-Wunused-variable" - detectorTowerAxis *pAxis = new detectorTowerAxis(pC, axis); + detectorTowerAxis *pAxis = new detectorTowerAxis( + pC, axisDetectorTower, liftOffsetAxis, tiltOffsetAxis); // Allow manipulation of the controller again pC->unlock(); @@ -1123,13 +1126,23 @@ itself. */ static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mcu1)", iocshArgString}; -static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt}; -static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0, - &CreateAxisArg1}; +static const iocshArg CreateAxisArg1 = {"Number of the detector angle axis", + iocshArgInt}; +static const iocshArg CreateAxisArg2 = {"Number of the lift offset axis", + iocshArgInt}; +static const iocshArg CreateAxisArg3 = {"Number of the tilt offset axis", + iocshArgInt}; +static const iocshArg *const CreateAxisArgs[] = { + &CreateAxisArg0, + &CreateAxisArg1, + &CreateAxisArg2, + &CreateAxisArg3, +}; static const iocshFuncDef configDetectorTowerCreateAxis = {"detectorTowerAxis", - 2, CreateAxisArgs}; + 4, CreateAxisArgs}; static void configDetectorTowerCreateAxisCallFunc(const iocshArgBuf *args) { - detectorTowerCreateAxis(args[0].sval, args[1].ival); + detectorTowerCreateAxis(args[0].sval, args[1].ival, args[2].ival, + args[3].ival); } // This function is made known to EPICS in detectorTower.dbd and is called by diff --git a/src/detectorTowerAxis.h b/src/detectorTowerAxis.h index 1cce70c..86dfab4 100644 --- a/src/detectorTowerAxis.h +++ b/src/detectorTowerAxis.h @@ -1,7 +1,7 @@ #ifndef detectorTowerAxis_H #define detectorTowerAxis_H +#include "offsetAxis.h" #include "turboPmacAxis.h" -#include // Forward declaration of the controller class to resolve the cyclic dependency // between the controller and the axis .h-file. See @@ -16,7 +16,8 @@ class detectorTowerAxis : public turboPmacAxis { * @param pController Pointer to the associated controller * @param axisNo Index of the axis */ - detectorTowerAxis(detectorTowerController *pController, int axisNo); + detectorTowerAxis(detectorTowerController *pController, int axisNo, + offsetAxis *liftOffsetAxis, offsetAxis *tiltOffsetAxis); /** * @brief Destroy the turboPmacAxis @@ -50,16 +51,10 @@ class detectorTowerAxis : public turboPmacAxis { asynStatus doPoll(bool *moving); /** - * @brief Empty dummy for the move method which is called by the EPICS - * movement control loop + * @brief Set the target value for the detector angle without starting a + * movement * - * For the detector tower, we want to start movements manually without - * using the EPICS control loop. Therefore, we call the `move` function of - * sinqMotor directly whenever a new value is written to the PV - * $(INSTR)$(M):TargetPosition". The reason is that we want to start a - * movement even if the VAL field of the motor record equals its RBV field - * because either the lift offset or the detector tilt offset might have - * changed. + * TODO * * @param position * @param relative @@ -68,20 +63,19 @@ class detectorTowerAxis : public turboPmacAxis { * @param acceleration * @return asynStatus */ - asynStatus move(double position, int relative, double min_velocity, - double max_velocity, double acceleration); + asynStatus doMove(double position, int relative, double min_velocity, + double max_velocity, double acceleration); /** - * @brief Start a movement based on the parameter library values for - * motorTargetPosition_, liftOffset_ and tiltOffset_ + * @brief Start a movement to the target positions of this axis and the + * attached offset axes. * * @return asynStatus */ - asynStatus moveFromParamLib(); + asynStatus startCombinedMove(); /** - * @brief Read the encoder type (incremental or absolute) for this axis from - * the MCU and store the information in the PV ENCODER_TYPE. + * @brief This axis type has an absolute encoder by default * * @return asynStatus */ @@ -103,14 +97,20 @@ class detectorTowerAxis : public turboPmacAxis { */ asynStatus reset(); + // If true, either this axis or one of the offsetAxis attached to it + // received a movement command. + bool receivedTarget_; + protected: detectorTowerController *pC_; - - bool scheduleMoveFromParamLib_; + double targetPosition_; int error_; + offsetAxis *tiltOffsetAxis_; + offsetAxis *liftOffsetAxis_; private: friend class detectorTowerController; + friend class offsetAxis; }; #endif diff --git a/src/detectorTowerController.cpp b/src/detectorTowerController.cpp index 94b4dcf..bcfd89c 100644 --- a/src/detectorTowerController.cpp +++ b/src/detectorTowerController.cpp @@ -27,105 +27,6 @@ detectorTowerController::detectorTowerController( asynStatus status = asynSuccess; - // ========================================================================= - // Create additional parameter library entries - - status = createParam("LIFT_OFFSET", asynParamFloat64, &liftOffset_); - if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " - "parameter failed with %s).\nTerminating IOC", - portName, __PRETTY_FUNCTION__, __LINE__, - stringifyAsynStatus(status)); - exit(-1); - } - - status = - createParam("LIFT_OFFSET_MOVE", asynParamFloat64, &liftOffsetMove_); - if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " - "parameter failed with %s).\nTerminating IOC", - portName, __PRETTY_FUNCTION__, __LINE__, - stringifyAsynStatus(status)); - exit(-1); - } - - status = createParam("LIFT_OFFSET_LOW_LIMIT", asynParamFloat64, - &liftOffsetLowLimit_); - if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " - "parameter failed with %s).\nTerminating IOC", - portName, __PRETTY_FUNCTION__, __LINE__, - stringifyAsynStatus(status)); - exit(-1); - } - - status = createParam("LIFT_OFFSET_HIGH_LIMIT", asynParamFloat64, - &liftOffsetHighLimit_); - if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " - "parameter failed with %s).\nTerminating IOC", - portName, __PRETTY_FUNCTION__, __LINE__, - stringifyAsynStatus(status)); - exit(-1); - } - - status = createParam("TILT_OFFSET", asynParamFloat64, &tiltOffset_); - if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " - "parameter failed with %s).\nTerminating IOC", - portName, __PRETTY_FUNCTION__, __LINE__, - stringifyAsynStatus(status)); - exit(-1); - } - - status = - createParam("TILT_OFFSET_MOVE", asynParamFloat64, &tiltOffsetMove_); - if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " - "parameter failed with %s).\nTerminating IOC", - portName, __PRETTY_FUNCTION__, __LINE__, - stringifyAsynStatus(status)); - exit(-1); - } - - status = createParam("TILT_OFFSET_LOW_LIMIT", asynParamFloat64, - &tiltOffsetLowLimit_); - if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " - "parameter failed with %s).\nTerminating IOC", - portName, __PRETTY_FUNCTION__, __LINE__, - stringifyAsynStatus(status)); - exit(-1); - } - - status = createParam("TILT_OFFSET_HIGH_LIMIT", asynParamFloat64, - &tiltOffsetHighLimit_); - if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " - "parameter failed with %s).\nTerminating IOC", - portName, __PRETTY_FUNCTION__, __LINE__, - stringifyAsynStatus(status)); - exit(-1); - } - - status = createParam("BEAM_ANGLE", asynParamFloat64, &beamAngle_); - if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " - "parameter failed with %s).\nTerminating IOC", - portName, __PRETTY_FUNCTION__, __LINE__, - stringifyAsynStatus(status)); - exit(-1); - } - status = createParam("POSITION_STATE", asynParamInt32, &positionState_); if (status != asynSuccess) { asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, @@ -136,16 +37,6 @@ detectorTowerController::detectorTowerController( exit(-1); } - status = createParam("START", asynParamInt32, &start_); - if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " - "parameter failed with %s).\nTerminating IOC", - portName, __PRETTY_FUNCTION__, __LINE__, - stringifyAsynStatus(status)); - exit(-1); - } - status = createParam("MOVE_TO_WORKING_POSITION", asynParamInt32, &moveToWorkingPosition_); if (status != asynSuccess) { @@ -174,7 +65,7 @@ asynStatus detectorTowerController::readInt32(asynUser *pasynUser, // Check if the axis is a detectorTowerAxis detectorTowerAxis *axis = getDetectorTowerAxis(pasynUser); if (axis == nullptr) { - // This is apparently a "normal" turboPmacAxis + // This is apparently a "normal" turboPmacAxis or an offsetAxis return turboPmacController::readInt32(pasynUser, value); } else { // The detector tower cannot be disabled @@ -196,31 +87,10 @@ asynStatus detectorTowerController::writeInt32(asynUser *pasynUser, detectorTowerAxis *axis = getDetectorTowerAxis(pasynUser); if (axis == nullptr) { - // This is apparently a "normal" turboPmacAxis + // This is apparently a "normal" turboPmacAxis or an offsetAxis return turboPmacController::writeInt32(pasynUser, value); } else { - // Handle custom PVs - if (function == start_) { - - // Start the movement, if the axis is not already moving. - // Otherwise stop it and schedule a movement. - int done = 0; - asynStatus status = - getIntegerParam(axis->axisNo_, motorStatusDone_, &done); - if (status != asynSuccess) { - return paramLibAccessFailed(status, "motorStatusDone_", - axis->axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - if (done == 1) { - return axis->moveFromParamLib(); - } else { - axis->scheduleMoveFromParamLib_ = true; - return axis->stop(0.0); - } - - } else if (function == moveToWorkingPosition_) { + if (function == moveToWorkingPosition_) { return axis->moveToWorkingPosition(value != 0); } else if (function == resetError_) { return axis->reset(); @@ -230,40 +100,6 @@ asynStatus detectorTowerController::writeInt32(asynUser *pasynUser, } } -asynStatus detectorTowerController::writeFloat64(asynUser *pasynUser, - epicsFloat64 value) { - int function = pasynUser->reason; - - detectorTowerAxis *axis = getDetectorTowerAxis(pasynUser); - - if (axis == nullptr) { - // This is apparently a "normal" turboPmacAxis - return turboPmacController::writeFloat64(pasynUser, value); - } else { - if (function == liftOffsetMove_) { - asynStatus status = - setDoubleParam(axis->axisNo_, liftOffset_, value); - if (status != asynSuccess) { - return paramLibAccessFailed(status, "liftOffset_", - axis->axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - return axis->moveFromParamLib(); - } else if (function == tiltOffsetMove_) { - asynStatus status = - setDoubleParam(axis->axisNo_, tiltOffset_, value); - if (status != asynSuccess) { - return paramLibAccessFailed(status, "tiltOffset_", - axis->axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - return axis->moveFromParamLib(); - } else { - return turboPmacController::writeFloat64(pasynUser, value); - } - } -} - /* Access one of the axes of the controller via the axis adress stored in asynUser. If the axis does not exist or is not a Axis, a nullptr is returned and an diff --git a/src/detectorTowerController.h b/src/detectorTowerController.h index 31c6614..c32755c 100644 --- a/src/detectorTowerController.h +++ b/src/detectorTowerController.h @@ -9,6 +9,7 @@ #ifndef detectorTowerController_H #define detectorTowerController_H #include "detectorTowerAxis.h" +#include "offsetAxis.h" #include "turboPmacController.h" class detectorTowerController : public turboPmacController { @@ -55,17 +56,6 @@ class detectorTowerController : public turboPmacController { */ asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); - /** - * @brief Overloaded function of turboPmacController - * - * The function is overloaded to immediate start of an offset movement - * - * @param pasynUser Specify the axis via the asynUser - * @param value New value - * @return asynStatus - */ - asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value); - /** * @brief Get the axis object * @@ -86,23 +76,14 @@ class detectorTowerController : public turboPmacController { private: // Indices of additional PVs -#define FIRST_detectorTower_PARAM liftOffset_ - int liftOffset_; - int liftOffsetMove_; - int liftOffsetLowLimit_; - int liftOffsetHighLimit_; - int tiltOffset_; - int tiltOffsetMove_; - int tiltOffsetLowLimit_; - int tiltOffsetHighLimit_; - int beamAngle_; +#define FIRST_detectorTower_PARAM positionState_ int positionState_; - int start_; int moveToWorkingPosition_; int resetError_; #define LAST_detectorTower_PARAM resetError_ friend class detectorTowerAxis; + friend class offsetAxis; }; #define NUM_detectorTower_DRIVER_PARAMS \ (&LAST_detectorTower_PARAM - &FIRST_detectorTower_PARAM + 1) diff --git a/src/offsetAxis.cpp b/src/offsetAxis.cpp new file mode 100644 index 0000000..fc89afe --- /dev/null +++ b/src/offsetAxis.cpp @@ -0,0 +1,224 @@ +#include "offsetAxis.h" +#include "detectorTowerAxis.h" +#include "detectorTowerController.h" +#include "turboPmacController.h" +#include +#include +#include + +/* +Contains all instances of offsetAxis which have been created and is used +in the initialization hook function. + */ +static std::vector axes; + +/** + * @brief Hook function to perform certain actions during the IOC initialization + * + * @param iState + */ +static void epicsInithookFunction(initHookState iState) { + if (iState == initHookAfterDatabaseRunning) { + // Iterate through all axes of each and call the initialization method + // on each one of them. + for (std::vector::iterator itA = axes.begin(); + itA != axes.end(); ++itA) { + offsetAxis *axis = *itA; + axis->init(); + } + } +} + +offsetAxis::offsetAxis(detectorTowerController *pC, int axisNo) + : turboPmacAxis(pC, axisNo, false), pC_(pC) { + + /* + The superclass constructor sinqAxis calls in turn its superclass constructor + asynMotorAxis. In the latter, a pointer to the constructed object this is + stored inside the array pAxes_: + + pC->pAxes_[axisNo] = this; + + Therefore, the axes are managed by the controller pC. If axisNo is out of + bounds, asynMotorAxis prints an error (see + https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorAxis.cpp, + line 40). However, we want the IOC creation to stop completely, since this + is a configuration error. + */ + if (axisNo >= pC->numAxes_) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d: FATAL ERROR: " + "Axis index %d must be smaller than the total number of axes " + "%d", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + axisNo_, pC->numAxes_); + exit(-1); + } + + // Initialize all member variables + // Assumed to be not ready by default, this is overwritten in the next poll + targetPosition_ = 0.0; + + // Register the hook function during construction of the first axis object + if (axes.empty()) { + initHookRegister(&epicsInithookFunction); + } + + // Collect all axes into this list which will be used in the hook + // function + axes.push_back(this); +} + +offsetAxis::~offsetAxis(void) { + // Since the controller memory is managed somewhere else, we don't need to + // clean up the pointer pC here. +} + +asynStatus offsetAxis::init() { + + // Local variable declaration + asynStatus status = asynSuccess; + double motorRecResolution = 0.0; + + // The parameter library takes some time to be initialized. Therefore we + // wait until the status is not asynParamUndefined anymore. + time_t now = time(NULL); + time_t maxInitTime = 60; + while (1) { + status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution); + if (status == asynParamUndefined) { + if (now + maxInitTime < time(NULL)) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line " + "%d\nInitializing the parameter library failed.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, + __LINE__); + return asynError; + } + } else if (status == asynSuccess) { + break; + } else if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorRecResolution_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + return asynSuccess; +} + +// Perform the actual poll +asynStatus offsetAxis::doPoll(bool *moving) { + + // Return value for the poll + asynStatus poll_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + int isMoving = 0; + + // ========================================================================= + + /* + The axis is moving if the detectorTowerAxis is moving -> We read the moving + status from the detectorTowerAxis. + */ + pl_status = + pC_->getIntegerParam(dTA_->axisNo_, pC_->motorStatusMoving_, &isMoving); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + *moving = isMoving != 0; + + // Update the parameter library + if (dTA_->error_ != 0) { + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + if (*moving == false) { + pl_status = setIntegerParam(pC_->motorMoveToHome_, 0); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + pl_status = setIntegerParam(pC_->motorStatusMoving_, *moving); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + pl_status = setIntegerParam(pC_->motorStatusDone_, !(*moving)); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // The limits are written into this class instance inside the doPoll + // function of detectorTowerAxis + return poll_status; +} + +asynStatus offsetAxis::doMove(double position, int relative, + double min_velocity, double max_velocity, + double acceleration) { + + double motorRecResolution = 0.0; + asynStatus pl_status = pC_->getDoubleParam( + axisNo_, pC_->motorRecResolution_, &motorRecResolution); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + // Signal to the deferredMovementCollectorLoop (of the detectorTowerAxis) + // that a movement should be started to the defined target position. + targetPosition_ = position * motorRecResolution; + dTA_->receivedTarget_ = true; + + return asynSuccess; +} + +asynStatus offsetAxis::stop(double acceleration) { + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rw_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + char response[pC_->MAXBUF_]; + + // ========================================================================= + + rw_status = pC_->writeRead(axisNo_, "P350=8", response, 0); + + if (rw_status != asynSuccess) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nStopping the movement " + "failed.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + return asynError; + } + + return rw_status; +} diff --git a/src/offsetAxis.h b/src/offsetAxis.h new file mode 100644 index 0000000..16a210e --- /dev/null +++ b/src/offsetAxis.h @@ -0,0 +1,78 @@ +#ifndef offsetAxis_H +#define offsetAxis_H +#include "turboPmacAxis.h" + +// Forward declaration of the controller class to resolve the cyclic dependency +// between the controller and the axis .h-file. See +// https://en.cppreference.com/w/cpp/language/class. +class detectorTowerController; +class detectorTowerAxis; + +class offsetAxis : public turboPmacAxis { + public: + /** + * @brief Construct a new detectorTowerAxis + * + * @param pController Pointer to the associated controller + * @param axisNo Index of the axis + */ + offsetAxis(detectorTowerController *pController, int axisNo); + + /** + * @brief Destroy the turboPmacAxis + * + */ + virtual ~offsetAxis(); + + /** + * @brief Readout of some values from the controller at IOC startup + * + * @return asynStatus + */ + asynStatus init(); + + /** + * @brief Implementation of the `stop` function from asynMotorAxis + * + * @param acceleration Acceleration ACCEL from the motor record. This + * value is currently not used. + * @return asynStatus + */ + asynStatus stop(double acceleration); + + /** + * @brief Implementation of the `doPoll` function from sinqAxis. The + * parameters are described in the documentation of `sinqAxis::doPoll`. + * + * @param moving + * @return asynStatus + */ + asynStatus doPoll(bool *moving); + + /** + * @brief Set the target value for the detector angle without starting a + * movement + * + * TODO + * + * @param position + * @param relative + * @param min_velocity + * @param max_velocity + * @param acceleration + * @return asynStatus + */ + asynStatus doMove(double position, int relative, double min_velocity, + double max_velocity, double acceleration); + + protected: + detectorTowerController *pC_; + detectorTowerAxis *dTA_; + double targetPosition_; + + private: + friend class detectorTowerController; + friend class detectorTowerAxis; +}; + +#endif