858 lines
34 KiB
C++
858 lines
34 KiB
C++
#include "sinqAxis.h"
|
|
#include "epicsExport.h"
|
|
#include "iocsh.h"
|
|
#include "sinqController.h"
|
|
#include <errlog.h>
|
|
#include <math.h>
|
|
#include <unistd.h>
|
|
|
|
sinqAxis::sinqAxis(class sinqController *pC, int axisNo)
|
|
: asynMotorAxis((asynMotorController *)pC, axisNo), pC_(pC) {
|
|
asynStatus status = asynSuccess;
|
|
|
|
watchdogMovActive_ = false;
|
|
scaleMovTimeout_ = 2.0;
|
|
offsetMovTimeout_ = 30;
|
|
targetPosition_ = 0.0;
|
|
wasMoving_ = false;
|
|
|
|
epicsTimeGetCurrent(&lastPollTime_);
|
|
|
|
// This check is also done in asynMotorAxis, but there the IOC continues
|
|
// running even though the configuration is incorrect. When failing this
|
|
// check, the IOC is stopped, since this is definitely a configuration
|
|
// problem.
|
|
if ((axisNo < 0) || (axisNo >= pC->numAxes())) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(axis index %d is not in range 0 to %d)\n. Terminating IOC",
|
|
pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, axisNo,
|
|
pC->numAxes() - 1);
|
|
exit(-1);
|
|
}
|
|
|
|
// Motor is assumed to be enabled
|
|
status = setIntegerParam(pC_->motorEnableRBV(), 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);
|
|
}
|
|
|
|
// By default, motors cannot be disabled
|
|
status = setIntegerParam(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);
|
|
}
|
|
|
|
// Provide a default value for the motor position.
|
|
status = setDoubleParam(pC_->motorPosition(), 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);
|
|
}
|
|
|
|
// We assume that the motor has no status problems initially
|
|
status = setIntegerParam(pC_->motorStatusProblem(), 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);
|
|
}
|
|
|
|
// Set the homing-related flags
|
|
status = setIntegerParam(pC_->motorStatusHome(), 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 = setIntegerParam(pC_->motorStatusHomed(), 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 = setIntegerParam(pC_->motorStatusAtHome(), 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);
|
|
}
|
|
}
|
|
|
|
asynStatus sinqAxis::poll(bool *moving) {
|
|
// Local variable declaration
|
|
asynStatus pl_status = asynSuccess;
|
|
asynStatus poll_status = asynSuccess;
|
|
int homing = 0;
|
|
int homed = 0;
|
|
int adaptivePolling = 0;
|
|
|
|
/*
|
|
If adaptive polling is enabled:
|
|
- Check if the motor was moving during the last poll
|
|
- If yes, perform the poll
|
|
- If no, check if the last poll was at least an idlePollPeriod ago
|
|
- If yes, perform the poll
|
|
- If no, skip it
|
|
*/
|
|
|
|
pl_status =
|
|
pC_->getIntegerParam(axisNo_, pC_->adaptivePolling(), &adaptivePolling);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "adaptivePolling", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
};
|
|
|
|
// Using the EPICS timestamp here, see
|
|
// https://docs.epics-controls.org/projects/base/en/latest/epicsTime_h.html#_CPPv414epicsTimeStamp
|
|
|
|
// Get the current time
|
|
epicsTimeStamp ts;
|
|
epicsTimeGetCurrent(&ts);
|
|
|
|
if (adaptivePolling != 0) {
|
|
// Motor wasn't moving during the last poll
|
|
if (!wasMoving_) {
|
|
|
|
// Add the idle poll period
|
|
epicsTimeStamp earliestTimeNextPoll = lastPollTime_;
|
|
epicsTimeAddSeconds(&earliestTimeNextPoll, pC_->idlePollPeriod());
|
|
|
|
if (epicsTimeLessThanEqual(&earliestTimeNextPoll, &ts) == 0) {
|
|
*moving = false;
|
|
return asynSuccess;
|
|
}
|
|
}
|
|
}
|
|
// Update the start time of the last poll
|
|
lastPollTime_ = ts;
|
|
|
|
/*
|
|
At the beginning of the poll, it is assumed that the axis has 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.
|
|
*/
|
|
pl_status = setIntegerParam(pC_->motorStatusProblem(), false);
|
|
if (pl_status != asynSuccess) {
|
|
pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
pl_status = setIntegerParam(pC_->motorStatusCommsError(), false);
|
|
if (pl_status != asynSuccess) {
|
|
pC_->paramLibAccessFailed(pl_status, "motorStatusCommsError_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
pl_status = setStringParam(pC_->motorMessageText(), "");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
// The poll function is just a wrapper around doPoll and
|
|
// handles mainly the callParamCallbacks() function. This wrapper is used
|
|
// to make sure callParamCallbacks() is called in case of a premature
|
|
// return.
|
|
poll_status = doPoll(moving);
|
|
|
|
// Motor was moving during this poll
|
|
wasMoving_ = *moving;
|
|
|
|
// The poll did not succeed: Something went wrong and the motor has a status
|
|
// problem.
|
|
if (poll_status != asynSuccess) {
|
|
pl_status = setIntegerParam(pC_->motorStatusProblem(), true);
|
|
if (pl_status != asynSuccess) {
|
|
pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
}
|
|
|
|
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorStatusHomed(), &homed);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusHomed_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorStatusHome(), &homing);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusHome_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
if (homing == 1 && !(*moving)) {
|
|
|
|
// Set the homing-related flags
|
|
pl_status = setIntegerParam(pC_->motorStatusHome(), 0);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusHome_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
pl_status = setIntegerParam(pC_->motorStatusHomed(), 1);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusHomed_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
pl_status = setIntegerParam(pC_->motorStatusAtHome(), 1);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusAtHome_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
}
|
|
|
|
// Check and update the watchdog
|
|
if (checkMovTimeoutWatchdog(*moving) != asynSuccess) {
|
|
return asynError;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
return poll_status;
|
|
}
|
|
|
|
asynStatus sinqAxis::doPoll(bool *moving) { return asynSuccess; }
|
|
|
|
asynStatus sinqAxis::move(double position, int relative, double minVelocity,
|
|
double maxVelocity, double acceleration) {
|
|
|
|
// Status of parameter library operations
|
|
asynStatus status = asynSuccess;
|
|
double motorRecResolution = 0.0;
|
|
|
|
// =========================================================================
|
|
|
|
// Store the target position internally
|
|
targetPosition_ = position * motorRecResolution;
|
|
|
|
status = doMove(position, relative, minVelocity, maxVelocity, acceleration);
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
|
|
// Since the move command was successfull, we assume that the motor has
|
|
// started its movement.
|
|
status = setIntegerParam(pC_->motorStatusHomed(), 0);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorStatusHomed_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
status = setIntegerParam(pC_->motorStatusAtHome(), 0);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorStatusAtHome_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
|
|
&motorRecResolution);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorRecResolution_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Needed for adaptive polling
|
|
wasMoving_ = true;
|
|
|
|
return pC_->callParamCallbacks();
|
|
}
|
|
|
|
asynStatus sinqAxis::doMove(double position, int relative, double minVelocity,
|
|
double maxVelocity, double acceleration) {
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::home(double minVelocity, double maxVelocity,
|
|
double acceleration, int forwards) {
|
|
|
|
asynStatus status = asynSuccess;
|
|
|
|
status = doHome(minVelocity, maxVelocity, acceleration, forwards);
|
|
|
|
if (status == asynSuccess) {
|
|
|
|
status = setStringParam(pC_->motorMessageText(), "Homing");
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
// Set the homing-related flags
|
|
status = setIntegerParam(pC_->motorStatusHome(), 1);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorStatusHome_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
// Set field ATHM to zero
|
|
status = setIntegerParam(pC_->motorStatusHomed(), 0);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorStatusHomed_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
status = setIntegerParam(pC_->motorStatusAtHome(), 0);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorStatusAtHome_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
// Update the motor record
|
|
return callParamCallbacks();
|
|
|
|
} else if (status == asynError) {
|
|
// asynError means that we tried to home an absolute encoder
|
|
status = setStringParam(pC_->motorMessageText(),
|
|
"Can't home a motor with absolute encoder");
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
// Update the motor record
|
|
return callParamCallbacks();
|
|
} else {
|
|
// Bubble up all other problems
|
|
return status;
|
|
}
|
|
}
|
|
|
|
asynStatus sinqAxis::doHome(double minVelocity, double maxVelocity,
|
|
double acceleration, int forwards) {
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::reset() {
|
|
asynStatus status = doReset();
|
|
if (status == asynSuccess) {
|
|
// Perform some fast polls
|
|
pC_->lock();
|
|
bool moving = false;
|
|
for (int i = 0; i < 5; i++) {
|
|
epicsThreadSleep(pC_->movingPollPeriod());
|
|
if (poll(&moving) == asynSuccess) {
|
|
break;
|
|
}
|
|
}
|
|
pC_->unlock();
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::doReset() { return asynError; }
|
|
|
|
asynStatus sinqAxis::enable(bool on) { return asynSuccess; }
|
|
|
|
asynStatus sinqAxis::motorPosition(double *motorPosition) {
|
|
asynStatus status = asynSuccess;
|
|
double motorRecResolution = 0.0;
|
|
|
|
status = pC_->getDoubleParam(axisNo(), pC_->motorRecResolution(),
|
|
&motorRecResolution);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorRecResolution_",
|
|
axisNo(), __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = pC_->getDoubleParam(axisNo(), pC_->motorPosition(), motorPosition);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorPosition_", axisNo(),
|
|
__PRETTY_FUNCTION__,
|
|
|
|
__LINE__);
|
|
}
|
|
*motorPosition = *motorPosition * motorRecResolution;
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::setMotorPosition(double motorPosition) {
|
|
asynStatus status = asynSuccess;
|
|
double motorRecResolution = 0.0;
|
|
|
|
status = pC_->getDoubleParam(axisNo(), pC_->motorRecResolution(),
|
|
&motorRecResolution);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorRecResolution_",
|
|
axisNo(), __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = setDoubleParam(pC_->motorPosition(),
|
|
motorPosition / motorRecResolution);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorPosition_", axisNo(),
|
|
__PRETTY_FUNCTION__,
|
|
|
|
__LINE__);
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::setVeloFields(double velo, double vbas, double vmax) {
|
|
asynStatus status = asynSuccess;
|
|
int variableSpeed = 0;
|
|
|
|
// Can the speed of the motor be varied?
|
|
status =
|
|
pC_->getIntegerParam(axisNo_, pC_->motorCanSetSpeed(), &variableSpeed);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorCanSetSpeed_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
if (variableSpeed == 1) {
|
|
|
|
// Check the inputs and create corresponding error messages
|
|
if (vbas > vmax) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nLower speed "
|
|
"limit vbas=%lf must not be smaller than upper limit "
|
|
"vmax=%lf.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
vbas, vmax);
|
|
|
|
status = setStringParam(
|
|
pC_->motorMessageText(),
|
|
"Lower speed limit must not be smaller than upper speed limit");
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
return asynError;
|
|
}
|
|
if (velo < vbas || velo > vmax) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nActual "
|
|
"speed velo=%lf must be between lower limit vbas=%lf and "
|
|
"upper limit vmax=%lf.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
velo, vbas, vmax);
|
|
|
|
status = setStringParam(pC_->motorMessageText(),
|
|
"Speed is not inside limits");
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
return asynError;
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver(), vbas);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVeloFromDriver(), velo);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVeloFromDriver_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver(), vmax);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
} else {
|
|
// Set minimum and maximum speed equal to the set speed
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver(), velo);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVeloFromDriver(), velo);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVeloFromDriver_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver(), velo);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::setAcclField(double accl) {
|
|
|
|
if (accl <= 0.0) {
|
|
return asynError;
|
|
}
|
|
|
|
asynStatus status = setDoubleParam(pC_->motorAcclFromDriver(), accl);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorAcclFromDriver_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::setWatchdogEnabled(bool enable) {
|
|
return setIntegerParam(pC_->motorEnableMovWatchdog(), enable);
|
|
}
|
|
|
|
asynStatus sinqAxis::startMovTimeoutWatchdog() {
|
|
asynStatus pl_status;
|
|
int enableMovWatchdog = 0;
|
|
|
|
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableMovWatchdog(),
|
|
&enableMovWatchdog);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorEnableMovWatchdog_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
if (enableMovWatchdog == 1) {
|
|
// These parameters are only needed in this branch
|
|
double motorPos = 0.0;
|
|
double motorVelocity = 0.0;
|
|
double motorVelocityRec = 0.0;
|
|
double motorAccel = 0.0;
|
|
double motorAccelRec = 0.0;
|
|
double motorRecResolution = 0.0;
|
|
time_t timeContSpeed = 0;
|
|
time_t timeAccel = 0;
|
|
|
|
// Activate the watchdog
|
|
watchdogMovActive_ = true;
|
|
|
|
/*
|
|
NOTE: This function must not be called in the constructor (e.g. in order
|
|
to save the read result to the member variable earlier), since the
|
|
parameter library is updated at a later stage!
|
|
*/
|
|
pl_status = motorPosition(&motorPos);
|
|
if (pl_status != asynSuccess) {
|
|
return pl_status;
|
|
}
|
|
|
|
/*
|
|
We use motorVelocity, which corresponds to the record field VELO.
|
|
From https://epics.anl.gov/docs/APS2015/14-Motor-Record.pdf:
|
|
* VELO = motorVelocity_ = Slew velocity
|
|
* VBAS = motorVelBase_ = Only used for stepper motors to minimize
|
|
resonance.
|
|
As documented in
|
|
https://epics.anl.gov/docs/APS2015/17-Motor-Driver.pdf, the
|
|
following relations apply: motorVelBase = VBAS / MRES motorVelocity
|
|
= VELO / MRES motorAccel = (motorVelocity - motorVelBase) / ACCL
|
|
Therefore, we need to correct the values from the 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 velocity
|
|
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorVelocity(),
|
|
&motorVelocityRec);
|
|
|
|
// Only calculate timeContSpeed if the motorVelocity has been populated
|
|
// with a sensible value (e.g. > 0)
|
|
if (pl_status == asynSuccess && motorVelocityRec > 0.0) {
|
|
// Convert back to the value in the VELO field
|
|
motorVelocity = motorVelocityRec * motorRecResolution;
|
|
if (pl_status == asynSuccess) {
|
|
|
|
timeContSpeed = std::ceil(
|
|
std::fabs(targetPosition_ - motorPos) / motorVelocity);
|
|
}
|
|
}
|
|
|
|
pl_status =
|
|
pC_->getDoubleParam(axisNo_, pC_->motorAccel(), &motorAccelRec);
|
|
if (pl_status == asynSuccess && motorVelocityRec > 0.0 &&
|
|
motorAccelRec > 0.0) {
|
|
|
|
// Convert back to the value in the ACCL field
|
|
motorAccel = motorVelocityRec / motorAccelRec;
|
|
|
|
// Calculate the time
|
|
timeAccel = 2 * std::ceil(motorVelocity / motorAccel);
|
|
}
|
|
|
|
// Calculate the expected arrival time
|
|
expectedArrivalTime_ =
|
|
time(NULL) + offsetMovTimeout_ +
|
|
scaleMovTimeout_ * (timeContSpeed + 2 * timeAccel);
|
|
} else {
|
|
watchdogMovActive_ = false;
|
|
}
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::checkMovTimeoutWatchdog(bool moving) {
|
|
asynStatus pl_status;
|
|
int enableMovWatchdog = 0;
|
|
|
|
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableMovWatchdog(),
|
|
&enableMovWatchdog);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorEnableMovWatchdog_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
// Not moving or watchdog not active / enabled
|
|
if (enableMovWatchdog == 0 || !moving || !watchdogMovActive_) {
|
|
watchdogMovActive_ = false;
|
|
return asynSuccess;
|
|
}
|
|
|
|
// Check if the expected time of arrival has been exceeded.
|
|
if (expectedArrivalTime_ < time(NULL)) {
|
|
// Check the watchdog
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nExceeded "
|
|
"expected arrival time %ld (current time is %ld).\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
expectedArrivalTime_, time(NULL));
|
|
|
|
pl_status = setStringParam(
|
|
pC_->motorMessageText(),
|
|
"Exceeded expected arrival time. Check if the axis is blocked.");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusProblem(), true);
|
|
if (pl_status != asynSuccess) {
|
|
pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Even if the movement timed out, the rest of the poll should continue.
|
|
return asynSuccess;
|
|
}
|
|
return asynSuccess;
|
|
}
|
|
|
|
// =============================================================================
|
|
// IOC shell functions
|
|
extern "C" {
|
|
|
|
/**
|
|
* @brief Enable / disable the watchdog (FFI implementation)
|
|
*
|
|
* @param portName Name of the controller
|
|
* @param axisNo Axis number
|
|
* @param enable If 0, disable the watchdog, otherwise enable
|
|
* it
|
|
* @return asynStatus
|
|
*/
|
|
asynStatus setWatchdogEnabled(const char *portName, int axisNo, int enable) {
|
|
|
|
sinqController *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);
|
|
sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
|
|
if (axis == nullptr) {
|
|
errlogPrintf(
|
|
"Controller \"%s\", axis %d => %s, line %d:\nAxis does not "
|
|
"exist or is not an instance of sinqAxis.",
|
|
portName, axisNo, __PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
return axis->setWatchdogEnabled(enable != 0);
|
|
}
|
|
|
|
static const iocshArg setWatchdogEnabledArg0 = {"Controller port name",
|
|
iocshArgString};
|
|
static const iocshArg setWatchdogEnabledArg1 = {"Axis number", iocshArgInt};
|
|
static const iocshArg setWatchdogEnabledArg2 = {
|
|
"Enabling / disabling the watchdog", iocshArgInt};
|
|
static const iocshArg *const setWatchdogEnabledArgs[] = {
|
|
&setWatchdogEnabledArg0, &setWatchdogEnabledArg1, &setWatchdogEnabledArg2};
|
|
static const iocshFuncDef setWatchdogEnabledDef = {"setWatchdogEnabled", 3,
|
|
setWatchdogEnabledArgs};
|
|
|
|
static void setWatchdogEnabledCallFunc(const iocshArgBuf *args) {
|
|
setWatchdogEnabled(args[0].sval, args[1].ival, args[2].ival);
|
|
}
|
|
|
|
// =============================================================================
|
|
|
|
/**
|
|
* @brief Set the offsetMovTimeout (FFI implementation)
|
|
*
|
|
* @param portName Name of the controller
|
|
* @param axisNo Axis number
|
|
* @param offsetMovTimeout Offset (in seconds)
|
|
* @return asynStatus
|
|
*/
|
|
asynStatus setOffsetMovTimeout(const char *portName, int axisNo,
|
|
double offsetMovTimeout) {
|
|
|
|
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);
|
|
sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
|
|
if (axis == nullptr) {
|
|
errlogPrintf("Controller \"%s\" => %s, line %d:\nAxis %d does not "
|
|
"exist or is not an "
|
|
"instance of sinqAxis.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__, axisNo);
|
|
}
|
|
|
|
return axis->setOffsetMovTimeout(offsetMovTimeout);
|
|
}
|
|
|
|
static const iocshArg setOffsetMovTimeoutArg0 = {"Controller port name",
|
|
iocshArgString};
|
|
static const iocshArg setOffsetMovTimeoutArg1 = {"Axis number", iocshArgInt};
|
|
static const iocshArg setOffsetMovTimeoutArg2 = {"Offset timeout for movement",
|
|
iocshArgDouble};
|
|
static const iocshArg *const setOffsetMovTimeoutArgs[] = {
|
|
&setOffsetMovTimeoutArg0, &setOffsetMovTimeoutArg1,
|
|
&setOffsetMovTimeoutArg2};
|
|
static const iocshFuncDef setOffsetMovTimeoutDef = {"setOffsetMovTimeout", 3,
|
|
setOffsetMovTimeoutArgs};
|
|
|
|
static void setOffsetMovTimeoutCallFunc(const iocshArgBuf *args) {
|
|
setOffsetMovTimeout(args[0].sval, args[1].ival, args[2].dval);
|
|
}
|
|
|
|
// =============================================================================
|
|
|
|
/**
|
|
* @brief Set the setScaleMovTimeout (FFI implementation)
|
|
*
|
|
* @param portName Name of the controller
|
|
* @param axisNo Axis number
|
|
* @param scaleMovTimeout Scaling factor (in seconds)
|
|
* @return asynStatus
|
|
*/
|
|
asynStatus setScaleMovTimeout(const char *portName, int axisNo,
|
|
double scaleMovTimeout) {
|
|
|
|
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);
|
|
sinqAxis *axis = dynamic_cast<sinqAxis *>(asynAxis);
|
|
if (axis == nullptr) {
|
|
errlogPrintf("Controller \"%s\" => %s, line %d:\nAxis %d does not "
|
|
"exist or is not an instance of sinqAxis.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__, axisNo);
|
|
return asynError;
|
|
}
|
|
|
|
return axis->setScaleMovTimeout(scaleMovTimeout);
|
|
}
|
|
|
|
static const iocshArg setScaleMovTimeoutArg0 = {"Controller port name",
|
|
iocshArgString};
|
|
static const iocshArg setScaleMovTimeoutArg1 = {"Axis number", iocshArgInt};
|
|
static const iocshArg setScaleMovTimeoutArg2 = {
|
|
"Multiplier for calculated move time", iocshArgDouble};
|
|
static const iocshArg *const setScaleMovTimeoutArgs[] = {
|
|
&setScaleMovTimeoutArg0, &setScaleMovTimeoutArg1, &setScaleMovTimeoutArg2};
|
|
static const iocshFuncDef setScaleMovTimeoutDef = {"setScaleMovTimeout", 3,
|
|
setScaleMovTimeoutArgs};
|
|
|
|
static void setScaleMovTimeoutCallFunc(const iocshArgBuf *args) {
|
|
setScaleMovTimeout(args[0].sval, args[1].ival, args[2].dval);
|
|
}
|
|
|
|
// =============================================================================
|
|
|
|
// This function is made known to EPICS in sinqMotor.dbd and is called by EPICS
|
|
// in order to register all functions in the IOC shell
|
|
static void sinqAxisRegister(void) {
|
|
iocshRegister(&setOffsetMovTimeoutDef, setOffsetMovTimeoutCallFunc);
|
|
iocshRegister(&setScaleMovTimeoutDef, setScaleMovTimeoutCallFunc);
|
|
iocshRegister(&setWatchdogEnabledDef, setWatchdogEnabledCallFunc);
|
|
}
|
|
epicsExportRegistrar(sinqAxisRegister);
|
|
|
|
} // extern C
|