diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..419dc78 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "turboPmac"] + path = turboPmac + url = https://gitea.psi.ch/lin-epics-modules/turboPmac diff --git a/Makefile b/Makefile index dd42e05..4ad5a22 100644 --- a/Makefile +++ b/Makefile @@ -9,29 +9,31 @@ ARCH_FILTER=RHEL% # Additional module dependencies REQUIRED+=motorBase -# Specify the version of motorBase we want to build against +# Specify the version of motorBase we want to dynamically link against motorBase_VERSION=7.2.2 -# Specify the version of turboPmac we want to build against -sinqMotor_VERSION=mathis_s - -# Specify the version of turboPmac we want to build against -turboPmac_VERSION=mathis_s - # These headers allow to depend on this library for derived drivers. -HEADERS += src/detectorTowerAuxiliaryAxis.h -HEADERS += src/detectorTowerAxis.h +HEADERS += src/detectorTowerAngleAxis.h HEADERS += src/detectorTowerController.h +HEADERS += src/detectorTowerLiftAxis.h # Source files to build -SOURCES += src/detectorTowerAuxiliaryAxis.cpp -SOURCES += src/detectorTowerAxis.cpp +SOURCES += turboPmac/src/turboPmacAxis.cpp +SOURCES += turboPmac/src/turboPmacController.cpp +SOURCES += turboPmac/src/pmacAsynIPPort.c +SOURCES += src/detectorTowerAngleAxis.cpp SOURCES += src/detectorTowerController.cpp +SOURCES += src/detectorTowerLiftAxis.cpp # Store the record files +TEMPLATES += turboPmac/sinqMotor/db/asynRecord.db +TEMPLATES += turboPmac/sinqMotor/db/sinqMotor.db +TEMPLATES += turboPmac/db/turboPmac.db TEMPLATES += db/detectorTower.db # This file registers the motor-specific functions in the IOC shell. +DBDS += turboPmac/sinqMotor/src/sinqMotor.dbd +DBDS += turboPmac/src/turboPmac.dbd DBDS += src/detectorTower.dbd USR_CFLAGS += -Wall -Wextra -Weffc++ -Wunused-result -Wextra -Werror # -Wpedantic // Does not work because EPICS macros trigger warnings diff --git a/README.md b/README.md index 23f845b..8711bed 100644 --- a/README.md +++ b/README.md @@ -5,14 +5,14 @@ ## Overview This is a driver for the detector tower which is based on the Turbo PMAC driver (https://git.psi.ch/sinq-epics-modules/turboPmac). It consists of the following three objects: -- `detectorTowerAxis`: This is a virtual axis which controls multiple physical motors in order to provide a synchronized movement. -- `detectorTowerController`: This is an expanded variant of `turboPmacController` provided by the Turbo PMAC library linked above.It is needed to operate a `detectorTowerAxis`, but it can also be used to operate a "normal" `turboPmacAxis`. -- `detectorTowerAuxiliaryAxis`: This is an auxiliary axis type attached to the main detector axis. Multiple instances of these axes are constructed automatically when creating a `detectorTowerAxis`. +- `detectorTowerAngleAxis`: This is a virtual axis which controls multiple physical motors in order to provide a synchronized movement. +- `detectorTowerController`: This is an expanded variant of `turboPmacController` provided by the Turbo PMAC library linked above.It is needed to operate a `detectorTowerAngleAxis`, but it can also be used to operate a "normal" `turboPmacAxis`. +- `detectorTowerLiftAxis`: This is an auxiliary axis type attached to the main detector axis. Multiple instances of these axes are constructed automatically when creating a `detectorTowerAngleAxis`. The header files contain detailed documentation for all public functions. The headers themselves are exported when building the library to allow other drivers to depend on this one. ## User guide -The centerpiece of this driver is the `detectorTowerAxis`, which controls the angle of the detector flight tube. Creating an instance of this axis type also creates multiple so-called `detectorTowerAuxiliaryAxis`, which are used to perform secondary movements. +The centerpiece of this driver is the `detectorTowerAngleAxis`, which controls the angle of the detector flight tube. Creating an instance of this axis type also creates multiple so-called `detectorTowerLiftAxis`, which are used to perform secondary movements. The utilities provided in the `utils` folder of https://git.psi.ch/sinq-epics-modules/turboPmac work with this driver as well. @@ -22,12 +22,12 @@ TODO: How to start combined movement detectorTower exports the following IOC shell functions: - `detectorTowerController`: Create a new controller object. -- `detectorTowerAxis`: Create a new axis object. +- `detectorTowerAngleAxis`: Create a new axis object. -The constructor function for `detectorTowerAxis` has the following syntax: +The constructor function for `detectorTowerAngleAxis` has the following syntax: ``` -detectorTowerAxis("$(NAME)",1, 2, 3) +detectorTowerAngleAxis("$(NAME)",1, 2, 3) ``` with 1 being the axis number / index of the detector flight tube angle axis, 2 being the lift offset axis and 3 being the tilt offset axis. These axes are parametrized in the same way as any "normal" axes via a substitution file (see corresponding section below). @@ -51,7 +51,7 @@ drvAsynIPPortConfigure("$(ASYN_PORT)","172.28.101.24:1025") detectorTowerController("$(NAME)", "$(ASYN_PORT)", 8, 0.05, 1, 1); # Slot 1, 2 and 3 are occupied by a detector tower axis and its attached auxiliary axes, while the slots 4 and 5 are "normal" Turbo PMAC axes. -detectorTowerAxis("$(NAME)",1, 2, 3); +detectorTowerAngleAxis("$(NAME)",1, 2, 3); turboPmacAxis("$(NAME)",4); turboPmacAxis("$(NAME)",5); @@ -76,7 +76,7 @@ dbLoadRecords("$(sinqMotor_DB)/asynRecord.db","P=$(INSTR)$(NAME),PORT=$(ASYN_POR From the perspective of EPICS, the main detector flight tube axis and the auxiliary axes are independent axes and therefore each axis needs its own parametrization in the substitution file. If additional axes are used in the axis, they are parametrized as usual: ``` -detectorTowerAxis("$(NAME)",1, 2, 3); +detectorTowerAngleAxis("$(NAME)",1, 2, 3); turboPmacAxis("$(NAME)",4); turboPmacAxis("$(NAME)",5); ``` @@ -99,13 +99,14 @@ Note that the speed of the detector tower axes 1, 2 and 3 cannot be set. Setting ### Code architecture -The code is designed around the `detectorTowerAxis`, which controls the angle of the beam guide and acts as a "master" axis. Other movements are realized as auxiliary axes (e.g. for the vertical lift offset and the tilt offset). The `detectorTowerAxis` has pointers to all associated auxiliary axes and (as described above) its IOC shell constructor also builds the associated auxiliary axes. +The code is designed around the `detectorTowerAngleAxis`, which controls the angle of the beam guide and acts as a "master" axis. Other movements are realized as auxiliary axes (e.g. for the vertical lift offset and the tilt offset). The `detectorTowerAngleAxis` has pointers to all associated auxiliary axes and (as described above) its IOC shell constructor also builds the associated auxiliary axes. -The `doPoll` implementation for `detectorTowerAxis` queries the status of both the `detectorTowerAxis` itself and all associated auxiliary axes. If any of the axes is moving, the `detectorTowerAxis` is set to "moving" as well. In turn, the `doPoll` implementation of a `detectorTowerAuxiliaryAxis` checks if its associated `detectorTowerAxis` is moving and sets its own movement status accordingly. +TODO +The `doPoll` implementation for `detectorTowerAngleAxis` queries the status of both the `detectorTowerAngleAxis` itself and all associated auxiliary axes. If any of the axes is moving, the `detectorTowerAngleAxis` is set to "moving" as well. In turn, the `doPoll` implementation of a `detectorTowerLiftAxis` checks if its associated `detectorTowerAngleAxis` is moving and sets its own movement status accordingly. The `detectorTowerController` is a thin wrapper around a `turboPmacController` which overwrites the `readInt32` and `writeInt32` in order to support the PVs "$(INSTR)$(M):ChangeState", "$(INSTR)$(M):PositionStateRBV" and "$(INSTR)$(M):ChangingStateRBV". Any calls to these two methods not concerning the aforementioned PVs are directly forwarded to `turboPmacController::readInt32` / `turboPmacController::writeInt32`. -In order to save on movement time, movement commands to auxiliary axes and the `detectorTowerAxis` are collected and then send as a single resulting movement command to the MCU. In order to do so, a "collector" thread is running which checks if a movement request has been send to one of the axes. If that is the case, it waits for some time and checks if commands for other axes are given as well. Then, it calls `detectorTowerAxis::startCombinedMove` which combines all commands to a single request which is sent to the MCU. This allows the user to start a combined movement e.g. via caput: +In order to save on movement time, movement commands to auxiliary axes and the `detectorTowerAngleAxis` are collected and then send as a single resulting movement command to the MCU. In order to do so, a "collector" thread is running which checks if a movement request has been send to one of the axes. If that is the case, it waits for some time and checks if commands for other axes are given as well. Then, it calls `detectorTowerAngleAxis::startCombinedMove` which combines all commands to a single request which is sent to the MCU. This allows the user to start a combined movement e.g. via caput: ``` caput tower 2 & caput liftZeroCorr 10 @@ -123,4 +124,4 @@ Please see the documentation for the module sinqMotor: https://git.psi.ch/sinq-e ### How to build it -Please see the documentation for the module sinqMotor: https://git.psi.ch/sinq-epics-modules/sinqmotor/-/blob/main/README.md. \ No newline at end of file +This driver can be compiled and installed by running `make install` from the same directory where the Makefile is located. However, since it uses the git submodule turboPmac, make sure that the correct version of the submodule repository is checked out AND the change is commited (`git status` shows no non-committed changes). Please see the section "Usage as static dependency" in https://git.psi.ch/sinq-epics-modules/sinqmotor/-/blob/main/README.md for more details. \ No newline at end of file diff --git a/db/detectorTower.db b/db/detectorTower.db index 2383154..95b5b91 100644 --- a/db/detectorTower.db +++ b/db/detectorTower.db @@ -1,12 +1,3 @@ -# Set to 0 to move the tower into working state and to 1 to move into changer position -record(longout, "$(INSTR)$(M):ChangeState") { - field(DTYP, "asynInt32") - field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) CHANGE_STATE") - field(PINI, "NO") - field(DRVH, "1") - field(DRVL, "0") -} - # Read the position state of the axis: # 0 = not ready # 1 = ready (in working position) @@ -20,28 +11,163 @@ record(longin, "$(INSTR)$(M):PositionStateRBV") field(PINI, "NO") } +# Set to 0 to move the tower into working state and to 1 to move into changer position. +# The PV "$(INSTR)$(M):ChangeStateRBV" can be used to check if starting the corresponding +# movement was successfull. +record(longout, "$(INSTR)$(M):ChangeState") { + field(DTYP, "asynInt32") + field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) CHANGE_STATE") + field(PINI, "NO") + field(DRVH, "1") + field(DRVL, "0") +} + +# When a value is written to the PV "$(INSTR)$(M):ChangeState", the tower needs to be +# idle (PositionState = 0), otherwise the state will not be changed. If the value +# of this PV is equal to "$(INSTR)$(M):ChangeState", starting a state change was +# successfull, otherwise not. +record(longin, "$(INSTR)$(M):ChangeStateRBV") +{ + field(DTYP, "asynInt32") + field(INP, "@asyn($(CONTROLLER),$(AXIS)) CHANGE_STATE_RBV") + field(SCAN, "I/O Intr") + field(PINI, "NO") +} + # This PV combines the position state and the movement readback value from the # motor record: # - 0, if the tower is in working state or transitioning to change position # - 1, If the tower is in change position or transitioning to working state record(calc, "$(INSTR)$(M):ChangingStateRBV") { - field(INPA, "$(INSTR)$(M).MOVN CP") - field(INPB, "$(INSTR)$(M):PositionStateRBV CP") - field(CALC, "(B == 1 || (B == 2 && A == 1)) ? 0 : 1") + field(INPA, "$(INSTR)$(M):PositionStateRBV CP") + field(INPB, "$(INSTR)$(M).MOVN CP") + field(CALC, "((A == 2 && B == 0) || A == 3) ? 1 : 0") } -# Convert the double value from the calc record to an integer value +# Convert the double value from the calc record to an integer value. record(longout, "$(INSTR)$(M):ChangingStateRBV_int") { field(DOL, "$(INSTR)$(M):ChangingStateRBV CP") field(OMSL, "closed_loop") } -# Set an additional offset to the target position of an auxiliary axis -# For all other axis types, writing to this parameter does nothing. -record(ao, "$(INSTR)$(M):TargetOffset") { +# Read out the origin of the corresponding axis by the given value. +# This PV does nothing for "normal" Turbo PMAC axes. +record(ai, "$(INSTR)$(M):Origin") { field(DTYP, "asynFloat64") - field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) MOTOR_TARGET_OFFSET") - field(PINI, "YES") + field(INP, "@asyn($(CONTROLLER),$(AXIS)) MOTOR_ORIGIN") + field(SCAN, "I/O Intr") + field(PINI, "NO") field(VAL, "0") -} \ No newline at end of file +} + +# Shift the origin of the corresponding axis by the given value. The axis will move to this +# position and the hardware controller will internally set this position as the +# new "0" value. This PV does nothing for "normal" Turbo PMAC axes. +record(ao, "$(INSTR)$(M):AdjustOrigin") { + field(DTYP, "Raw Soft Channel") + field(PINI, "NO") + field(FLNK, "$(INSTR)$(M):ResetAO") + field(VAL, "0") +} + +# Reset the PV $(INSTR)$(M):AdjustOrigin to zero after it has been written to and +# forward the value to $(INSTR)$(M):WriteAO which does the actual writing. +record(seq, "$(INSTR)$(M):ResetAO") { + field(DESC, "Write value to hardware then reset to 0") + field(DOL1, "$(INSTR)$(M):AdjustOrigin") + field(LNK1, "$(INSTR)$(M):WriteAO.VAL PP") # Perform write to hardware + field(DOL2, "0.0") + field(LNK2, "$(INSTR)$(M):AdjustOrigin.VAL PP") # Reset to zero +} + +# This record forwards the adjustment of the origin to the asyn driver. +record(ao, "$(INSTR)$(M):WriteAO") { + field(DTYP, "asynFloat64") + field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) MOTOR_ADJUST_ORIGIN") + field(PINI, "NO") + field(VAL, "0") +} + +# Read out the origin of the lift axis support motor (FTZ at AMOR). +# This PV does nothing for "normal" Turbo PMAC axes or the angle axis. +record(ai, "$(INSTR)$(M):SupportOrigin") { + field(DTYP, "asynFloat64") + field(INP, "@asyn($(CONTROLLER),$(AXIS)) SUPPORT_ORIGIN") + field(SCAN, "I/O Intr") + field(PINI, "NO") + field(VAL, "0") +} + +# Shift the origin of the lift support axis by the given value. The axis will +# move to this position and the hardware controller will internally set this +# position as the new "0" value. This PV does nothing for "normal" Turbo PMAC +# axes or the angle axis. +record(ao, "$(INSTR)$(M):AdjustSupportOrigin") { + field(DTYP, "Raw Soft Channel") + field(PINI, "NO") + field(DRVH, "200") # Limit in encoder steps (not mm!) + field(DRVL, "-200") # Limit in encoder steps (not mm!) + field(FLNK, "$(INSTR)$(M):ResetASO") + field(VAL, "0") +} + +# Reset the PV $(INSTR)$(M):AdjustOrigin to zero after it has been written to and +# forward the value to $(INSTR)$(M):WriteAO which does the actual writing. +record(seq, "$(INSTR)$(M):ResetASO") { + field(DESC, "Write value to hardware then reset to 0") + field(DOL1, "$(INSTR)$(M):AdjustSupportOrigin") + field(LNK1, "$(INSTR)$(M):WriteASO.VAL PP") # Perform write to hardware + field(DOL2, "0.0") + field(LNK2, "$(INSTR)$(M):AdjustSupportOrigin.VAL PP") # Reset to zero +} + +# This record forwards the adjustment of the origin to the asyn driver. +record(ao, "$(INSTR)$(M):WriteASO") { + field(DTYP, "asynFloat64") + field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) SUPPORT_ADJUST_ORIGIN") + field(PINI, "NO") + field(VAL, "0") +} + +# This record pair reads the parameter library value for "AdjustOriginHighLimitFromDriver_" +# and pushes it to the high limit field of the "$(INSTR)$(M):AdjustOrigin" PV. +# This can be used to read limits from the hardware and correspondingly update the motor record from the driver. +# The implementation strategy is taken from https://epics.anl.gov/tech-talk/2022/msg00464.php. +# This record is coupled to the parameter library via AdjustOriginHighLimitFromDriver_ -> MOTOR_AOHL_FROM_DRIVER. +record(ai, "$(INSTR)$(M):AOHL_RBV") +{ + field(DTYP, "asynFloat64") + field(INP, "@asyn($(CONTROLLER),$(AXIS)) MOTOR_AOHL_FROM_DRIVER") + field(SCAN, "I/O Intr") + field(FLNK, "$(INSTR)$(M):PushAOHL2Field") + field(PINI, "NO") +} +# Abbreviated name because EPICS apparently has a maximum record name length +record(ao, "$(INSTR)$(M):PushAOHL2Field") { + field(DOL, "$(INSTR)$(M):AOHL_RBV NPP") + field(OUT, "$(INSTR)$(M):AdjustOrigin.DRVH") + field(OMSL, "closed_loop") + field(PINI, "NO") +} + +# This record pair reads the parameter library value for "motorLowLimitFromDriver_" +# and pushes it to the low limit field of the "$(INSTR)$(M):AdjustOrigin" PV. +# This can be used to read limits from the hardware and correspondingly update the motor record from the driver. +# The implementation strategy is taken from https://epics.anl.gov/tech-talk/2022/msg00464.php. +# This record is coupled to the parameter library via AdjustOriginLowLimitFromDriver_ -> MOTOR_AOLL_FROM_DRIVER. +record(ai, "$(INSTR)$(M):AOLL_RBV") +{ + field(DTYP, "asynFloat64") + field(INP, "@asyn($(CONTROLLER),$(AXIS)) MOTOR_AOLL_FROM_DRIVER") + field(SCAN, "I/O Intr") + field(FLNK, "$(INSTR)$(M):PushAOLL2Field") + field(PINI, "NO") +} +# Abbreviated name because EPICS apparently has a maximum record name length +record(ao, "$(INSTR)$(M):PushAOLL2Field") { + field(DOL, "$(INSTR)$(M):AOLL_RBV NPP") + field(OUT, "$(INSTR)$(M):AdjustOrigin.DRVL") + field(OMSL, "closed_loop") + field(PINI, "NO") +} diff --git a/src/detectorTowerAngleAxis.cpp b/src/detectorTowerAngleAxis.cpp new file mode 100644 index 0000000..8447e8a --- /dev/null +++ b/src/detectorTowerAngleAxis.cpp @@ -0,0 +1,693 @@ +#include "detectorTowerAngleAxis.h" +#include "detectorTowerController.h" +#include "turboPmacController.h" +#include +#include +#include + +/* +Contains all instances of detectorTowerAngleAxis 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) { + detectorTowerAngleAxis *axis = *itA; + axis->init(); + } + } +} + +static void deferredMovementCollectorLoop(void *drvPvt) { + detectorTowerAngleAxis *axis = (detectorTowerAngleAxis *)drvPvt; + while (1) { + if (axis->receivedTarget_) { + // Wait for 100 ms and then start the movement with the information + // available + axis->startingDeferredMovement_ = true; + epicsThreadSleep(axis->deferredMovementWait_); + axis->startCombinedMove(); + + // After the movement command has been send, reset the flag + axis->receivedTarget_ = false; + } + // Limit this loop to an idle frequency of 1 kHz + epicsThreadSleep(0.001); + } +} + +detectorTowerAngleAxis::detectorTowerAngleAxis(detectorTowerController *pC, + int axisNo, + detectorTowerLiftAxis *liftAxis) + : 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_->pasynUser(), 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 + error_ = 0; + receivedTarget_ = false; + startingDeferredMovement_ = false; + deferredMovementWait_ = 0.1; // seconds + liftAxis_ = liftAxis; + liftAxis_->angleAxis_ = this; + + // Will be populated in the init() method + beamRadius_ = 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); + + // 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); +} + +detectorTowerAngleAxis::~detectorTowerAngleAxis(void) { + // Since the controller memory is managed somewhere else, we don't need to + // clean up the pointer pC here. +} + +asynStatus detectorTowerAngleAxis::init() { + + // Local variable declaration + asynStatus status = asynSuccess; + double motorRecResolution = 0.0; + char response[pC_->MAXBUF_] = {0}; + int nvals = 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_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis " + "%ddeferredMovementCollectorLoop => %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__); + } + } + + // Read the detector radius + const char *command = "P459"; + status = pC_->writeRead(axisNo_, command, response, 1); + if (status != asynSuccess) { + return status; + } + + nvals = sscanf(response, "%lf", &beamRadius_); + if (nvals != 1) { + return pC_->couldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Initialize the motorStatusMoving flag + status = setIntegerParam(pC_->motorStatusMoving(), 0); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorStatusMoving_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + return status; +} + +// Perform the actual poll +asynStatus detectorTowerAngleAxis::poll(bool *moving) { + asynStatus status = asynSuccess; + + // Is this axis the one with the smallest index? + // If not, just read out the movement status and update *moving + if (axisNo() < liftAxis_->axisNo()) { + status = pC_->pollDetectorAxes(moving, this, liftAxis_); + } else { + status = pC_->getIntegerParam(axisNo(), pC_->motorStatusMoving(), + (int *)moving); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorStatusMoving", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + wasMoving_ = *moving; + return status; +} + +asynStatus detectorTowerAngleAxis::doPoll(bool *moving) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nThe doPoll method " + "of this axis type should not be reachable. This is a bug.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + return asynError; +} + +asynStatus detectorTowerAngleAxis::doMove(double position, int relative, + double min_velocity, + double max_velocity, + double acceleration) { + double motorRecResolution = 0.0; + asynStatus plStatus = pC_->getDoubleParam( + axisNo_, pC_->motorRecResolution(), &motorRecResolution); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "motorRecResolution_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + // Signal to the deferredMovementCollectorLoop that a movement should be + // started to the defined target position. + targetPosition_ = position * motorRecResolution; + receivedTarget_ = true; + + return asynSuccess; +} + +asynStatus detectorTowerAngleAxis::startCombinedMove() { + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rwStatus = asynSuccess; + + // Status of parameter library operations + asynStatus plStatus = asynSuccess; + + char command[pC_->MAXBUF_] = {0}; + char response[pC_->MAXBUF_] = {0}; + double motorCoordinatesPosition = 0.0; + int positionState = 0; + + // ========================================================================= + + plStatus = + pC_->getIntegerParam(axisNo_, pC_->positionStateRBV(), &positionState); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "positionStateRBV_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // If the axis is in changer position, it must be moved into working + // position before any move can be started. + bool isInChangerPos = positionState == 2 || positionState == 3; + if (pC_->getMsgPrintControl().shouldBePrinted( + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + isInChangerPos, pC_->pasynUser())) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nAxis cannot be " + "moved because it is moving from working to changer " + "position, is in changer position or is moving from changer " + "to working position.%s\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + pC_->getMsgPrintControl().getSuffix()); + } + if (isInChangerPos) { + startingDeferredMovement_ = false; + return asynError; + } + + // Set the target positions for beam tilt, detector tilt offset and lift + // offset + snprintf(command, sizeof(command), "Q451=%lf Q454=%lf P350=1", + targetPosition_, liftAxis_->targetPosition_); + + // 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 + rwStatus = pC_->writeRead(axisNo_, command, response, 0); + + // Free the controller again + pC_->unlock(); + + if (rwStatus != asynSuccess) { + asynPrint( + pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nStarting movement to " + "target position %lf failed.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + motorCoordinatesPosition); + plStatus = setIntegerParam(pC_->motorStatusProblem(), true); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + return rwStatus; +} + +asynStatus detectorTowerAngleAxis::stop(double acceleration) { + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rwStatus = asynSuccess; + + // Status of parameter library operations + asynStatus plStatus = asynSuccess; + + char response[pC_->MAXBUF_] = {0}; + + // ========================================================================= + + rwStatus = pC_->writeRead(axisNo_, "P350=8", response, 0); + + if (rwStatus != asynSuccess) { + asynPrint( + pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nStopping the movement " + "failed.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + plStatus = setIntegerParam(pC_->motorStatusProblem(), true); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + return asynError; + } + + return rwStatus; +} + +// The detector tower axis uses absolute encoders +asynStatus detectorTowerAngleAxis::readEncoderType() { + asynStatus status = setStringParam(pC_->encoderType(), AbsoluteEncoder); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "encoderType_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + return asynSuccess; +} + +asynStatus +detectorTowerAngleAxis::toggleWorkingChangerState(bool toChangingPosition) { + + char response[pC_->MAXBUF_] = {0}; + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rwStatus = asynSuccess; + + // Status of parameter library operations + asynStatus plStatus = asynSuccess; + + bool moving = false; + int positionState = 0; + + // ========================================================================= + + rwStatus = poll(&moving); + if (rwStatus != asynSuccess) { + return rwStatus; + } + + plStatus = + pC_->getIntegerParam(axisNo_, pC_->positionStateRBV(), &positionState); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "positionStateRBV_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + if (moving) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nAxis is not " + "idle and can therefore not be moved to %s state.%s\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + toChangingPosition ? "changer" : "working", + pC_->getMsgPrintControl().getSuffix()); + + plStatus = setStringParam( + pC_->motorMessageText(), + "Axis cannot be moved to changer position while it is moving."); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + plStatus = setIntegerParam(pC_->changeStateRBV(), !toChangingPosition); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "changeStateRBV", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + return asynError; + } + + // Axis is already in the correct position + bool isAlreadyThere = (toChangingPosition == false && positionState == 1) || + (toChangingPosition == true && positionState == 2); + + if (isAlreadyThere) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW, + "Controller \"%s\", axis %d => %s, line %d\nAxis is already " + "in %s position.%s\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + toChangingPosition ? "changer" : "working", + pC_->getMsgPrintControl().getSuffix()); + + // Update the PV anyway, even though nothing changed. + plStatus = setIntegerParam(pC_->changeStateRBV(), toChangingPosition); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "changeStateRBV", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + return asynSuccess; + } + + // Move the axis into changer or working position + if (toChangingPosition) { + rwStatus = pC_->writeRead(axisNo_, "P350=2", response, 0); + plStatus = setStringParam(pC_->motorMessageText(), + "Moving to changer position ..."); + } else { + rwStatus = pC_->writeRead(axisNo_, "P350=3", response, 0); + plStatus = setStringParam(pC_->motorMessageText(), + "Moving to working state ..."); + } + if (plStatus != asynSuccess) { + plStatus = setIntegerParam(pC_->changeStateRBV(), !toChangingPosition); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "changeStateRBV", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + return pC_->paramLibAccessFailed(plStatus, "motorMessageText_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + plStatus = setIntegerParam(pC_->changeStateRBV(), toChangingPosition); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "changeStateRBV", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + return rwStatus; +} + +asynStatus detectorTowerAngleAxis::doReset() { + + char response[pC_->MAXBUF_] = {0}; + asynStatus plStatus = asynSuccess; + int positionState = 0; + + plStatus = + pC_->getIntegerParam(axisNo_, pC_->positionStateRBV(), &positionState); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "positionStateRBV_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + /* + Check which action should be performed: + - If error_ == 10 or 11 (FTZ motor error): P352 = 3 (Recover FTZ) + - If any other error: P352 = 2 (Reset error) + - Otherwise: P352 = 1 (Set axis in closed-loop mode) + */ + if (error_ == 10 || error_ == 11) { + return pC_->writeRead(axisNo_, "P352=3", response, 0); + } else if (error_ != 0) { + return pC_->writeRead(axisNo_, "P352=2", response, 0); + } else { + return pC_->writeRead(axisNo_, "P352=1", response, 0); + } +} + +asynStatus detectorTowerAngleAxis::adjustOrigin(double newOrigin) { + + asynStatus status = asynSuccess; + char command[pC_->MAXBUF_] = {0}; + char response[pC_->MAXBUF_] = {0}; + int positionState = 0; + + // ========================================================================= + + status = + pC_->getIntegerParam(axisNo_, pC_->positionStateRBV(), &positionState); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "positionStateRBV_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // If the axis is in changer position, it must be moved into working + // position before any move can be started. + bool isInChangerPos = positionState == 2 || positionState == 3; + if (pC_->getMsgPrintControl().shouldBePrinted( + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + isInChangerPos, pC_->pasynUser())) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nAxis cannot be " + "moved because it is moving from working to changer " + "position, is in changer position or is moving from changer " + "to working position.%s\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + pC_->getMsgPrintControl().getSuffix()); + } + + // Set the new origin for the angle axis + snprintf(command, sizeof(command), "Q556=%lf P350=4", newOrigin); + + // We don't expect an answer + status = pC_->writeRead(axisNo_, command, response, 0); + + if (status != asynSuccess) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nSetting new " + "angle origin %lf failed.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + newOrigin); + status = setIntegerParam(pC_->motorStatusProblem(), true); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + return status; +} + +/*************************************************************************************/ +/** The following functions are C-wrappers, and can be called directly from + * iocsh */ + +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 angleAxisIdx, + int liftAxisIdx) { + + /* + findAsynPortDriver is a asyn library FFI function which uses the C ABI. + Therefore it returns a void pointer instead of e.g. a pointer to a + superclass of the controller such as asynPortDriver. Type-safe upcasting + via dynamic_cast is therefore not possible directly. However, we do know + that the void pointer is either a pointer to asynPortDriver (if a driver + with the specified name exists) or a nullptr. Therefore, we first do a + nullptr check, then a cast to asynPortDriver and lastly a (typesafe) + dynamic_upcast to Controller + https://stackoverflow.com/questions/70906749/is-there-a-safe-way-to-cast-void-to-class-pointer-in-c + */ + void *ptr = findAsynPortDriver(portName); + if (ptr == nullptr) { + /* + 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'taxis + works w/o that, but doesn't offer the comfort provided + by the asynTrace-facility + */ + errlogPrintf("Controller \"%s\" => %s, line %d\nPort not found.", + portName, __PRETTY_FUNCTION__, __LINE__); + return asynError; + } + // Unsafe cast of the pointer to an asynPortDriver + asynPortDriver *apd = (asynPortDriver *)(ptr); + + // Safe downcast + detectorTowerController *pC = dynamic_cast(apd); + if (pC == nullptr) { + errlogPrintf("Controller \"%s\" => %s, line %d\nController " + "is not a detectorTowerController.", + portName, __PRETTY_FUNCTION__, __LINE__); + return asynError; + } + + // Assert that the three indices are different from each other + if (angleAxisIdx == liftAxisIdx) { + errlogPrintf("Controller \"%s\" => %s, line %d\nAll 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(); + + /* + We create a new instance of the axis, using the "new" keyword to + allocate it on the heap while avoiding RAII. + https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorController.cpp + https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/asynPortDriver.cpp + + The created object is registered in EPICS in its constructor and can safely + be "leaked" here. + */ + + detectorTowerLiftAxis *liftAxis = + new detectorTowerLiftAxis(pC, liftAxisIdx); + +#pragma GCC diagnostic ignored "-Wunused-but-set-variable" +#pragma GCC diagnostic ignored "-Wunused-variable" + detectorTowerAngleAxis *pAxis = + new detectorTowerAngleAxis(pC, angleAxisIdx, liftAxis); + + // Allow manipulation of the controller again + pC->unlock(); + return asynSuccess; +} + +static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mcu1)", + iocshArgString}; +static const iocshArg CreateAxisArg1 = {"Number of the detector angle axis", + iocshArgInt}; +static const iocshArg CreateAxisArg2 = {"Number of the lift axis", iocshArgInt}; +static const iocshArg *const CreateAxisArgs[] = { + &CreateAxisArg0, + &CreateAxisArg1, + &CreateAxisArg2, +}; +static const iocshFuncDef configDetectorTowerCreateAxis = {"detectorTowerAxis", + 3, CreateAxisArgs}; +static void configDetectorTowerCreateAxisCallFunc(const iocshArgBuf *args) { + detectorTowerCreateAxis(args[0].sval, args[1].ival, args[2].ival); +} + +// ============================================================================= + +/** + * @brief Set the setDeferredMovementWait (FFI implementation) + * + * @param portName Name of the controller + * @param axisNo Axis number + * @param scaleMovTimeout Scaling factor (in seconds) + * @return asynStatus + */ +asynStatus setDeferredMovementWait(const char *portName, int axisNo, + double deferredMovementWait) { + + sinqController *pC; + pC = (sinqController *)findAsynPortDriver(portName); + if (pC == nullptr) { + errlogPrintf("Controller \"%s\" => %s, line %d:\nPort %s not found.", + portName, __PRETTY_FUNCTION__, __LINE__, portName); + return asynError; + } + + asynMotorAxis *asynAxis = pC->getAxis(axisNo); + detectorTowerAngleAxis *axis = + dynamic_cast(asynAxis); + if (axis == nullptr) { + errlogPrintf("Controller \"%s\" => %s, line %d:\nAxis %d does not " + "exist or is not an instance of detectorTowerAngleAxis.", + portName, __PRETTY_FUNCTION__, __LINE__, axisNo); + return asynError; + } + + axis->deferredMovementWait_ = deferredMovementWait; + return asynSuccess; +} + +static const iocshArg setDeferredMovementWaitArg0 = {"Controller port name", + iocshArgString}; +static const iocshArg setDeferredMovementWaitArg1 = {"Axis number", + iocshArgInt}; +static const iocshArg setDeferredMovementWaitArg2 = { + "Minimum time a deferred movement will be delayed in order to collect " + "commands in seconds", + iocshArgDouble}; +static const iocshArg *const setDeferredMovementWaitArgs[] = { + &setDeferredMovementWaitArg0, &setDeferredMovementWaitArg1, + &setDeferredMovementWaitArg2}; +static const iocshFuncDef setDeferredMovementWaitDef = { + "setDeferredMovementWait", 3, setDeferredMovementWaitArgs}; + +static void setDeferredMovementWaitCallFunc(const iocshArgBuf *args) { + setDeferredMovementWait(args[0].sval, args[1].ival, args[2].dval); +} + +// ============================================================================= + +// This function is made known to EPICS in detectorTower.dbd and is +// called by EPICS in order to register both functions in the IOC shell +static void detectorTowerAxisRegister(void) { + iocshRegister(&configDetectorTowerCreateAxis, + configDetectorTowerCreateAxisCallFunc); + iocshRegister(&setDeferredMovementWaitDef, setDeferredMovementWaitCallFunc); +} +epicsExportRegistrar(detectorTowerAxisRegister); + +} // extern "C" diff --git a/src/detectorTowerAxis.h b/src/detectorTowerAngleAxis.h similarity index 63% rename from src/detectorTowerAxis.h rename to src/detectorTowerAngleAxis.h index 7e8fa29..6e017e7 100644 --- a/src/detectorTowerAxis.h +++ b/src/detectorTowerAngleAxis.h @@ -1,6 +1,6 @@ -#ifndef detectorTowerAxis_H -#define detectorTowerAxis_H -#include "detectorTowerAuxiliaryAxis.h" +#ifndef detectorTowerAngleAxis_H +#define detectorTowerAngleAxis_H +#include "detectorTowerLiftAxis.h" #include "turboPmacAxis.h" // Forward declaration of the controller class to resolve the cyclic dependency @@ -8,10 +8,10 @@ // https://en.cppreference.com/w/cpp/language/class. class detectorTowerController; -class detectorTowerAxis : public turboPmacAxis { +class detectorTowerAngleAxis : public turboPmacAxis { public: /** - * @brief Construct a new detectorTowerAxis + * @brief Construct a new detectorTowerAngleAxis * * @param pController Pointer to the associated controller * @param axisNo Index of the axis @@ -20,15 +20,14 @@ class detectorTowerAxis : public turboPmacAxis { * @param tiltZeroCorrAxis Pointer to the attached axis which controls * the tilt offset */ - detectorTowerAxis(detectorTowerController *pController, int axisNo, - detectorTowerAuxiliaryAxis *liftZeroCorrAxis, - detectorTowerAuxiliaryAxis *tiltZeroCorrAxis); + detectorTowerAngleAxis(detectorTowerController *pController, int axisNo, + detectorTowerLiftAxis *liftAxis); /** - * @brief Destroy the detectorTowerAxis + * @brief Destroy the detectorTowerAngleAxis * */ - virtual ~detectorTowerAxis(); + virtual ~detectorTowerAngleAxis(); /** * @brief Readout of some values from the controller at IOC startup @@ -47,12 +46,17 @@ class detectorTowerAxis : public turboPmacAxis { asynStatus stop(double acceleration); /** - * @brief Implementation of the `doPoll` function from sinqAxis. The - * parameters are described in the documentation of `sinqAxis::doPoll`. + * @brief Poll all detector tower axes, if this axis is the one with the + * smallest index. + * + * We do not use the doPoll framework from sinqMotor here on purpose, since + * we want to e.g. reset the motorStatusProblem for all axes at once at the + * beginning of the poll. * * @param moving * @return asynStatus */ + asynStatus poll(bool *moving); asynStatus doPoll(bool *moving); /** @@ -100,7 +104,32 @@ class detectorTowerAxis : public turboPmacAxis { */ asynStatus doReset(); - // If true, either this axis or one of the detectorTowerAuxiliaryAxis + /** + * @brief Move the axis to the position `newOrigin` and recalibrate + * + * When calling this function, the angle axis moves to `newOrigin` and the + * hardware sets this position as the new origin. + * + * @param origin + * @return asynStatus + */ + asynStatus adjustOrigin(double newOrigin); + + /** + * @brief Read out the beam radius + * + * @return double + */ + double beamRadius() { return beamRadius_; } + + /** + * @brief Return a pointer to the associated angle axis + * + * @return detectorTowerAngleAxis* + */ + detectorTowerLiftAxis *liftAxis() { return liftAxis_; } + + // If true, either this axis or one of the detectorTowerLiftAxis // attached to it received a movement command. bool receivedTarget_; @@ -115,15 +144,19 @@ class detectorTowerAxis : public turboPmacAxis { */ double deferredMovementWait_; + /** + * @brief Variable holding the axis error for later use + * + */ + int error_; + protected: detectorTowerController *pC_; - int error_; - detectorTowerAuxiliaryAxis *tiltZeroCorrAxis_; - detectorTowerAuxiliaryAxis *liftZeroCorrAxis_; + detectorTowerLiftAxis *liftAxis_; double beamRadius_; private: - friend class detectorTowerAuxiliaryAxis; + friend class detectorTowerLiftAxis; }; #endif diff --git a/src/detectorTowerAuxiliaryAxis.cpp b/src/detectorTowerAuxiliaryAxis.cpp deleted file mode 100644 index 8e54f63..0000000 --- a/src/detectorTowerAuxiliaryAxis.cpp +++ /dev/null @@ -1,244 +0,0 @@ -#include "detectorTowerAuxiliaryAxis.h" -#include "detectorTowerAxis.h" -#include "detectorTowerController.h" -#include "turboPmacController.h" -#include -#include -#include - -/* -Contains all instances of detectorTowerAuxiliaryAxis 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) { - detectorTowerAuxiliaryAxis *axis = *itA; - axis->init(); - } - } -} - -detectorTowerAuxiliaryAxis::detectorTowerAuxiliaryAxis( - 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_->pasynUser(), 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); - } - - // 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); -} - -detectorTowerAuxiliaryAxis::~detectorTowerAuxiliaryAxis(void) { - // Since the controller memory is managed somewhere else, we don't need to - // clean up the pointer pC here. -} - -asynStatus detectorTowerAuxiliaryAxis::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_->pasynUser(), 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 detectorTowerAuxiliaryAxis::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, "motorStatusMoving_", - dTA_->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__); - } - - // According to the function documentation of asynMotorAxis::poll, this - // function should be called at the end of a poll implementation. - pl_status = callParamCallbacks(); - bool wantToPrint = pl_status != asynSuccess; - if (pC_->getMsgPrintControl().shouldBePrinted( - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, wantToPrint, - pC_->pasynUser())) { - asynPrint(pC_->pasynUser(), 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_->getMsgPrintControl().getSuffix()); - } - if (wantToPrint) { - poll_status = pl_status; - } - - // The limits are written into this class instance inside the doPoll - // function of detectorTowerAxis - return poll_status; -} - -asynStatus detectorTowerAuxiliaryAxis::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 detectorTowerAuxiliaryAxis::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_] = {0}; - - // ========================================================================= - - rw_status = pC_->writeRead(axisNo_, "P350=8", response, 0); - - if (rw_status != asynSuccess) { - asynPrint( - pC_->pasynUser(), 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; -} - -asynStatus detectorTowerAuxiliaryAxis::reset() { return dTA_->reset(); }; diff --git a/src/detectorTowerAuxiliaryAxis.h b/src/detectorTowerAuxiliaryAxis.h deleted file mode 100644 index 1d11cb0..0000000 --- a/src/detectorTowerAuxiliaryAxis.h +++ /dev/null @@ -1,82 +0,0 @@ -#ifndef detectorTowerAuxiliaryAxis_H -#define detectorTowerAuxiliaryAxis_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 detectorTowerAuxiliaryAxis : public turboPmacAxis { - public: - /** - * @brief Construct a new detectorTowerAxis - * - * @param pController Pointer to the associated controller - * @param axisNo Index of the axis - */ - detectorTowerAuxiliaryAxis(detectorTowerController *pController, - int axisNo); - - /** - * @brief Destroy the turboPmacAxis - * - */ - virtual ~detectorTowerAuxiliaryAxis(); - - /** - * @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 and trigger a position - * collection cycle - * - * @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); - - /** - * @brief Call the reset function of the associated `detectorTowerAxis`. - * - * @return asynStatus - */ - asynStatus reset(); - - protected: - detectorTowerController *pC_; - detectorTowerAxis *dTA_; - - private: - friend class detectorTowerAxis; -}; - -#endif diff --git a/src/detectorTowerAxis.cpp b/src/detectorTowerAxis.cpp deleted file mode 100644 index 829eadc..0000000 --- a/src/detectorTowerAxis.cpp +++ /dev/null @@ -1,1221 +0,0 @@ -#include "detectorTowerAxis.h" -#include "detectorTowerController.h" -#include "turboPmacController.h" -#include -#include -#include -#include - -/* -Contains all instances of detectorTowerAxis 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) { - detectorTowerAxis *axis = *itA; - axis->init(); - } - } -} - -static void deferredMovementCollectorLoop(void *drvPvt) { - detectorTowerAxis *axis = (detectorTowerAxis *)drvPvt; - while (1) { - if (axis->receivedTarget_) { - // Wait for 100 ms and then start the movement with the information - // available - axis->startingDeferredMovement_ = true; - epicsThreadSleep(axis->deferredMovementWait_); - axis->startCombinedMove(); - - // After the movement command has been send, reset the flag - axis->receivedTarget_ = false; - } - // Limit this loop to an idle frequency of 1 kHz - epicsThreadSleep(0.001); - } -} - -detectorTowerAxis::detectorTowerAxis( - detectorTowerController *pC, int axisNo, - detectorTowerAuxiliaryAxis *liftZeroCorrAxis, - detectorTowerAuxiliaryAxis *tiltZeroCorrAxis) - : 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_->pasynUser(), 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 - error_ = 0; - receivedTarget_ = false; - startingDeferredMovement_ = false; - deferredMovementWait_ = 0.1; // seconds - liftZeroCorrAxis_ = liftZeroCorrAxis; - tiltZeroCorrAxis_ = tiltZeroCorrAxis; - liftZeroCorrAxis->dTA_ = this; - tiltZeroCorrAxis->dTA_ = this; - - // Will be populated in the init() method - beamRadius_ = 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); - - // 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) { - // Since the controller memory is managed somewhere else, we don't need to - // clean up the pointer pC here. -} - -asynStatus detectorTowerAxis::init() { - - // Local variable declaration - asynStatus status = asynSuccess; - double motorRecResolution = 0.0; - char response[pC_->MAXBUF_] = {0}; - int nvals = 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_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis " - "%ddeferredMovementCollectorLoop => %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__); - } - } - - // Read the detector radius - const char *command = "RADIUS"; - status = pC_->writeRead(axisNo_, command, response, 10); - if (status != asynSuccess) { - return status; - } - - nvals = sscanf(response, "%lf", &beamRadius_); - if (nvals != 1) { - return pC_->couldNotParseResponse(command, response, axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } - - return status; -} - -asynStatus detectorTowerAxis::doPoll(bool *moving) { - - // Return value for the poll - asynStatus poll_status = asynSuccess; - - // 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 userMessage[pC_->MAXBUF_] = {0}; - char response[pC_->MAXBUF_] = {0}; - int nvals = 0; - - int direction = 0; - int error = 0; - int positionState = 0; - int inPosition = 0; - double beamTiltAngle = 0.0; - double previousBeamTiltAngle = 0.0; - double motorRecResolution = 0.0; - double limitsOffset = 0.0; - double highLimit = 0.0; - double lowLimit = 0.0; - double detectorHeight = 0.0; - double liftZeroCorrHighLimit = 0.0; - double liftZeroCorrLowLimit = 0.0; - double tiltZeroCorrHighLimit = 0.0; - double tiltZeroCorrLowLimit = 0.0; - - // ========================================================================= - - // Motor resolution from parameter library - pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(), - &motorRecResolution); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - // Read the previous motor position - pl_status = motorPosition(&previousBeamTiltAngle); - if (pl_status != asynSuccess) { - return pl_status; - } - - /* - Query the following information: - - In-position flag - - Position state - - Axis error - - Beam tilt angle - - Beam tilt angle limits - - Beam lift position - - Beam lift offset limits - - Detector tilt offset limits - */ - const char *command = - "P354 P358 P359 Q510 Q513 Q514 Q310 Q353 Q354 Q553 Q554"; - rw_status = pC_->writeRead(axisNo_, command, response, 10); - if (rw_status != asynSuccess) { - return rw_status; - } - - nvals = sscanf(response, "%d %d %d %lf %lf %lf %lf %lf %lf %lf %lf", - &inPosition, &positionState, &error, &beamTiltAngle, - &highLimit, &lowLimit, &detectorHeight, - &liftZeroCorrHighLimit, &liftZeroCorrLowLimit, - &tiltZeroCorrHighLimit, &tiltZeroCorrLowLimit); - if (nvals != 10) { - return pC_->couldNotParseResponse(command, response, axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } - - /* - The axis limits are set as: ({[]}) - where [] are the positive and negative limits set in EPICS/NICOS, {} are the - software limits set on the MCU and () are the hardware limit switches. In - other words, the EPICS/NICOS limits should be stricter than the software - limits on the MCU which in turn should be stricter than the hardware limit - switches. For example, if the hardware limit switches are at [-10, 10], the - software limits could be at [-9, 9] and the EPICS / NICOS limits could be at - [-8, 8]. Therefore, we cannot use the software limits read from the MCU - directly, but need to shrink them a bit. In this case, we're shrinking them - by limitsOffset on both sides. - */ - pl_status = - pC_->getDoubleParam(axisNo_, pC_->motorLimitsOffset(), &limitsOffset); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorLimitsOffset_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - highLimit = highLimit - limitsOffset; - lowLimit = lowLimit + limitsOffset; - - // Store the axis status - error_ = error; - - // Update the working position state PV - pl_status = setIntegerParam(pC_->positionStateRBV(), positionState); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "positionStateRBV_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - // Axis is always enabled - pl_status = setIntegerParam(pC_->motorEnableRBV(), 1); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorEnableRBV_", axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } - - // Check if the motor is moving - if (inPosition == 1) { - // By now, the controller has actually started the movement - startingDeferredMovement_ = false; - *moving = true; - } else { - if (startingDeferredMovement_) { - *moving = true; - } else { - *moving = false; - } - } - - // Create the unique callsite identifier manually so it can be used later in - // the shouldBePrinted calls. - msgPrintControlKey keyPosState = msgPrintControlKey( - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); - - // Reset the count, if the status is not an error state - bool resetCountPosState = true; - - // Interpret the position state - switch (positionState) { - case 0: - // Axis not ready - break; - case 1: - // Axis ready - break; - case 2: - // Axis is moving to or in changing position - break; - case 3: - // Axis is moving to working state - resetCountPosState = false; - break; - default: - - if (pC_->getMsgPrintControl().shouldBePrinted(keyPosState, true, - pC_->pasynUser())) { - asynPrint( - pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nReached unknown " - "state P354 = %d.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - positionState, pC_->getMsgPrintControl().getSuffix()); - } - resetCountPosState = false; - - snprintf(userMessage, sizeof(userMessage), - "Unknown state P354 = %d has been reached. Please call " - "the support.", - positionState); - pl_status = setStringParam(pC_->motorMessageText(), userMessage); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - } - - if (resetCountPosState) { - pC_->getMsgPrintControl().resetCount(keyPosState, pC_->pasynUser()); - } - - if (*moving) { - // If the axis is moving, evaluate the movement direction - if ((beamTiltAngle - previousBeamTiltAngle) > 0) { - direction = 1; - } else { - direction = 0; - } - } - - // Create the unique callsite identifier manually so it can be used later in - // the shouldBePrinted calls. - msgPrintControlKey keyError = msgPrintControlKey( - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); - bool resetError = true; - - // Error handling - switch (error) { - case 0: - // No error - break; - case 1: - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint( - pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nBrake COZ not " - "released (P359=1).%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - pl_status = - setStringParam(pC_->motorMessageText(), - "Brake COZ not released. Try resetting the axis. " - "If the problem persists, please call the support."); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - - case 2: - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint( - pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nMove command " - "rejected because axis is already moving.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - pl_status = setStringParam(pC_->motorMessageText(), - "Move command rejected because axis is " - "already moving. Please call the support"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - - case 3: - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nError motor " - "FTZ.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - pl_status = - setStringParam(pC_->motorMessageText(), - "Error in motor FTZ. Try resetting the axis. If " - "the problem persists, please call the support"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - - case 4: - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint( - pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nAxis stopped " - "unexpectedly.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - pl_status = - setStringParam(pC_->motorMessageText(), - "Axis stopped unexpectedly. Try resetting the axis " - "and issue the move command again. If the problem " - "persists, please call the support."); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - - case 5: - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint( - pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nRelease removed " - "while moving.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - pl_status = setStringParam( - pC_->motorMessageText(), - "Axis release was removed while moving. Try resetting the axis " - "and issue the move command again. If the problem persists, please " - "call the support."); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - - case 6: - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint( - pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nEmergency stop " - "activated.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - pl_status = - setStringParam(pC_->motorMessageText(), "Emergency stop activate"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - - case 7: - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nError motor " - "COZ.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - pl_status = - setStringParam(pC_->motorMessageText(), - "Error in motor COZ. Try resetting the axis. If " - "the problem persists, please call the support"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - - case 8: - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nError motor " - "COM.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - pl_status = - setStringParam(pC_->motorMessageText(), - "Error in motor COM. Try resetting the axis. If " - "the problem persists, please call the support"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - - case 9: - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nError motor " - "COX.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - pl_status = - setStringParam(pC_->motorMessageText(), - "Error in motor COX. Try resetting the axis. If " - "the problem persists, please call the support"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - - case 10: - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nHit end " - "switch FTZ.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - pl_status = - setStringParam(pC_->motorMessageText(), "Hit end switch FTZ"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - - case 11: - // Following error - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint( - pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nMaximum allowed " - "following error FTZ exceeded.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - snprintf(userMessage, sizeof(userMessage), - "Maximum allowed following error exceeded (P359 = %d). " - "Check if movement range is blocked. " - "Otherwise please call the support.", - error); - pl_status = setStringParam(pC_->motorMessageText(), userMessage); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - - default: - if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, - pC_->pasynUser())) { - asynPrint( - pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nUnknown error " - "P359 = %d.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, error, - pC_->getMsgPrintControl().getSuffix()); - } - resetError = false; - - snprintf(userMessage, sizeof(userMessage), - "Unknown error P359 = %d. Please call the support.", error); - pl_status = setStringParam(pC_->motorMessageText(), userMessage); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - poll_status = asynError; - break; - } - - if (resetError) { - pC_->getMsgPrintControl().resetCount(keyError, pC_->pasynUser()); - } - - // Update the parameter library - if (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__); - } - - pl_status = setIntegerParam(pC_->motorStatusDirection(), direction); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusDirection_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - pl_status = pC_->setDoubleParam(axisNo_, pC_->motorHighLimitFromDriver(), - highLimit); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - pl_status = - pC_->setDoubleParam(axisNo_, pC_->motorLowLimitFromDriver(), lowLimit); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorLowLimitFromDriver_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - // Set the values for the offset axes - int liftAxisNo = liftZeroCorrAxis_->axisNo_; - double liftZeroCorrPosition = - detectorHeight + beamRadius_ * sin(beamTiltAngle); - - pl_status = setMotorPosition(liftZeroCorrPosition); - if (pl_status != asynSuccess) { - return pl_status; - } - pl_status = pC_->setDoubleParam(liftAxisNo, pC_->motorHighLimitFromDriver(), - liftZeroCorrHighLimit); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_", - liftAxisNo, __PRETTY_FUNCTION__, - __LINE__); - } - pl_status = pC_->setDoubleParam(liftAxisNo, pC_->motorLowLimitFromDriver(), - liftZeroCorrLowLimit); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorLowLimitFromDriver_", - liftAxisNo, __PRETTY_FUNCTION__, - __LINE__); - } - - int tiltAxisNo = tiltZeroCorrAxis_->axisNo_; - - // There exists no readback value for tiltZeroCorrAxis_, hence we just set - // the current position to the target position. - tiltZeroCorrAxis_->setMotorPosition(tiltZeroCorrAxis_->targetPosition_); - if (pl_status != asynSuccess) { - return pl_status; - } - pl_status = pC_->setDoubleParam(tiltAxisNo, pC_->motorHighLimitFromDriver(), - tiltZeroCorrHighLimit); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_", - tiltAxisNo, __PRETTY_FUNCTION__, - __LINE__); - } - pl_status = pC_->setDoubleParam(tiltAxisNo, pC_->motorLowLimitFromDriver(), - tiltZeroCorrLowLimit); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorLowLimitFromDriver_", - tiltAxisNo, __PRETTY_FUNCTION__, - __LINE__); - } - - // Transform from motor to EPICS coordinates (see comment in - // turboPmacAxis::init()) - pl_status = setMotorPosition(beamTiltAngle); - if (pl_status != asynSuccess) { - return pl_status; - } - - pl_status = setIntegerParam(pC_->positionStateRBV(), positionState); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "positionStateRBV_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - return poll_status; -} - -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, "motorRecResolution_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - // Signal to the deferredMovementCollectorLoop that a movement should be - // started to the defined target position. - targetPosition_ = position * motorRecResolution; - receivedTarget_ = true; - - return asynSuccess; -} - -asynStatus detectorTowerAxis::startCombinedMove() { - - // 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 command[pC_->MAXBUF_] = {0}; - char response[pC_->MAXBUF_] = {0}; - double motorCoordinatesPosition = 0.0; - double offsetliftZeroCorr = 0.0; - double offsettiltZeroCorr = 0.0; - int positionState = 0; - - // ========================================================================= - - // If the axis is in changer position, it must be moved into working - // position before any move can be started. - bool isInChangerPos = positionState == 2 || positionState == 3; - if (pC_->getMsgPrintControl().shouldBePrinted( - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - isInChangerPos, pC_->pasynUser())) { - asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nAxis cannot be " - "moved because it is moving from working to changer " - "position, is in changer position or is moving from changer " - "to working position.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - pC_->getMsgPrintControl().getSuffix()); - } - if (isInChangerPos) { - startingDeferredMovement_ = false; - return asynError; - } - - // Read out any offsets for the zero correction axes - pl_status = - pC_->getDoubleParam(tiltZeroCorrAxis_->axisNo(), - pC_->motorTargetOffset(), &offsettiltZeroCorr); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorTargetOffset", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - pl_status = - pC_->getDoubleParam(liftZeroCorrAxis_->axisNo(), - pC_->motorTargetOffset(), &offsetliftZeroCorr); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorTargetOffset", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - // Set the target positions for beam tilt, detector tilt offset and lift - // offset - snprintf(command, sizeof(command), "Q451=%lf Q454=%lf Q456=%lf P350=1", - targetPosition_, - liftZeroCorrAxis_->targetPosition_ + offsetliftZeroCorr, - tiltZeroCorrAxis_->targetPosition_ + offsettiltZeroCorr); - - // 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_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nStarting movement to " - "target position %lf failed.\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - motorCoordinatesPosition); - pl_status = setIntegerParam(pC_->motorStatusProblem(), true); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - } - - return rw_status; -} - -asynStatus detectorTowerAxis::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_] = {0}; - - // ========================================================================= - - rw_status = pC_->writeRead(axisNo_, "P350=8", response, 0); - - if (rw_status != asynSuccess) { - asynPrint( - pC_->pasynUser(), 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; -} - -// 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__); - } - return asynSuccess; -} - -asynStatus -detectorTowerAxis::toggleWorkingChangerState(bool toChangingPosition) { - - char response[pC_->MAXBUF_] = {0}; - - // Status of read-write-operations of ASCII commands to the controller - asynStatus rw_status = asynSuccess; - - // Status of parameter library operations - asynStatus pl_status = asynSuccess; - - bool moving = false; - int positionState = 0; - - // ========================================================================= - - doPoll(&moving); - - pl_status = - pC_->getIntegerParam(axisNo_, pC_->positionStateRBV(), &positionState); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "positionStateRBV_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - if (moving) { - asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nAxis is not " - "idle and can therefore not be moved to %s state.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - toChangingPosition ? "changer" : "working", - pC_->getMsgPrintControl().getSuffix()); - - pl_status = setStringParam( - pC_->motorMessageText(), - "Axis cannot be moved to changer position while it is moving."); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - return asynError; - } - - // Axis is already in the correct position - bool isAlreadyThere = (toChangingPosition == false && positionState == 1) || - (toChangingPosition == true && positionState == 2); - - if (isAlreadyThere) { - asynPrint(pC_->pasynUser(), ASYN_TRACE_WARNING, - "Controller \"%s\", axis %d => %s, line %d\nAxis is already " - "in %s position.%s\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - toChangingPosition ? "changer" : "working", - pC_->getMsgPrintControl().getSuffix()); - return asynSuccess; - } - - // Move the axis into changer or working position - if (toChangingPosition) { - rw_status = pC_->writeRead(axisNo_, "P350=2", response, 0); - pl_status = setStringParam(pC_->motorMessageText(), - "Moving to changer position ..."); - } else { - rw_status = pC_->writeRead(axisNo_, "P350=3", response, 0); - pl_status = setStringParam(pC_->motorMessageText(), - "Moving to working state ..."); - } - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - return rw_status; -} - -asynStatus detectorTowerAxis::doReset() { - - char response[pC_->MAXBUF_] = {0}; - asynStatus pl_status = asynSuccess; - int positionState = 0; - - pl_status = - pC_->getIntegerParam(axisNo_, pC_->positionStateRBV(), &positionState); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "positionStateRBV_", - axisNo_, __PRETTY_FUNCTION__, - __LINE__); - } - - /* - Check which action should be performed: - - If positionState == 0 (not ready): P352 = 1 (Set axis in closed-loop - mode) - - If error_ == 10 or 11 (FTZ motor error): P352 = 3 (Recover FTZ) - - If any other error: P352 = 2 (Reset error) - */ - if (positionState == 0) { - return pC_->writeRead(axisNo_, "P352=1", response, 0); - } else if (error_ == 10 || error_ == 11) { - return pC_->writeRead(axisNo_, "P352=3", response, 0); - } else if (error_ != 0) { - return pC_->writeRead(axisNo_, "P352=2", response, 0); - } else { - return asynSuccess; - } -} - -/*************************************************************************************/ -/** The following functions are C-wrappers, and can be called directly from - * iocsh */ - -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 axisDetectorTower, - int liftZeroCorraxisNo, - int tiltZeroCorraxisNo) { - - /* - findAsynPortDriver is a asyn library FFI function which uses the C ABI. - Therefore it returns a void pointer instead of e.g. a pointer to a - superclass of the controller such as asynPortDriver. Type-safe upcasting - via dynamic_cast is therefore not possible directly. However, we do know - that the void pointer is either a pointer to asynPortDriver (if a driver - with the specified name exists) or a nullptr. Therefore, we first do a - nullptr check, then a cast to asynPortDriver and lastly a (typesafe) - dynamic_upcast to Controller - https://stackoverflow.com/questions/70906749/is-there-a-safe-way-to-cast-void-to-class-pointer-in-c - */ - void *ptr = findAsynPortDriver(portName); - if (ptr == nullptr) { - /* - 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'taxis - works w/o that, but doesn't offer the comfort provided - by the asynTrace-facility - */ - errlogPrintf("Controller \"%s\" => %s, line %d\nPort not found.", - portName, __PRETTY_FUNCTION__, __LINE__); - return asynError; - } - // Unsafe cast of the pointer to an asynPortDriver - asynPortDriver *apd = (asynPortDriver *)(ptr); - - // Safe downcast - detectorTowerController *pC = dynamic_cast(apd); - if (pC == nullptr) { - errlogPrintf("Controller \"%s\" => %s, line %d\nController " - "is not a detectorTowerController.", - portName, __PRETTY_FUNCTION__, __LINE__); - return asynError; - } - - // Assert that the three indices are different from each other - if (axisDetectorTower == liftZeroCorraxisNo || - axisDetectorTower == tiltZeroCorraxisNo || - tiltZeroCorraxisNo == liftZeroCorraxisNo) { - 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(); - - /* - We create a new instance of the axis, using the "new" keyword to - allocate it on the heap while avoiding RAII. - https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorController.cpp - https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/asynPortDriver.cpp - - The created object is registered in EPICS in its constructor and can safely - be "leaked" here. - */ - - detectorTowerAuxiliaryAxis *liftZeroCorrAxis = - new detectorTowerAuxiliaryAxis(pC, liftZeroCorraxisNo); - detectorTowerAuxiliaryAxis *tiltZeroCorrAxis = - new detectorTowerAuxiliaryAxis(pC, tiltZeroCorraxisNo); - -#pragma GCC diagnostic ignored "-Wunused-but-set-variable" -#pragma GCC diagnostic ignored "-Wunused-variable" - detectorTowerAxis *pAxis = new detectorTowerAxis( - pC, axisDetectorTower, liftZeroCorrAxis, tiltZeroCorrAxis); - - // Allow manipulation of the controller again - pC->unlock(); - return asynSuccess; -} - -static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mcu1)", - iocshArgString}; -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", - 4, CreateAxisArgs}; -static void configDetectorTowerCreateAxisCallFunc(const iocshArgBuf *args) { - detectorTowerCreateAxis(args[0].sval, args[1].ival, args[2].ival, - args[3].ival); -} - -// ============================================================================= - -/** - * @brief Set the setDeferredMovementWait (FFI implementation) - * - * @param portName Name of the controller - * @param axisNo Axis number - * @param scaleMovTimeout Scaling factor (in seconds) - * @return asynStatus - */ -asynStatus setDeferredMovementWait(const char *portName, int axisNo, - double deferredMovementWait) { - - sinqController *pC; - pC = (sinqController *)findAsynPortDriver(portName); - if (pC == nullptr) { - errlogPrintf("Controller \"%s\" => %s, line %d:\nPort %s not found.", - portName, __PRETTY_FUNCTION__, __LINE__, portName); - return asynError; - } - - asynMotorAxis *asynAxis = pC->getAxis(axisNo); - detectorTowerAxis *axis = dynamic_cast(asynAxis); - if (axis == nullptr) { - errlogPrintf("Controller \"%s\" => %s, line %d:\nAxis %d does not " - "exist or is not an instance of detectorTowerAxis.", - portName, __PRETTY_FUNCTION__, __LINE__, axisNo); - return asynError; - } - - axis->deferredMovementWait_ = deferredMovementWait; - return asynSuccess; -} - -static const iocshArg setDeferredMovementWaitArg0 = {"Controller port name", - iocshArgString}; -static const iocshArg setDeferredMovementWaitArg1 = {"Axis number", - iocshArgInt}; -static const iocshArg setDeferredMovementWaitArg2 = { - "Minimum time a deferred movement will be delayed in order to collect " - "commands in seconds", - iocshArgDouble}; -static const iocshArg *const setDeferredMovementWaitArgs[] = { - &setDeferredMovementWaitArg0, &setDeferredMovementWaitArg1, - &setDeferredMovementWaitArg2}; -static const iocshFuncDef setDeferredMovementWaitDef = { - "setDeferredMovementWait", 3, setDeferredMovementWaitArgs}; - -static void setDeferredMovementWaitCallFunc(const iocshArgBuf *args) { - setDeferredMovementWait(args[0].sval, args[1].ival, args[2].dval); -} - -// ============================================================================= - -// This function is made known to EPICS in detectorTower.dbd and is -// called by EPICS in order to register both functions in the IOC shell -static void detectorTowerAxisRegister(void) { - iocshRegister(&configDetectorTowerCreateAxis, - configDetectorTowerCreateAxisCallFunc); - iocshRegister(&setDeferredMovementWaitDef, setDeferredMovementWaitCallFunc); -} -epicsExportRegistrar(detectorTowerAxisRegister); - -} // extern "C" diff --git a/src/detectorTowerController.cpp b/src/detectorTowerController.cpp index 53d4a10..a760fd0 100644 --- a/src/detectorTowerController.cpp +++ b/src/detectorTowerController.cpp @@ -1,10 +1,14 @@ #include "detectorTowerController.h" -#include "detectorTowerAxis.h" +#include "detectorTowerAngleAxis.h" #include "turboPmacController.h" #include #include #include +// Necessary to make M_PI available +#define _USE_MATH_DEFINES +#include + /** * @brief Construct a new detectorTowerController object * @@ -48,7 +52,72 @@ detectorTowerController::detectorTowerController( exit(-1); } - status = createParam("MOTOR_OFFSET", asynParamFloat64, &motorTargetOffset_); + status = createParam("CHANGE_STATE_RBV", asynParamInt32, &changeStateRBV_); + 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("MOTOR_ORIGIN", asynParamFloat64, &motorOrigin_); + 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("MOTOR_ADJUST_ORIGIN", asynParamFloat64, + &motorAdjustOrigin_); + 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("MOTOR_AOHL_FROM_DRIVER", asynParamFloat64, + &motorAdjustOriginHighLimitFromDriver_); + 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("MOTOR_AOLL_FROM_DRIVER", asynParamFloat64, + &motorAdjustOriginLowLimitFromDriver_); + 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("SUPPORT_ORIGIN", asynParamFloat64, &liftSupportOrigin_); + 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("SUPPORT_ADJUST_ORIGIN", asynParamFloat64, + &liftSupportAdjustOrigin_); if (status != asynSuccess) { asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a " @@ -64,14 +133,14 @@ asynStatus detectorTowerController::readInt32(asynUser *pasynUser, // The detector tower or its auxiliary axes cannot be disabled if (pasynUser->reason == motorCanDisable_) { - detectorTowerAxis *dTAxis = getDetectorTowerAxis(pasynUser); - if (dTAxis != nullptr) { + detectorTowerAngleAxis *angleAxis = + getDetectorTowerAngleAxis(pasynUser); + if (angleAxis != nullptr) { *value = 0; return asynSuccess; } - detectorTowerAuxiliaryAxis *dTAAxis = - getDetectorTowerAuxiliaryAxis(pasynUser); - if (dTAAxis != nullptr) { + detectorTowerLiftAxis *liftAxis = getDetectorTowerLiftAxis(pasynUser); + if (liftAxis != nullptr) { *value = 0; return asynSuccess; } @@ -84,10 +153,15 @@ asynStatus detectorTowerController::writeInt32(asynUser *pasynUser, // ===================================================================== - if (pasynUser->reason == motorCanDisable_) { - detectorTowerAxis *dTAxis = getDetectorTowerAxis(pasynUser); - if (dTAxis != nullptr) { - return dTAxis->toggleWorkingChangerState(value); + if (pasynUser->reason == changeState_) { + detectorTowerAngleAxis *angleAxis = + getDetectorTowerAngleAxis(pasynUser); + if (angleAxis != nullptr) { + return angleAxis->toggleWorkingChangerState(value); + } + detectorTowerLiftAxis *liftAxis = getDetectorTowerLiftAxis(pasynUser); + if (liftAxis != nullptr) { + return liftAxis->angleAxis()->toggleWorkingChangerState(value); } } return turboPmacController::writeInt32(pasynUser, value); @@ -96,65 +170,862 @@ asynStatus detectorTowerController::writeInt32(asynUser *pasynUser, asynStatus detectorTowerController::writeFloat64(asynUser *pasynUser, epicsFloat64 value) { int function = pasynUser->reason; + asynStatus status = asynSuccess; // ===================================================================== - if (function == motorTargetOffset_) { + if (function == motorAdjustOrigin_) { - // Write into the parameter library and trigger a movement cycle, if the - // writing succeeded - asynStatus status = turboPmacController::writeFloat64(pasynUser, value); - if (status == asynSuccess) { - detectorTowerAxis *dTAxis = getDetectorTowerAxis(pasynUser); - if (dTAxis != nullptr) { - dTAxis->receivedTarget_ = true; + // Is this function called by an asynUser of the angle or lift axis? + detectorTowerAngleAxis *aAxis = getDetectorTowerAngleAxis(pasynUser); + if (aAxis != nullptr) { + status = aAxis->adjustOrigin(value); + if (status != asynSuccess) { + return status; + } + return turboPmacController::writeFloat64(pasynUser, value); + } else { + detectorTowerLiftAxis *lAxis = getDetectorTowerLiftAxis(pasynUser); + if (lAxis != nullptr) { + status = lAxis->adjustOrigin(value); + if (status != asynSuccess) { + return status; + } + return turboPmacController::writeFloat64(pasynUser, value); } } - return status; + return asynSuccess; + } else if (function == liftSupportAdjustOrigin_) { + detectorTowerLiftAxis *lAxis = getDetectorTowerLiftAxis(pasynUser); + if (lAxis != nullptr) { + status = lAxis->adjustSupportOrigin(value); + if (status != asynSuccess) { + return status; + } + return turboPmacController::writeFloat64(pasynUser, value); + } + return asynSuccess; + } 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 +error is emitted. +*/ +detectorTowerAngleAxis * +detectorTowerController::getDetectorTowerAngleAxis(asynUser *pasynUser) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser); + return dynamic_cast(asynAxis); +} + +/* +Access one of the axes of the controller via the axis index. +If the axis does not exist or is not a Axis, the function must return Null +*/ +detectorTowerAngleAxis * +detectorTowerController::getDetectorTowerAngleAxis(int axisNo) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo); + return dynamic_cast(asynAxis); +} + +/* +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 +error is emitted. +*/ +detectorTowerLiftAxis * +detectorTowerController::getDetectorTowerLiftAxis(asynUser *pasynUser) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser); + return dynamic_cast(asynAxis); +} + +/* +Access one of the axes of the controller via the axis index. +If the axis does not exist or is not a Axis, the function must return Null +*/ +detectorTowerLiftAxis * +detectorTowerController::getDetectorTowerLiftAxis(int axisNo) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo); + return dynamic_cast(asynAxis); +} + +asynStatus +detectorTowerController::pollDetectorAxes(bool *moving, + detectorTowerAngleAxis *angleAxis, + detectorTowerLiftAxis *liftAxis) { + + // Return value for the poll + asynStatus pollStatus = asynSuccess; + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rwStatus = asynSuccess; + + // Status of parameter library operations + asynStatus plStatus = asynSuccess; + + char userMessage[MAXBUF_] = {0}; + char response[MAXBUF_] = {0}; + int nvals = 0; + + int angleDir = 0; + int liftDir = 0; + + int error = 0; + int positionState = 0; + int inPosition = 0; + + double angle = 0.0; + double prevAngle = 0.0; + double limitsOffset = 0.0; + double angleHighLimit = 0.0; + double angleLowLimit = 0.0; + + double liftHighLimit = 0.0; + double liftLowLimit = 0.0; + double lift = 0.0; + double prevLift = 0.0; + + double liftOrigin = 0.0; + double liftAdjustOriginHighLimit = 0.0; + double liftAdjustOriginLowLimit = 0.0; + + double supportOrigin = 0.0; + + double angleOrigin = 0.0; + double angleAdjustOriginHighLimit = 0.0; + double angleAdjustOriginLowLimit = 0.0; + + int angleAxisNo = angleAxis->axisNo(); + int liftAxisNo = liftAxis->axisNo(); + + /* + For messages which in principle concern both axes, use the smaller of the + two indices. + */ + int comAxisNo = angleAxisNo > liftAxisNo ? liftAxisNo : angleAxisNo; + + // ========================================================================= + + // Reset stored errors + angleAxis->error_ = 0; + + /* + At the beginning of the poll, it is assumed that the axes have no status + problems and therefore all error indicators are reset. This does not affect + the PVs until callParamCallbacks has been called! + + The motorStatusProblem_ field changes the motor record fields SEVR and STAT. + */ + plStatus = angleAxis->setIntegerParam(motorStatusProblem(), false); + if (plStatus != asynSuccess) { + paramLibAccessFailed(plStatus, "motorStatusProblem_", angleAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + plStatus = liftAxis->setIntegerParam(motorStatusProblem(), false); + if (plStatus != asynSuccess) { + paramLibAccessFailed(plStatus, "motorStatusProblem_", liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); } - return turboPmacController::writeFloat64(pasynUser, value); -} + plStatus = angleAxis->setIntegerParam(motorStatusCommsError(), false); + if (plStatus != asynSuccess) { + paramLibAccessFailed(plStatus, "motorStatusCommsError_", angleAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + plStatus = liftAxis->setIntegerParam(motorStatusCommsError(), false); + if (plStatus != asynSuccess) { + paramLibAccessFailed(plStatus, "motorStatusCommsError_", liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } -/* -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 -error is emitted. -*/ -detectorTowerAxis * -detectorTowerController::getDetectorTowerAxis(asynUser *pasynUser) { - asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser); - return dynamic_cast(asynAxis); -} + // Read the previous motor positions + plStatus = angleAxis->motorPosition(&prevAngle); + if (plStatus != asynSuccess) { + return plStatus; + } + plStatus = liftAxis->motorPosition(&prevLift); + if (plStatus != asynSuccess) { + return plStatus; + } -/* -Access one of the axes of the controller via the axis index. -If the axis does not exist or is not a Axis, the function must return Null -*/ -detectorTowerAxis *detectorTowerController::getDetectorTowerAxis(int axisNo) { - asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo); - return dynamic_cast(asynAxis); -} + /* + Query the following information: + - In-position flag + - Position state + - Axis error + - Beam angle position (COM at AMOR) + - Beam angle limits + - Beam angle origin + - Beam angle origin limits + - Beam lift position (COZ at AMOR) + - Beam lift limits + - Beam lift origin + - Beam lift origin limits + - Support motor origin (FTZ at AMOR) + */ + const char *command = "P354 P358 P359 Q510 Q513 Q514 Q515 Q553 Q554 Q310 " + "Q351 Q352 Q315 Q353 Q354 Q415"; + rwStatus = writeRead(liftAxisNo, command, response, 16); + if (rwStatus != asynSuccess) { + return rwStatus; + } -/* -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 -error is emitted. -*/ -detectorTowerAuxiliaryAxis * -detectorTowerController::getDetectorTowerAuxiliaryAxis(asynUser *pasynUser) { - asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser); - return dynamic_cast(asynAxis); -} + nvals = + sscanf(response, + "%d %d %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", + &inPosition, &positionState, &error, &angle, &angleHighLimit, + &angleLowLimit, &angleOrigin, &angleAdjustOriginHighLimit, + &angleAdjustOriginLowLimit, &lift, &liftHighLimit, &liftLowLimit, + &liftOrigin, &liftAdjustOriginHighLimit, + &liftAdjustOriginLowLimit, &supportOrigin); + if (nvals != 16) { + return couldNotParseResponse(command, response, liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } -/* -Access one of the axes of the controller via the axis index. -If the axis does not exist or is not a Axis, the function must return Null -*/ -detectorTowerAuxiliaryAxis * -detectorTowerController::getDetectorTowerAuxiliaryAxis(int axisNo) { - asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo); - return dynamic_cast(asynAxis); + /* + The axis limits are set as: ({[]}) + where [] are the positive and negative limits set in EPICS/NICOS, {} are the + software limits set on the MCU and () are the hardware limit switches. In + other words, the EPICS/NICOS limits should be stricter than the software + limits on the MCU which in turn should be stricter than the hardware limit + switches. For example, if the hardware limit switches are at [-10, 10], the + software limits could be at [-9, 9] and the EPICS / NICOS limits could be at + [-8, 8]. Therefore, we cannot use the software limits read from the MCU + directly, but need to shrink them a bit. In this case, we're shrinking them + by limitsOffset on both sides. + */ + + // Angle + plStatus = getDoubleParam(angleAxisNo, motorLimitsOffset(), &limitsOffset); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorLimitsOffset_", angleAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + angleHighLimit = angleHighLimit - limitsOffset; + angleLowLimit = angleLowLimit + limitsOffset; + + // Lift + plStatus = getDoubleParam(liftAxisNo, motorLimitsOffset(), &limitsOffset); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorLimitsOffset_", liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + liftHighLimit = liftHighLimit - limitsOffset; + liftLowLimit = liftLowLimit + limitsOffset; + + // Store the axis error + angleAxis->error_ = error; + + // Check if the tower is moving + if (inPosition == 1 || positionState > 2) { + // By now, the controller has actually started the movement + angleAxis->startingDeferredMovement_ = false; + *moving = true; + } else { + if (angleAxis->startingDeferredMovement_) { + *moving = true; + } else { + *moving = false; + } + } + + // Calculate the lift position /w consideration of the angle + lift = lift + angleAxis->beamRadius() * sin(angle * M_PI / 180); + + // Create the unique callsite identifier manually so it can be used later in + // the shouldBePrinted calls. + msgPrintControlKey keyPosState = + msgPrintControlKey(portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__); + + // Reset the count, if the status is not an error state + bool resetCountPosState = true; + + // Interpret the position state + switch (positionState) { + case 0: + if (getMsgPrintControl().shouldBePrinted(keyPosState, true, + pasynUser())) { + asynPrint(pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nTower not " + "ready (P358 = 0). Reset the tower via the \"Reset\" PV " + "before continuing.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetCountPosState = false; + + plStatus = + setStringParam(motorMessageText(), "Reset one of the tower axes."); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorMessageText_", + comAxisNo, __PRETTY_FUNCTION__, + __LINE__); + } + + pollStatus = asynError; + break; + case 1: + // Axis ready + break; + case 2: + // Axis is moving to or in changing position + break; + case 3: + // Axis is moving to working state + resetCountPosState = false; + break; + case 4: + // Axis is adjusting the angle origin + break; + case 5: + // Axis is adjusting the lift origin + break; + case 6: + // Axis is adjusting the lift support (FTZ) origin + break; + default: + + if (getMsgPrintControl().shouldBePrinted(keyPosState, true, + pasynUser())) { + asynPrint( + pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nReached unknown " + "state P354 = %d.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + positionState, getMsgPrintControl().getSuffix()); + } + resetCountPosState = false; + + snprintf(userMessage, sizeof(userMessage), + "Unknown state P358 = %d has been reached. Please call " + "the support.", + positionState); + plStatus = setStringParam(motorMessageText(), userMessage); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorMessageText_", + comAxisNo, __PRETTY_FUNCTION__, + __LINE__); + } + + pollStatus = asynError; + } + + if (resetCountPosState) { + getMsgPrintControl().resetCount(keyPosState, pasynUser()); + } + + if (*moving) { + if ((angle - prevAngle) > 0) { + angleDir = 1; + } else { + angleDir = 0; + } + if ((lift - prevLift) > 0) { + liftDir = 1; + } else { + liftDir = 0; + } + } + + // Create the unique callsite identifier manually so it can be used later in + // the shouldBePrinted calls. + msgPrintControlKey keyError = + msgPrintControlKey(portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__); + bool resetError = true; + + // Error handling + switch (error) { + case 0: + // No error + break; + case 1: + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint( + pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nBrake COZ not " + "released (P359=1).%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), + "Brake COZ not released. Try resetting the axis."); + + pollStatus = asynError; + break; + + case 2: + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint( + pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nMove command " + "rejected because axis is already moving.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), + "Move command rejected because axis is already moving. Try " + "resetting the axis."); + + pollStatus = asynError; + break; + + case 3: + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint(pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nError motor " + "FTZ.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), + "Error in motor FTZ. Try resetting the axis."); + + pollStatus = asynError; + break; + + case 4: + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint( + pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nAxis stopped " + "unexpectedly.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), + "Axis stopped unexpectedly. Try resetting the axis " + "and issue the move command again."); + + pollStatus = asynError; + break; + + case 5: + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint( + pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nRelease removed " + "while moving.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf( + userMessage, sizeof(userMessage), + "Axis release was removed while moving. Try resetting the axis " + "and issue the move command again."); + + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorMessageText_", + comAxisNo, __PRETTY_FUNCTION__, + __LINE__); + } + + pollStatus = asynError; + break; + + case 6: + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint( + pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nEmergency stop " + "activated.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), "Emergency stop activated."); + + pollStatus = asynError; + break; + + case 7: + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint(pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nError motor " + "COZ.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), + "Error in motor COZ. Try resetting the axis."); + + pollStatus = asynError; + break; + + case 8: + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint(pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nError motor " + "COM.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), + "Error in motor COM. Try resetting the axis."); + + pollStatus = asynError; + break; + + case 9: + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint(pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nError motor " + "COX.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), + "Error in motor COX. Try resetting the axis."); + + pollStatus = asynError; + break; + + case 10: + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint(pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nHit end " + "switch FTZ.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf( + userMessage, sizeof(userMessage), + "Hit end switch FTZ. Try moving the axis in the other direction."); + + pollStatus = asynError; + break; + + case 11: + // Following error + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint( + pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nMaximum allowed " + "following error FTZ exceeded.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), + "Maximum allowed following error exceeded (P359 = %d). " + "Check if movement range is blocked.", + error); + + pollStatus = asynError; + break; + + case 12: + // Exceeded negative limit for angle + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint(pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nMinimum " + "allowed angle exceeded.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), + "Minimum allowed angle exceeded (P359 = %d). Try moving the " + "axis in the other direction.", + error); + + pollStatus = asynError; + break; + + case 13: + // Exceeded negative limit for angle + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint(pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nMaximum " + "allowed angle exceeded.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), + "Maximum allowed angle exceeded (P359 = %d). Try moving the " + "axis in the other direction.", + error); + + pollStatus = asynError; + break; + + default: + if (getMsgPrintControl().shouldBePrinted(keyError, true, pasynUser())) { + asynPrint( + pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nUnknown error " + "P359 = %d.%s\n", + portName, comAxisNo, __PRETTY_FUNCTION__, __LINE__, error, + getMsgPrintControl().getSuffix()); + } + resetError = false; + + snprintf(userMessage, sizeof(userMessage), "Unknown error P359 = %d.", + error); + + pollStatus = asynError; + break; + } + + if (resetError) { + getMsgPrintControl().resetCount(keyError, pasynUser()); + } + + // Update the parameter library for both axes + if (error != 0) { + + plStatus = angleAxis->setIntegerParam(motorStatusProblem(), true); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorStatusProblem_", + angleAxisNo, __PRETTY_FUNCTION__, + __LINE__); + } + + plStatus = liftAxis->setIntegerParam(motorStatusProblem(), true); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorStatusProblem_", + liftAxisNo, __PRETTY_FUNCTION__, + __LINE__); + } + } + + // Update the user message text + plStatus = angleAxis->setStringParam(motorMessageText(), userMessage); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorMessageText_", angleAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + plStatus = liftAxis->setStringParam(motorMessageText(), userMessage); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorMessageText_", liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + + // Update the working position state PV + plStatus = angleAxis->setIntegerParam(positionStateRBV(), positionState); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "positionStateRBV_", angleAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + plStatus = liftAxis->setIntegerParam(positionStateRBV(), positionState); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "positionStateRBV_", liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + + // The axes are always enabled + plStatus = angleAxis->setIntegerParam(motorEnableRBV(), 1); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorEnableRBV_", angleAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + plStatus = liftAxis->setIntegerParam(motorEnableRBV(), 1); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorEnableRBV_", liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + + // Are the axes currently moving? + plStatus = angleAxis->setIntegerParam(motorStatusMoving(), *moving); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorStatusMoving_", angleAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + plStatus = liftAxis->setIntegerParam(motorStatusMoving(), *moving); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorStatusMoving_", liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + + // Is the axis movement done? + plStatus = angleAxis->setIntegerParam(motorStatusDone(), !(*moving)); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorStatusDone_", angleAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + plStatus = liftAxis->setIntegerParam(motorStatusDone(), !(*moving)); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorStatusDone_", liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + + // In which angleDir are the axes currently moving? + plStatus = angleAxis->setIntegerParam(motorStatusDirection(), angleDir); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorStatusDirection_", + angleAxisNo, __PRETTY_FUNCTION__, __LINE__); + } + plStatus = liftAxis->setIntegerParam(motorStatusDirection(), liftDir); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorStatusDirection_", + liftAxisNo, __PRETTY_FUNCTION__, __LINE__); + } + + // High limits from hardware + plStatus = + setDoubleParam(angleAxisNo, motorHighLimitFromDriver(), angleHighLimit); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorHighLimitFromDriver", + angleAxisNo, __PRETTY_FUNCTION__, __LINE__); + } + plStatus = + setDoubleParam(liftAxisNo, motorHighLimitFromDriver(), liftHighLimit); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorHighLimitFromDriver", + liftAxisNo, __PRETTY_FUNCTION__, __LINE__); + } + + // Low limits from hardware + plStatus = + setDoubleParam(angleAxisNo, motorLowLimitFromDriver(), angleLowLimit); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorLowLimitFromDriver", + angleAxisNo, __PRETTY_FUNCTION__, __LINE__); + } + plStatus = + setDoubleParam(liftAxisNo, motorLowLimitFromDriver(), liftLowLimit); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorLowLimitFromDriver", + liftAxisNo, __PRETTY_FUNCTION__, __LINE__); + } + + // Write the motor origin + plStatus = setDoubleParam(angleAxisNo, motorOrigin(), angleOrigin); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorOrigin", angleAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + plStatus = setDoubleParam(liftAxisNo, motorOrigin(), liftOrigin); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorOrigin", liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + + // Write the lift support origin motor + plStatus = setDoubleParam(liftAxisNo, liftSupportOrigin(), supportOrigin); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "liftSupportOrigin", liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + + // Origin adjustment high limit + plStatus = + setDoubleParam(angleAxisNo, motorAdjustOriginHighLimitFromDriver(), + angleAdjustOriginHighLimit); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorOriginHighLimitFromDriver", + angleAxisNo, __PRETTY_FUNCTION__, __LINE__); + } + plStatus = + setDoubleParam(liftAxisNo, motorAdjustOriginHighLimitFromDriver(), + liftAdjustOriginHighLimit); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, "motorOriginHighLimitFromDriver", + liftAxisNo, __PRETTY_FUNCTION__, __LINE__); + } + + // Origin adjustment low limit + plStatus = + setDoubleParam(angleAxisNo, motorAdjustOriginLowLimitFromDriver(), + angleAdjustOriginLowLimit); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, + "motorAdjustOriginLowLimitFromDriver", + angleAxisNo, __PRETTY_FUNCTION__, __LINE__); + } + plStatus = setDoubleParam(liftAxisNo, motorAdjustOriginLowLimitFromDriver(), + liftAdjustOriginLowLimit); + if (plStatus != asynSuccess) { + return paramLibAccessFailed(plStatus, + "motorAdjustOriginLowLimitFromDriver", + liftAxisNo, __PRETTY_FUNCTION__, __LINE__); + } + + // Axes positions + plStatus = angleAxis->setMotorPosition(angle); + if (plStatus != asynSuccess) { + return plStatus; + } + plStatus = liftAxis->setMotorPosition(lift); + if (plStatus != asynSuccess) { + return plStatus; + } + + // Something went wrong -> Report a status problem + if (pollStatus != asynSuccess) { + plStatus = angleAxis->setIntegerParam(motorStatusProblem(), true); + if (plStatus != asynSuccess) { + paramLibAccessFailed(plStatus, "motorStatusProblem_", angleAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + plStatus = liftAxis->setIntegerParam(motorStatusProblem(), true); + if (plStatus != asynSuccess) { + paramLibAccessFailed(plStatus, "motorStatusProblem_", liftAxisNo, + __PRETTY_FUNCTION__, __LINE__); + } + } + + // Update the parameter library + bool wantToPrint = false; + + plStatus = angleAxis->callParamCallbacks(); + wantToPrint = plStatus != asynSuccess; + if (getMsgPrintControl().shouldBePrinted(portName, angleAxisNo, + __PRETTY_FUNCTION__, __LINE__, + wantToPrint, pasynUser())) { + asynPrint(pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line " + "%d:\ncallParamCallbacks failed with %s.%s\n", + portName, angleAxisNo, __PRETTY_FUNCTION__, __LINE__, + stringifyAsynStatus(pollStatus), + getMsgPrintControl().getSuffix()); + } + if (wantToPrint) { + pollStatus = plStatus; + } + plStatus = liftAxis->callParamCallbacks(); + wantToPrint = plStatus != asynSuccess; + if (getMsgPrintControl().shouldBePrinted(portName, liftAxisNo, + __PRETTY_FUNCTION__, __LINE__, + wantToPrint, pasynUser())) { + asynPrint(pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line " + "%d:\ncallParamCallbacks failed with %s.%s\n", + portName, liftAxisNo, __PRETTY_FUNCTION__, __LINE__, + stringifyAsynStatus(pollStatus), + getMsgPrintControl().getSuffix()); + } + if (wantToPrint) { + pollStatus = plStatus; + } + + return pollStatus; } /*************************************************************************************/ diff --git a/src/detectorTowerController.h b/src/detectorTowerController.h index 6daf0b2..f31d9d7 100644 --- a/src/detectorTowerController.h +++ b/src/detectorTowerController.h @@ -8,8 +8,8 @@ #ifndef detectorTowerController_H #define detectorTowerController_H -#include "detectorTowerAuxiliaryAxis.h" -#include "detectorTowerAxis.h" +#include "detectorTowerAngleAxis.h" +#include "detectorTowerLiftAxis.h" #include "turboPmacController.h" class detectorTowerController : public turboPmacController { @@ -19,7 +19,7 @@ class detectorTowerController : public turboPmacController { * @brief Construct a new detectorTowerController object * * This controller object can handle both "normal" TurboPMAC axes created - with the turboPmacAxis constructor and the special detectorTowerAxis. + with the turboPmacAxis constructor and the special detectorTowerAngleAxis. * * @param portName See sinqController constructor * @param ipPortConfigName See sinqController constructor @@ -65,51 +65,77 @@ class detectorTowerController : public turboPmacController { * @brief Get the axis object * * @param pasynUser Specify the axis via the asynUser - * @return detectorTowerAxis* If no axis could be found, this is a - * nullptr + * @return detectorTowerAngleAxis* If no axis could be found, this is + * a nullptr */ - detectorTowerAxis *getDetectorTowerAxis(asynUser *pasynUser); + detectorTowerAngleAxis *getDetectorTowerAngleAxis(asynUser *pasynUser); /** * @brief Get the axis object * * @param axisNo Specify the axis via its index - * @return detectorTowerAxis* If no axis could be found, this is a - * nullptr + * @return detectorTowerAngleAxis* If no axis could be found, this is + * a nullptr */ - detectorTowerAxis *getDetectorTowerAxis(int axisNo); + detectorTowerAngleAxis *getDetectorTowerAngleAxis(int axisNo); /** * @brief Get the axis object * * @param pasynUser Specify the axis via the asynUser - * @return detectorTowerAuxiliaryAxis* If no axis could be found, + * @return detectorTowerLiftAxis* If no axis could be found, * this is a nullptr */ - detectorTowerAuxiliaryAxis * - getDetectorTowerAuxiliaryAxis(asynUser *pasynUser); + detectorTowerLiftAxis *getDetectorTowerLiftAxis(asynUser *pasynUser); /** * @brief Get the axis object * * @param axisNo Specify the axis via its index - * @return detectorTowerAuxiliaryAxis* If no axis could be found, + * @return detectorTowerLiftAxis* If no axis could be found, * this is a nullptr */ - detectorTowerAuxiliaryAxis *getDetectorTowerAuxiliaryAxis(int axisNo); + detectorTowerLiftAxis *getDetectorTowerLiftAxis(int axisNo); + + /** + * @brief Common poll function for both lift and angle axes + * + * @param moving + * @param angleAxis + * @param liftAxis + * @return asynStatus + */ + asynStatus pollDetectorAxes(bool *moving, detectorTowerAngleAxis *angleAxis, + detectorTowerLiftAxis *liftAxis); // Accessors for additional PVs int positionStateRBV() { return positionStateRBV_; } int changeState() { return changeState_; } - int motorTargetOffset() { return motorTargetOffset_; } + int changeStateRBV() { return changeStateRBV_; } + int motorOrigin() { return motorOrigin_; } + int motorAdjustOrigin() { return motorAdjustOrigin_; } + int motorAdjustOriginHighLimitFromDriver() { + return motorAdjustOriginHighLimitFromDriver_; + } + int motorAdjustOriginLowLimitFromDriver() { + return motorAdjustOriginLowLimitFromDriver_; + } + int liftSupportOrigin() { return liftSupportOrigin_; } + int liftSupportAdjustOrigin() { return liftSupportAdjustOrigin_; } private: // Indices of additional PVs #define FIRST_detectorTower_PARAM positionStateRBV_ int positionStateRBV_; int changeState_; - int motorTargetOffset_; -#define LAST_detectorTower_PARAM motorTargetOffset_ + int changeStateRBV_; + int motorOrigin_; + int motorAdjustOrigin_; + int motorAdjustOriginHighLimitFromDriver_; + int motorAdjustOriginLowLimitFromDriver_; + int liftSupportOrigin_; + int liftSupportAdjustOrigin_; +#define LAST_detectorTower_PARAM liftSupportAdjustOrigin_ }; #define NUM_detectorTower_DRIVER_PARAMS \ (&LAST_detectorTower_PARAM - &FIRST_detectorTower_PARAM + 1) diff --git a/src/detectorTowerLiftAxis.cpp b/src/detectorTowerLiftAxis.cpp new file mode 100644 index 0000000..ae51463 --- /dev/null +++ b/src/detectorTowerLiftAxis.cpp @@ -0,0 +1,308 @@ +#include "detectorTowerLiftAxis.h" +#include "detectorTowerAngleAxis.h" +#include "detectorTowerController.h" +#include "turboPmacController.h" +#include +#include +#include + +/* +Contains all instances of detectorTowerLiftAxis 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) { + detectorTowerLiftAxis *axis = *itA; + axis->init(); + } + } +} + +detectorTowerLiftAxis::detectorTowerLiftAxis(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_->pasynUser(), 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); + } + + // 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); +} + +detectorTowerLiftAxis::~detectorTowerLiftAxis(void) { + // Since the controller memory is managed somewhere else, we don't need to + // clean up the pointer pC here. +} + +asynStatus detectorTowerLiftAxis::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_->pasynUser(), 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__); + } + } + + // Initialize the motorStatusMoving flag + status = setIntegerParam(pC_->motorStatusMoving(), 0); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorStatusMoving_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + return asynSuccess; +} + +// Perform the actual poll +asynStatus detectorTowerLiftAxis::poll(bool *moving) { + asynStatus status = asynSuccess; + + // Is this axis the one with the smallest index? + // If not, just read out the movement status and update *moving + if (axisNo() < angleAxis_->axisNo()) { + status = pC_->pollDetectorAxes(moving, angleAxis_, this); + } else { + status = pC_->getIntegerParam(axisNo(), pC_->motorStatusMoving(), + (int *)moving); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorStatusMoving", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + wasMoving_ = *moving; + return status; +} + +asynStatus detectorTowerLiftAxis::doPoll(bool *moving) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nThe doPoll method " + "of this axis type should not be reachable. This is a bug.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + return asynError; +} + +asynStatus detectorTowerLiftAxis::doMove(double position, int relative, + double min_velocity, + double max_velocity, + double acceleration) { + + double motorRecResolution = 0.0; + asynStatus plStatus = pC_->getDoubleParam( + axisNo_, pC_->motorRecResolution(), &motorRecResolution); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "motorRecResolution_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + // Signal to the deferredMovementCollectorLoop (of the + // detectorTowerAngleAxis) that a movement should be started to the defined + // target position. + targetPosition_ = position * motorRecResolution; + angleAxis_->receivedTarget_ = true; + + return asynSuccess; +} + +asynStatus detectorTowerLiftAxis::stop(double acceleration) { + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rwStatus = asynSuccess; + + // Status of parameter library operations + asynStatus plStatus = asynSuccess; + + char response[pC_->MAXBUF_] = {0}; + + // ========================================================================= + + rwStatus = pC_->writeRead(axisNo_, "P350=8", response, 0); + + if (rwStatus != asynSuccess) { + asynPrint( + pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nStopping the movement " + "failed.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + plStatus = setIntegerParam(pC_->motorStatusProblem(), true); + if (plStatus != asynSuccess) { + return pC_->paramLibAccessFailed(plStatus, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + return asynError; + } + + return rwStatus; +} + +asynStatus detectorTowerLiftAxis::reset() { return angleAxis_->reset(); }; + +asynStatus detectorTowerLiftAxis::adjustOrigin(double newOrigin) { + + asynStatus status = asynSuccess; + char command[pC_->MAXBUF_] = {0}; + char response[pC_->MAXBUF_] = {0}; + int positionState = 0; + + // ========================================================================= + + status = + pC_->getIntegerParam(axisNo_, pC_->positionStateRBV(), &positionState); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "positionStateRBV_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // If the axis is in changer position, it must be moved into working + // position before any move can be started. + bool isInChangerPos = positionState == 2 || positionState == 3; + if (pC_->getMsgPrintControl().shouldBePrinted( + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + isInChangerPos, pC_->pasynUser())) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nAxis cannot be " + "moved because it is moving from working to changer " + "position, is in changer position or is moving from changer " + "to working position.%s\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + pC_->getMsgPrintControl().getSuffix()); + } + + // Set the new adjust for the lift axis + snprintf(command, sizeof(command), "Q356=%lf P350=5", newOrigin); + + // We don't expect an answer + status = pC_->writeRead(axisNo_, command, response, 0); + + if (status != asynSuccess) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nSetting new " + "lift origin %lf failed.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + newOrigin); + status = setIntegerParam(pC_->motorStatusProblem(), true); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + return status; +} + +asynStatus detectorTowerLiftAxis::adjustSupportOrigin(double newOrigin) { + + asynStatus status = asynSuccess; + char command[pC_->MAXBUF_] = {0}; + char response[pC_->MAXBUF_] = {0}; + int positionState = 0; + + // ========================================================================= + + status = + pC_->getIntegerParam(axisNo(), pC_->positionStateRBV(), &positionState); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "positionStateRBV_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // If the axis is in changer position, it must be moved into working + // position before any move can be started. + bool isInChangerPos = positionState == 2 || positionState == 3; + if (pC_->getMsgPrintControl().shouldBePrinted( + pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__, + isInChangerPos, pC_->pasynUser())) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nAxis cannot be " + "moved because it is moving from working to changer " + "position, is in changer position or is moving from changer " + "to working position.%s\n", + pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__, + pC_->getMsgPrintControl().getSuffix()); + } + + // Set the new adjust for the lift axis + snprintf(command, sizeof(command), "Q656=%lf P350=6", newOrigin); + + // We don't expect an answer + status = pC_->writeRead(axisNo_, command, response, 0); + + if (status != asynSuccess) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nSetting new " + "lift origin %lf failed.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + newOrigin); + status = setIntegerParam(pC_->motorStatusProblem(), true); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + return status; +} diff --git a/src/detectorTowerLiftAxis.h b/src/detectorTowerLiftAxis.h new file mode 100644 index 0000000..a85ce76 --- /dev/null +++ b/src/detectorTowerLiftAxis.h @@ -0,0 +1,117 @@ +#ifndef detectorTowerLiftAxis_H +#define detectorTowerLiftAxis_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 detectorTowerAngleAxis; + +class detectorTowerLiftAxis : public turboPmacAxis { + public: + /** + * @brief Construct a new detectorTowerAngleAxis + * + * @param pController Pointer to the associated controller + * @param axisNo Index of the axis + */ + detectorTowerLiftAxis(detectorTowerController *pController, int axisNo); + + /** + * @brief Destroy the turboPmacAxis + * + */ + virtual ~detectorTowerLiftAxis(); + + /** + * @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 Poll all detector tower axes, if this axis is the one with the + * smallest index. + * + * We do not use the doPoll framework from sinqMotor here on purpose, since + * we want to e.g. reset the motorStatusProblem for all axes at once at the + * beginning of the poll. + * + * @param moving + * @return asynStatus + */ + asynStatus poll(bool *moving); + asynStatus doPoll(bool *moving); + + /** + * @brief Set the target value for the detector angle and trigger a position + * collection cycle + * + * @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); + + /** + * @brief Call the reset function of the associated + * `detectorTowerAngleAxis`. + * + * @return asynStatus + */ + asynStatus reset(); + + /** + * @brief Move the axis to the position `newOrigin` and recalibrate + * + * When calling this function, the lift axis moves to `newOrigin` and the + * hardware sets this position as the new origin. + * + * @param origin + * @return asynStatus + */ + asynStatus adjustOrigin(double newOrigin); + + /** + * @brief Move the support motor of the beam lift to the position + * `newOrigin` and recalibrate + * + * When calling this function, the lift axis support motor moves to + * `newOrigin` and the hardware sets this position as the new origin. + * + * @param origin + * @return asynStatus + */ + asynStatus adjustSupportOrigin(double newOrigin); + + /** + * @brief Return a pointer to the associated angle axis + * + * @return detectorTowerAngleAxis* + */ + detectorTowerAngleAxis *angleAxis() { return angleAxis_; } + + protected: + detectorTowerController *pC_; + detectorTowerAngleAxis *angleAxis_; + + private: + friend class detectorTowerAngleAxis; +}; + +#endif diff --git a/turboPmac b/turboPmac new file mode 160000 index 0000000..5298b5e --- /dev/null +++ b/turboPmac @@ -0,0 +1 @@ +Subproject commit 5298b5ef6986dcfee22d8272fb8c059ef78e61f6