1024 lines
38 KiB
C++
1024 lines
38 KiB
C++
#include "detectorTowerController.h"
|
|
#include "detectorTowerAngleAxis.h"
|
|
#include "detectorTowerLiftAxis.h"
|
|
#include "detectorTowerSupportAxis.h"
|
|
#include <epicsExport.h>
|
|
#include <errlog.h>
|
|
#include <iocsh.h>
|
|
|
|
// Necessary to make M_PI available
|
|
#define _USE_MATH_DEFINES
|
|
#include <math.h>
|
|
|
|
/**
|
|
* @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<detectorTowerAngleAxis *>(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<detectorTowerAngleAxis *>(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<detectorTowerLiftAxis *>(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<detectorTowerLiftAxis *>(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<detectorTowerSupportAxis *>(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<detectorTowerSupportAxis *>(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"
|