404 lines
15 KiB
C++
404 lines
15 KiB
C++
#include "seleneOffsetAxis.h"
|
|
#include "epicsExport.h"
|
|
#include "iocsh.h"
|
|
#include "seleneGuideController.h"
|
|
#include "seleneLiftAxis.h"
|
|
#include "turboPmacController.h"
|
|
#include <errlog.h>
|
|
|
|
/*
|
|
Contains all instances of seleneLiftAxis which have been created and is used in
|
|
the initialization hook function.
|
|
*/
|
|
static std::vector<seleneOffsetAxis *> 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<seleneOffsetAxis *>::iterator itA = axes.begin();
|
|
itA != axes.end(); ++itA) {
|
|
seleneOffsetAxis *axis = *itA;
|
|
axis->init();
|
|
}
|
|
}
|
|
}
|
|
|
|
seleneOffsetAxis::seleneOffsetAxis(seleneGuideController *pController,
|
|
int axisNo, double xPos, double zPos)
|
|
: turboPmacAxis(pController, axisNo, false), pC_(pController) {
|
|
asynStatus status = asynSuccess;
|
|
|
|
xOffset_ = xPos;
|
|
zOffset_ = zPos;
|
|
|
|
// Placeholders, will be overwritten later
|
|
liftPosition_ = 0.0;
|
|
targetSet_ = false;
|
|
absolutePositionLiftCS_ = 0.0;
|
|
highLimit_ = 0.0;
|
|
lowLimit_ = 0.0;
|
|
distHighLimit_ = 0.0;
|
|
distLowLimit_ = 0.0;
|
|
absoluteHighLimitLiftCS_ = 0.0;
|
|
absoluteLowLimitLiftCS_ = 0.0;
|
|
|
|
status = pC_->setIntegerParam(axisNo_, pC_->motorStatusDone(), 1);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
status = pC_->setIntegerParam(axisNo_, pC_->motorStatusMoving(), 0);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorEncoderPosition(), 0.0);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
// 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);
|
|
|
|
// Selene guide motors cannot be disabled
|
|
status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable(), 0);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
// Even though this happens already in sinqAxis, a default value for
|
|
// motorMessageText is set here again, because apparently the sinqAxis
|
|
// constructor is not run before the string is accessed?
|
|
status = setStringParam(pC_->motorMessageText(), "");
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
}
|
|
|
|
seleneOffsetAxis::~seleneOffsetAxis() {}
|
|
|
|
asynStatus seleneOffsetAxis::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__);
|
|
}
|
|
}
|
|
|
|
// Read the initial limits from the paramlib
|
|
getAxisParamChecked(this, motorHighLimit, &highLimit_);
|
|
getAxisParamChecked(this, motorLowLimit, &lowLimit_);
|
|
|
|
// The high limits read from the paramLib are divided by motorRecResolution,
|
|
// hence we need to multiply them to undo this change.
|
|
highLimit_ = highLimit_ * motorRecResolution;
|
|
lowLimit_ = lowLimit_ * motorRecResolution;
|
|
|
|
return status;
|
|
}
|
|
|
|
double seleneOffsetAxis::targetPosition() {
|
|
if (targetSet_) {
|
|
return turboPmacAxis::targetPosition();
|
|
} else {
|
|
double targetPos = 0;
|
|
motorPosition(&targetPos);
|
|
return targetPos;
|
|
}
|
|
}
|
|
|
|
asynStatus seleneOffsetAxis::doPoll(bool *moving) {
|
|
asynStatus status = asynSuccess;
|
|
asynStatus errorStatus = asynSuccess;
|
|
double highLimit = 0.0;
|
|
double lowLimit = 0.0;
|
|
double encoderPosition = 0.0;
|
|
int isMoving = 0;
|
|
int error = 0;
|
|
int nvals = 0;
|
|
|
|
char command[pC_->MAXBUF_] = {0};
|
|
char response[pC_->MAXBUF_] = {0};
|
|
char userMessage[pC_->MAXBUF_] = {0};
|
|
|
|
// =========================================================================
|
|
|
|
/*
|
|
Read the following informations:
|
|
- Whether the axis is moving (P158)
|
|
- Axis position (absolute encoder value!)
|
|
- Error code
|
|
- Absolute high limit
|
|
- Absolute low limit
|
|
*/
|
|
snprintf(command, sizeof(command),
|
|
"P158 Q%2.2d10 P%2.2d01 Q%2.2d13 Q%2.2d14", axisNo_, axisNo_,
|
|
axisNo_, axisNo_);
|
|
status = pC_->writeRead(axisNo_, command, response, 5);
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
|
|
nvals = sscanf(response, "%d %lf %d %lf %lf", &isMoving, &encoderPosition,
|
|
&error, &highLimit, &lowLimit);
|
|
if (nvals != 5) {
|
|
return pC_->couldNotParseResponse(command, response, axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
*moving = (isMoving != 0);
|
|
|
|
status = setPositionsFromEncoderPosition(encoderPosition);
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
|
|
// =========================================================================
|
|
// Update the parameter library
|
|
|
|
errorStatus = handleError(error, userMessage, sizeof(userMessage));
|
|
|
|
// Update the parameter library
|
|
if (error != 0) {
|
|
setAxisParamChecked(this, motorMessageText, userMessage);
|
|
setAxisParamChecked(this, motorStatusProblem, true);
|
|
}
|
|
|
|
// Convert into lift coordinate system
|
|
absoluteHighLimitLiftCS_ = highLimit - zOffset_;
|
|
absoluteLowLimitLiftCS_ = lowLimit - zOffset_;
|
|
|
|
// Calculate the distance of the axis to its upper and lower limits
|
|
distHighLimit_ = highLimit - encoderPosition;
|
|
distLowLimit_ = encoderPosition - lowLimit;
|
|
|
|
// Check if the relative limits need to be shrunken because we are too close
|
|
// to the absolute limit
|
|
double relHighLimit = highLimit_;
|
|
double relLowLimit = lowLimit_;
|
|
if ((highLimit - encoderPosition) < relHighLimit) {
|
|
relHighLimit = highLimit - encoderPosition;
|
|
}
|
|
if ((lowLimit - encoderPosition) > relLowLimit) {
|
|
relLowLimit = lowLimit - encoderPosition;
|
|
}
|
|
|
|
setAxisParamChecked(this, motorHighLimitFromDriver, relHighLimit);
|
|
setAxisParamChecked(this, motorLowLimitFromDriver, relLowLimit);
|
|
|
|
/*
|
|
If this axis is moving as a result of an individual move command to the axis
|
|
alone, recalculate the RBV value as the difference between the absolute
|
|
position and the lift position.
|
|
*/
|
|
if (*moving && !liftAxis_->virtualAxisMovement()) {
|
|
status = setMotorPosition(absolutePositionLiftCS_ - liftPosition_);
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
}
|
|
|
|
setAxisParamChecked(this, motorStatusMoving, *moving);
|
|
setAxisParamChecked(this, motorStatusDone, !(*moving));
|
|
|
|
// Axis is always shown as enabled, since it is enabled automatically by the
|
|
// controller when a move command P150=1 is sent and disabled once the
|
|
// movement is completed.
|
|
setAxisParamChecked(this, motorEnableRBV, true);
|
|
|
|
return errorStatus;
|
|
}
|
|
|
|
asynStatus
|
|
seleneOffsetAxis::setPositionsFromEncoderPosition(double encoderPosition) {
|
|
setAxisParamChecked(this, motorAbsolutePositionRBV, encoderPosition);
|
|
absolutePositionLiftCS_ = encoderPosition - zOffset_;
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus seleneOffsetAxis::doMove(double position, int relative,
|
|
double min_velocity,
|
|
double maxOffset_velocity,
|
|
double acceleration) {
|
|
|
|
double motorRecResolution = 0.0;
|
|
getAxisParamChecked(this, motorRecResolution, &motorRecResolution);
|
|
|
|
// Set the target position
|
|
setTargetPosition(position * motorRecResolution);
|
|
targetSet_ = true;
|
|
asynStatus status = liftAxis_->startCombinedMoveFromOffsetAxis();
|
|
targetSet_ = false;
|
|
return status;
|
|
}
|
|
|
|
asynStatus seleneOffsetAxis::enable(bool on) {
|
|
setAxisParamChecked(this, motorMessageText,
|
|
"Axis cannot be enabled / disabled");
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus seleneOffsetAxis::normalizePositions() {
|
|
return liftAxis_->normalizePositions();
|
|
}
|
|
|
|
/*************************************************************************************/
|
|
/** 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 seleneOffsetAxisCreateAxis(const char *portName, int axisNo,
|
|
double xPos, double zPos) {
|
|
|
|
/*
|
|
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;
|
|
}
|
|
asynPortDriver *apd = (asynPortDriver *)(ptr);
|
|
|
|
// Safe downcast
|
|
seleneGuideController *pC = dynamic_cast<seleneGuideController *>(apd);
|
|
if (pC == nullptr) {
|
|
errlogPrintf("Controller \"%s\" => %s, line %d\nController "
|
|
"is not a seleneGuideController.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__);
|
|
return asynError;
|
|
}
|
|
|
|
// Prevent manipulation of the controller from other threads while we
|
|
// create the new axis.
|
|
pC->lock();
|
|
|
|
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
|
|
#pragma GCC diagnostic ignored "-Wunused-variable"
|
|
seleneOffsetAxis *pAxis = new seleneOffsetAxis(pC, axisNo, xPos, zPos);
|
|
|
|
// 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 axis", iocshArgInt};
|
|
static const iocshArg CreateAxisArg2 = {"X-coordinate of the axis",
|
|
iocshArgDouble};
|
|
static const iocshArg CreateAxisArg3 = {"Z-coordinate of the axis",
|
|
iocshArgDouble};
|
|
|
|
static const iocshArg *const CreateAxisArgs[] = {
|
|
&CreateAxisArg0,
|
|
&CreateAxisArg1,
|
|
&CreateAxisArg2,
|
|
&CreateAxisArg3,
|
|
};
|
|
static const iocshFuncDef configSeleneOffsetAxisCreateAxis = {
|
|
"seleneOffsetAxis", 4, CreateAxisArgs};
|
|
static void configSeleneOffsetAxisCreateAxisCallFunc(const iocshArgBuf *args) {
|
|
seleneOffsetAxisCreateAxis(args[0].sval, args[1].ival, args[2].dval,
|
|
args[3].dval);
|
|
}
|
|
|
|
// =============================================================================
|
|
|
|
// This function is made known to EPICS in seleneLift.dbd and is
|
|
// called by EPICS in order to register both functions in the IOC shell
|
|
static void seleneOffsetAxisRegister(void) {
|
|
iocshRegister(&configSeleneOffsetAxisCreateAxis,
|
|
configSeleneOffsetAxisCreateAxisCallFunc);
|
|
}
|
|
epicsExportRegistrar(seleneOffsetAxisRegister);
|
|
|
|
} // extern "C"
|