#include "detectorTowerController.h" #include "detectorTowerAngleAxis.h" #include "detectorTowerLiftAxis.h" #include "detectorTowerSupportAxis.h" #include #include #include // Necessary to make M_PI available #define _USE_MATH_DEFINES #include /** * @brief Construct a new detectorTowerController object * * @param portName See documentation of sinqController * @param ipPortConfigName See documentation of sinqController * @param numAxes See documentation of sinqController * @param movingPollPeriod See documentation of sinqController * @param idlePollPeriod See documentation of sinqController * @param comTimeout Time after which a communication timeout error * is declared in writeRead (in seconds) * @param extraParams See documentation of sinqController */ detectorTowerController::detectorTowerController( const char *portName, const char *ipPortConfigName, int numAxes, double movingPollPeriod, double idlePollPeriod, double comTimeout) : turboPmacController(portName, ipPortConfigName, numAxes, movingPollPeriod, idlePollPeriod, NUM_detectorTower_DRIVER_PARAMS) { asynStatus status = asynSuccess; status = createParam("POSITION_STATE_RBV", asynParamInt32, &positionStateRBV_); 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("CHANGE_STATE", asynParamInt32, &changeState_); 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("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); } } detectorTowerController::~detectorTowerController() {} asynStatus detectorTowerController::readInt32(asynUser *pasynUser, epicsInt32 *value) { // The detector axes cannot be disabled if (pasynUser->reason == motorCanDisable()) { detectorTowerAngleAxis *aAxis = getDetectorTowerAngleAxis(pasynUser); if (aAxis != nullptr) { *value = 0; return asynSuccess; } detectorTowerLiftAxis *lAxis = getDetectorTowerLiftAxis(pasynUser); if (lAxis != nullptr) { *value = 0; return asynSuccess; } detectorTowerSupportAxis *sAxis = getDetectorTowerSupportAxis(pasynUser); if (sAxis != nullptr) { *value = 0; return asynSuccess; } } return turboPmacController::readInt32(pasynUser, value); } asynStatus detectorTowerController::writeInt32(asynUser *pasynUser, epicsInt32 value) { if (pasynUser->reason == changeState_) { detectorTowerAngleAxis *aAxis = getDetectorTowerAngleAxis(pasynUser); if (aAxis != nullptr) { return aAxis->toggleWorkingChangerState(value); } detectorTowerLiftAxis *lAxis = getDetectorTowerLiftAxis(pasynUser); if (lAxis != nullptr) { return lAxis->angleAxis()->toggleWorkingChangerState(value); } detectorTowerSupportAxis *sAxis = getDetectorTowerSupportAxis(pasynUser); if (sAxis != nullptr) { return sAxis->angleAxis()->toggleWorkingChangerState(value); } } return turboPmacController::writeInt32(pasynUser, value); } asynStatus detectorTowerController::writeFloat64(asynUser *pasynUser, epicsFloat64 value) { int function = pasynUser->reason; asynStatus status = asynSuccess; // ===================================================================== if (function == motorAdjustOrigin_) { // Is this function called by a detector axis? detectorTowerAngleAxis *aAxis = getDetectorTowerAngleAxis(pasynUser); if (aAxis != nullptr) { status = aAxis->adjustOrigin(value); if (status != asynSuccess) { return status; } return turboPmacController::writeFloat64(pasynUser, value); } detectorTowerLiftAxis *lAxis = getDetectorTowerLiftAxis(pasynUser); if (lAxis != nullptr) { status = lAxis->adjustOrigin(value); if (status != asynSuccess) { return status; } return turboPmacController::writeFloat64(pasynUser, value); } detectorTowerSupportAxis *sAxis = getDetectorTowerSupportAxis(pasynUser); if (sAxis != nullptr) { status = sAxis->adjustOrigin(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); } /* 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. */ detectorTowerSupportAxis * detectorTowerController::getDetectorTowerSupportAxis(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 */ detectorTowerSupportAxis * detectorTowerController::getDetectorTowerSupportAxis(int axisNo) { asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo); return dynamic_cast(asynAxis); } asynStatus detectorTowerController::pollDetectorAxes( bool *moving, detectorTowerAngleAxis *angleAxis, detectorTowerLiftAxis *liftAxis, detectorTowerSupportAxis *supportAxis) { // 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 errorMessage[MAXBUF_] = {0}; // User message which was created in one of the other axis methods char waitingErrorMessage[MAXBUF_] = {0}; char response[MAXBUF_] = {0}; int nvals = 0; int angleDir = 0; int liftDir = 0; int error = 0; int positionState = 0; int notInPosition = 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 angleOrigin = 0.0; double angleAdjustOriginHighLimit = 0.0; double angleAdjustOriginLowLimit = 0.0; double supportOrigin = 0.0; int angleAxisNo = angleAxis->axisNo(); int liftAxisNo = liftAxis->axisNo(); int supportAxisNo = supportAxis->axisNo(); /* For messages which in principle concern all axes, use the smallest index */ int comAxisNo = angleAxisNo; if (comAxisNo > liftAxisNo) { comAxisNo = liftAxisNo; } if (comAxisNo > supportAxisNo) { comAxisNo = supportAxisNo; } // ========================================================================= // 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. */ setAxisParamChecked(angleAxis, motorStatusProblem, false); setAxisParamChecked(liftAxis, motorStatusProblem, false); setAxisParamChecked(supportAxis, motorStatusProblem, false); setAxisParamChecked(angleAxis, motorStatusCommsError, false); setAxisParamChecked(liftAxis, motorStatusCommsError, false); setAxisParamChecked(supportAxis, motorStatusCommsError, false); // Read the previous motor positions plStatus = angleAxis->motorPosition(&prevAngle); if (plStatus != asynSuccess) { return plStatus; } plStatus = liftAxis->motorPosition(&prevLift); if (plStatus != asynSuccess) { return plStatus; } /* 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; } 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__); } /* 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 getAxisParamChecked(angleAxis, motorLimitsOffset, &limitsOffset); angleHighLimit = angleHighLimit - limitsOffset; angleLowLimit = angleLowLimit + limitsOffset; // Lift getAxisParamChecked(liftAxis, motorLimitsOffset, &limitsOffset); liftHighLimit = liftHighLimit - limitsOffset; liftLowLimit = liftLowLimit + limitsOffset; // Store the axis error angleAxis->error_ = error; // Check if the tower is moving if (notInPosition == 1 || positionState > 2) { *moving = true; // Since the tower is now moving, the axis by definition do not wait for // movement start anymore. liftAxis->waitingForStart_ = false; angleAxis->waitingForStart_ = false; } 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; snprintf(errorMessage, sizeof(errorMessage), "Reset one of the tower axes."); 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(errorMessage, sizeof(errorMessage), "Unknown state P358 = %d has been reached. Please call " "the support.", positionState); 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(errorMessage, sizeof(errorMessage), "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(errorMessage, sizeof(errorMessage), "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(errorMessage, sizeof(errorMessage), "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(errorMessage, sizeof(errorMessage), "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( errorMessage, sizeof(errorMessage), "Axis release was removed while moving. Try resetting the axis " "and issue the move command again."); 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(errorMessage, sizeof(errorMessage), "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(errorMessage, sizeof(errorMessage), "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(errorMessage, sizeof(errorMessage), "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(errorMessage, sizeof(errorMessage), "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( errorMessage, sizeof(errorMessage), "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(errorMessage, sizeof(errorMessage), "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(errorMessage, sizeof(errorMessage), "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(errorMessage, sizeof(errorMessage), "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(errorMessage, sizeof(errorMessage), "Unknown error P359 = %d.", error); pollStatus = asynError; break; } if (resetError) { getMsgPrintControl().resetCount(keyError, pasynUser()); } /* Does the paramLib already contain an error message? If either this is the case or if error != 0, report a status problem for all axes. */ getAxisParamChecked(angleAxis, motorMessageText, &waitingErrorMessage); if (error != 0 || errorMessage[0] != '\0' || waitingErrorMessage[0] != '\0') { setAxisParamChecked(angleAxis, motorStatusProblem, true); setAxisParamChecked(liftAxis, motorStatusProblem, true); setAxisParamChecked(supportAxis, motorStatusProblem, true); } // Update the error message text with the one created in this poll (in case // it is not empty). if (errorMessage[0] != '\0') { setAxisParamChecked(angleAxis, motorMessageText, errorMessage); setAxisParamChecked(liftAxis, motorMessageText, errorMessage); setAxisParamChecked(supportAxis, motorMessageText, errorMessage); } // Update the working position state PV setAxisParamChecked(angleAxis, positionStateRBV, positionState); setAxisParamChecked(liftAxis, positionStateRBV, positionState); setAxisParamChecked(supportAxis, positionStateRBV, positionState); // The axes are always enabled setAxisParamChecked(angleAxis, motorEnableRBV, true); setAxisParamChecked(liftAxis, motorEnableRBV, true); setAxisParamChecked(supportAxis, motorEnableRBV, true); // If the axis is starting a movement, mark it as moving. Otherwise use the // moving parameter. if (angleAxis->waitingForStart_) { setAxisParamChecked(angleAxis, motorStatusMoving, true); setAxisParamChecked(angleAxis, motorStatusDone, false); } else { setAxisParamChecked(angleAxis, motorStatusMoving, *moving); setAxisParamChecked(angleAxis, motorStatusDone, !(*moving)); } if (liftAxis->waitingForStart_) { setAxisParamChecked(liftAxis, motorStatusMoving, true); setAxisParamChecked(liftAxis, motorStatusDone, false); } else { setAxisParamChecked(liftAxis, motorStatusMoving, *moving); setAxisParamChecked(liftAxis, motorStatusDone, !(*moving)); } // Support axis status is solely deduced from system movement state setAxisParamChecked(supportAxis, motorStatusMoving, *moving); setAxisParamChecked(supportAxis, motorStatusDone, !(*moving)); // In which direction are the axes currently moving? setAxisParamChecked(angleAxis, motorStatusDirection, angleDir); setAxisParamChecked(liftAxis, motorStatusDirection, liftDir); // Using the lift direction for the support axis is done on purpose! setAxisParamChecked(supportAxis, motorStatusDirection, liftDir); // High limits from hardware setAxisParamChecked(angleAxis, motorHighLimitFromDriver, angleHighLimit); setAxisParamChecked(liftAxis, motorHighLimitFromDriver, liftHighLimit); // Using the lift high limit for the support axis is done on purpose! setAxisParamChecked(supportAxis, motorHighLimitFromDriver, liftHighLimit); // Low limits from hardware setAxisParamChecked(angleAxis, motorLowLimitFromDriver, angleLowLimit); setAxisParamChecked(liftAxis, motorLowLimitFromDriver, liftLowLimit); // Using the lift low limit for the support axis is done on purpose! setAxisParamChecked(supportAxis, motorLowLimitFromDriver, liftLowLimit); // Write the motor origin setAxisParamChecked(angleAxis, motorOrigin, angleOrigin); setAxisParamChecked(liftAxis, motorOrigin, liftOrigin); setAxisParamChecked(supportAxis, motorOrigin, supportOrigin); // Origin adjustment high limit setAxisParamChecked(angleAxis, motorAdjustOriginHighLimitFromDriver, angleAdjustOriginHighLimit); setAxisParamChecked(liftAxis, motorAdjustOriginHighLimitFromDriver, liftAdjustOriginHighLimit); // Using the lift high limit for the support axis is done on purpose! setAxisParamChecked(supportAxis, motorAdjustOriginHighLimitFromDriver, liftAdjustOriginHighLimit); // Origin adjustment low limit setAxisParamChecked(angleAxis, motorAdjustOriginLowLimitFromDriver, angleAdjustOriginLowLimit); setAxisParamChecked(liftAxis, motorAdjustOriginLowLimitFromDriver, liftAdjustOriginLowLimit); // Using the lift low limit for the support axis is done on purpose! setAxisParamChecked(supportAxis, motorAdjustOriginLowLimitFromDriver, liftAdjustOriginLowLimit); // Axes positions plStatus = angleAxis->setMotorPosition(angle); if (plStatus != asynSuccess) { return plStatus; } plStatus = liftAxis->setMotorPosition(lift); if (plStatus != asynSuccess) { return plStatus; } // Using the lift position for the support axis is done on purpose! plStatus = supportAxis->setMotorPosition(lift); if (plStatus != asynSuccess) { return plStatus; } // Update the parameter library for all three axes 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; } plStatus = supportAxis->callParamCallbacks(); wantToPrint = plStatus != asynSuccess; if (getMsgPrintControl().shouldBePrinted(portName, supportAxisNo, __PRETTY_FUNCTION__, __LINE__, wantToPrint, pasynUser())) { asynPrint(pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line " "%d:\ncallParamCallbacks failed with %s.%s\n", portName, supportAxisNo, __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(pollStatus), getMsgPrintControl().getSuffix()); } if (wantToPrint) { pollStatus = plStatus; } /* Reset the message text for the next poll cycle AFTER updating the PVs. This makes sure that non-persistent error messages are shown for at least one poll cycle, but are cleared afterwards. Persisting error messages will be recreated during each poll. */ setAxisParamChecked(angleAxis, motorMessageText, ""); setAxisParamChecked(liftAxis, motorMessageText, ""); setAxisParamChecked(supportAxis, motorMessageText, ""); return pollStatus; } /*************************************************************************************/ /** The following functions are C-wrappers, and can be called directly from * iocsh */ extern "C" { /* C wrapper for the controller constructor. Please refer to the detectorTowerController constructor documentation. */ asynStatus detectorTowerCreateController(const char *portName, const char *ipPortConfigName, int numAxes, double movingPollPeriod, double idlePollPeriod, double comTimeout) { /* We create a new instance of the controller, 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. */ #pragma GCC diagnostic ignored "-Wunused-but-set-variable" #pragma GCC diagnostic ignored "-Wunused-variable" detectorTowerController *pController = new detectorTowerController( portName, ipPortConfigName, numAxes, movingPollPeriod, idlePollPeriod, comTimeout); return asynSuccess; } /* Define name and type of the arguments for the CreateController function in the iocsh. This is done by creating structs with the argument names and types and then providing "factory" functions (configCreateControllerCallFunc). These factory functions are used to register the constructors during compilation. */ static const iocshArg CreateControllerArg0 = {"Controller name (e.g. mcu1)", iocshArgString}; static const iocshArg CreateControllerArg1 = {"Asyn IP port name (e.g. pmcu1)", iocshArgString}; static const iocshArg CreateControllerArg2 = {"Number of axes", iocshArgInt}; static const iocshArg CreateControllerArg3 = {"Moving poll rate (s)", iocshArgDouble}; static const iocshArg CreateControllerArg4 = {"Idle poll rate (s)", iocshArgDouble}; static const iocshArg CreateControllerArg5 = {"Communication timeout (s)", iocshArgDouble}; static const iocshArg *const CreateControllerArgs[] = { &CreateControllerArg0, &CreateControllerArg1, &CreateControllerArg2, &CreateControllerArg3, &CreateControllerArg4, &CreateControllerArg5}; static const iocshFuncDef configDetectorTowerCreateController = { "detectorTowerController", 6, CreateControllerArgs}; static void configDetectorTowerCreateControllerCallFunc(const iocshArgBuf *args) { detectorTowerCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].dval, args[4].dval, args[5].dval); } // This function is made known to EPICS in turboPmac.dbd and is called by EPICS // in order to register both functions in the IOC shell static void detectorTowerControllerRegister(void) { iocshRegister(&configDetectorTowerCreateController, configDetectorTowerCreateControllerCallFunc); } epicsExportRegistrar(detectorTowerControllerRegister); } // extern "C"