527 lines
20 KiB
C++
527 lines
20 KiB
C++
#include "sinqAxis.h"
|
|
#include "sinqController.h"
|
|
#include <math.h>
|
|
#include <unistd.h>
|
|
|
|
sinqAxis::sinqAxis(class sinqController *pC, int axisNo)
|
|
: asynMotorAxis((asynMotorController *)pC, axisNo), pC_(pC) {
|
|
|
|
initial_poll_ = true;
|
|
watchdogMovActive_ = false;
|
|
init_poll_counter_ = 0;
|
|
scaleMovTimeout_ = 2.0;
|
|
offsetMovTimeout_ = 30;
|
|
}
|
|
|
|
asynStatus sinqAxis::atFirstPoll() {
|
|
asynStatus status = asynSuccess;
|
|
int variableSpeed = 0;
|
|
|
|
// Motor is assumed to be enabled
|
|
status = pC_->setIntegerParam(axisNo_, pC_->motorEnableRBV_, 1);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorEnableRBV_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// By default, motors cannot be disabled
|
|
status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable_, 0);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorCanDisable_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
status =
|
|
pC_->getIntegerParam(axisNo_, pC_->motorCanSetSpeed_, &variableSpeed);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorCanSetSpeed_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
if (variableSpeed == 1) {
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver_, 0.0);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver_,
|
|
1000000000.0);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
} else {
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver_, 0.0);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver_, 0.0);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::poll(bool *moving) {
|
|
// Local variable declaration
|
|
asynStatus pl_status = asynSuccess;
|
|
asynStatus poll_status = asynSuccess;
|
|
|
|
// =========================================================================
|
|
|
|
// If this poll is the initial poll, check if the parameter library has
|
|
// already been initialized. If not, force EPICS to repeat the poll until
|
|
// the initialization is complete (or until a timeout is reached). Once the
|
|
// parameter library has been initialized, read configuration data from the
|
|
// motor controller into it.
|
|
if (initial_poll_) {
|
|
poll_status = atFirstPoll();
|
|
|
|
if (poll_status == asynSuccess) {
|
|
initial_poll_ = false;
|
|
} else {
|
|
// Send a message to the IOC shell every 10 trials.
|
|
init_poll_counter_ += 1;
|
|
if (init_poll_counter_ % 10 == 0) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nRunning function 'atFirstPoll' "
|
|
"failed %d times with error %s.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, init_poll_counter_,
|
|
pC_->stringifyAsynStatus(poll_status));
|
|
}
|
|
|
|
// Wait for 100 ms until trying the entire poll again
|
|
usleep(100000);
|
|
return poll_status;
|
|
}
|
|
}
|
|
|
|
/*
|
|
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_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
pl_status = setIntegerParam(pC_->motorStatusCommsError_, false);
|
|
if (pl_status != asynSuccess) {
|
|
pC_->paramLibAccessFailed(pl_status, "motorStatusCommsError_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
pl_status = setStringParam(pC_->motorMessageText_, "");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__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);
|
|
|
|
// 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_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
}
|
|
|
|
// Check and update the watchdog
|
|
if (checkMovTimeoutWatchdog(*moving) != asynSuccess) {
|
|
return asynError;
|
|
}
|
|
|
|
if (pl_status != asynSuccess) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nFunction isEnabled failed with %s.\n",
|
|
__PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(poll_status));
|
|
pl_status = setStringParam(pC_->motorMessageText_,
|
|
"Could not check whether the motor is "
|
|
"enabled or not. Please call the support");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
}
|
|
|
|
// According to the function documentation of asynMotorAxis::poll, this
|
|
// function should be called at the end of a poll implementation.
|
|
pl_status = callParamCallbacks();
|
|
if (pl_status != asynSuccess) {
|
|
// If we can't communicate with the parameter library, it doesn't make
|
|
// sense to try and upstream this to the user -> Just log the error
|
|
asynPrint(
|
|
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\ncallParamCallbacks failed with %s for axis %d",
|
|
__PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(poll_status), axisNo_);
|
|
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) {
|
|
|
|
double targetPosition = 0.0;
|
|
|
|
// Status of parameter library operations
|
|
asynStatus pl_status = asynSuccess;
|
|
|
|
// =========================================================================
|
|
|
|
// Calculate the (absolute) target position
|
|
if (relative) {
|
|
|
|
double motorPosition = 0.0;
|
|
pl_status =
|
|
pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &motorPosition);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
targetPosition = position + motorPosition;
|
|
} else {
|
|
targetPosition = position;
|
|
}
|
|
|
|
// Set the target position
|
|
pl_status = setDoubleParam(pC_->motorTargetPosition_, targetPosition);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorTargetPosition_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
return doMove(position, relative, minVelocity, maxVelocity, acceleration);
|
|
}
|
|
|
|
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) {
|
|
|
|
double targetPosition = 0.0;
|
|
double position = 0.0;
|
|
double highLimit = 0.0;
|
|
double lowLimit = 0.0;
|
|
|
|
// Status of parameter library operations
|
|
asynStatus pl_status = asynSuccess;
|
|
|
|
// =========================================================================
|
|
|
|
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &position);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorHighLimit_, &highLimit);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorHighLimit_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorLowLimit_, &lowLimit);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
if (std::fabs(position - highLimit) > std::fabs(position - lowLimit)) {
|
|
targetPosition = highLimit;
|
|
} else {
|
|
targetPosition = lowLimit;
|
|
}
|
|
|
|
// Set the target position
|
|
pl_status = setDoubleParam(pC_->motorTargetPosition_, targetPosition);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorTargetPosition_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
return doHome(minVelocity, maxVelocity, acceleration, forwards);
|
|
}
|
|
|
|
asynStatus sinqAxis::doHome(double minVelocity, double maxVelocity,
|
|
double acceleration, int forwards) {
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus sinqAxis::enable(bool on) { return asynSuccess; }
|
|
|
|
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_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
if (variableSpeed == 1) {
|
|
|
|
// Check the inputs and create corresponding error messages
|
|
if (vbas > vmax) {
|
|
asynPrint(pC_->lowLevelPortUser_, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nLower speed limit vbas=%lf must not be "
|
|
"smaller than upper limit vmax=%lf.\n",
|
|
__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_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
return asynError;
|
|
}
|
|
if (velo < vbas || velo > vmax) {
|
|
asynPrint(pC_->lowLevelPortUser_, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nActual speed velo=%lf must be between "
|
|
"lower limit vbas=%lf and upper limit vmx=%lf.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, velo, vbas, vmax);
|
|
|
|
status = setStringParam(pC_->motorMessageText_,
|
|
"Speed is not inside limits");
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
return asynError;
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver_, vbas);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVeloFromDriver_, velo);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVeloFromDriver_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver_, vmax);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
|
|
__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_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVeloFromDriver_, velo);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVeloFromDriver_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver_, velo);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::setAcclField(double accl) {
|
|
|
|
if (accl <= 0.0) {
|
|
return asynError;
|
|
}
|
|
|
|
asynStatus status =
|
|
pC_->setDoubleParam(axisNo_, pC_->motorAcclFromDriver_, accl);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorAcclFromDriver_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus sinqAxis::setWatchdogEnabled(bool enable) {
|
|
return pC_->setIntegerParam(axisNo_, 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_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
if (enableMovWatchdog == 1) {
|
|
// These parameters are only needed in this branch
|
|
double motorPosition = 0.0;
|
|
double motorPositionRec = 0.0;
|
|
double motorTargetPositionRec = 0.0;
|
|
double motorTargetPosition = 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 = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
|
|
&motorRecResolution);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorPosition_,
|
|
&motorPositionRec);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorPosition",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
motorPosition = motorPositionRec * motorRecResolution;
|
|
|
|
/*
|
|
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.
|
|
*/
|
|
|
|
// 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;
|
|
|
|
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorTargetPosition_,
|
|
&motorTargetPositionRec);
|
|
motorTargetPosition = motorTargetPositionRec * motorRecResolution;
|
|
if (pl_status == asynSuccess) {
|
|
|
|
timeContSpeed =
|
|
std::ceil(std::fabs(motorTargetPosition - motorPosition) /
|
|
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_",
|
|
__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_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nAxis %d exceeded the expected arrival time "
|
|
"%ld (current time is %ld).\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, 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_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
|
if (pl_status != asynSuccess) {
|
|
pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Even if the movement timed out, the rest of the poll should continue.
|
|
return asynSuccess;
|
|
}
|
|
return asynSuccess;
|
|
} |