Bugfix for movTimeoutWatchdog
Update of README.md
This commit is contained in:
+379
-206
@@ -1,5 +1,8 @@
|
||||
#include "sinqAxis.h"
|
||||
#include "epicsExport.h"
|
||||
#include "iocsh.h"
|
||||
#include "sinqController.h"
|
||||
#include <errlog.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
|
||||
@@ -12,124 +15,95 @@ sinqAxis::sinqAxis(class sinqController *pC, int axisNo)
|
||||
init_poll_counter_ = 0;
|
||||
scaleMovTimeout_ = 2.0;
|
||||
offsetMovTimeout_ = 30;
|
||||
targetPosition_ = 0.0;
|
||||
|
||||
// Motor is assumed to be enabled
|
||||
status = pC_->setIntegerParam(axisNo_, pC_->motorEnableRBV_, 1);
|
||||
status = setIntegerParam(pC_->motorEnableRBV_, 1);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(
|
||||
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nFATAL ERROR (setting a parameter value failed "
|
||||
"with %s)\n. Terminating IOC",
|
||||
__PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status));
|
||||
asynPrint(pC_->pasynUserSelf, 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 = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable_, 0);
|
||||
status = setIntegerParam(pC_->motorCanDisable_, 0);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(
|
||||
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nFATAL ERROR (setting a parameter value failed "
|
||||
"with %s)\n. Terminating IOC",
|
||||
__PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status));
|
||||
asynPrint(pC_->pasynUserSelf, 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 = pC_->setDoubleParam(axisNo_, pC_->motorPosition_, 0.0);
|
||||
status = setDoubleParam(pC_->motorPosition_, 0.0);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(
|
||||
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nFATAL ERROR (setting a parameter value failed "
|
||||
"with %s)\n. Terminating IOC",
|
||||
__PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status));
|
||||
asynPrint(pC_->pasynUserSelf, 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 = pC_->setIntegerParam(axisNo_, pC_->motorStatusProblem_, 0);
|
||||
status = setIntegerParam(pC_->motorStatusProblem_, 0);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(
|
||||
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nFATAL ERROR (setting a parameter value failed "
|
||||
"with %s)\n. Terminating IOC",
|
||||
__PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status));
|
||||
asynPrint(pC_->pasynUserSelf, 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::atFirstPoll() {
|
||||
asynStatus status = asynSuccess;
|
||||
int variableSpeed = 0;
|
||||
|
||||
status =
|
||||
pC_->getIntegerParam(axisNo_, pC_->motorCanSetSpeed_, &variableSpeed);
|
||||
// Set the homing-related flags
|
||||
status = setIntegerParam(pC_->motorStatusHome_, 0);
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, "motorCanSetSpeed_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
asynPrint(pC_->pasynUserSelf, 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);
|
||||
}
|
||||
|
||||
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__);
|
||||
}
|
||||
status = setIntegerParam(pC_->motorStatusHomed_, 0);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(pC_->pasynUserSelf, 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_->pasynUserSelf, 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);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
int homing = 0;
|
||||
int homed = 0;
|
||||
|
||||
/*
|
||||
At the beginning of the poll, it is assumed that the axis has no status
|
||||
@@ -140,18 +114,19 @@ asynStatus sinqAxis::poll(bool *moving) {
|
||||
*/
|
||||
pl_status = setIntegerParam(pC_->motorStatusProblem_, false);
|
||||
if (pl_status != asynSuccess) {
|
||||
pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
||||
pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
pl_status = setIntegerParam(pC_->motorStatusCommsError_, false);
|
||||
if (pl_status != asynSuccess) {
|
||||
pC_->paramLibAccessFailed(pl_status, "motorStatusCommsError_",
|
||||
pC_->paramLibAccessFailed(pl_status, "motorStatusCommsError_", axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
pl_status = setStringParam(pC_->motorMessageText_, "");
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
// The poll function is just a wrapper around doPoll and
|
||||
@@ -165,41 +140,62 @@ asynStatus sinqAxis::poll(bool *moving) {
|
||||
if (poll_status != asynSuccess) {
|
||||
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
||||
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;
|
||||
}
|
||||
|
||||
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_);
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"Controller \"%s\", axis %d => %s, line "
|
||||
"%d:\ncallParamCallbacks failed with %s.\n",
|
||||
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
||||
pC_->stringifyAsynStatus(poll_status));
|
||||
poll_status = pl_status;
|
||||
}
|
||||
|
||||
@@ -211,35 +207,35 @@ 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;
|
||||
double motorRecResolution = 0.0;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
// 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);
|
||||
// When a new move is done, the motor is not homed anymore
|
||||
pl_status = setIntegerParam(pC_->motorStatusHomed_, 0);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorTargetPosition_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorStatusHomed_",
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
pl_status = setIntegerParam(pC_->motorStatusAtHome_, 0);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorStatusAtHome_",
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
|
||||
&motorRecResolution);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
// Store the target position internally
|
||||
targetPosition_ = position * motorRecResolution;
|
||||
|
||||
return doMove(position, relative, minVelocity, maxVelocity, acceleration);
|
||||
}
|
||||
@@ -252,48 +248,58 @@ asynStatus sinqAxis::doMove(double position, int relative, double minVelocity,
|
||||
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;
|
||||
asynStatus status = asynSuccess;
|
||||
|
||||
// Status of parameter library operations
|
||||
asynStatus pl_status = asynSuccess;
|
||||
status = doHome(minVelocity, maxVelocity, acceleration, forwards);
|
||||
|
||||
// =========================================================================
|
||||
if (status == asynSuccess) {
|
||||
|
||||
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &position);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
status = setStringParam(pC_->motorMessageText_, "Homing");
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorHighLimit_, &highLimit);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorHighLimit_",
|
||||
__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__);
|
||||
}
|
||||
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__);
|
||||
}
|
||||
|
||||
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorLowLimit_, &lowLimit);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
// Update the motor record
|
||||
return callParamCallbacks();
|
||||
|
||||
if (std::fabs(position - highLimit) > std::fabs(position - lowLimit)) {
|
||||
targetPosition = highLimit;
|
||||
} 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 {
|
||||
targetPosition = lowLimit;
|
||||
// Bubble up all other problems
|
||||
return status;
|
||||
}
|
||||
|
||||
// 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,
|
||||
@@ -311,7 +317,7 @@ asynStatus sinqAxis::setVeloFields(double velo, double vbas, double vmax) {
|
||||
status =
|
||||
pC_->getIntegerParam(axisNo_, pC_->motorCanSetSpeed_, &variableSpeed);
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, "motorCanSetSpeed_",
|
||||
return pC_->paramLibAccessFailed(status, "motorCanSetSpeed_", axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
if (variableSpeed == 1) {
|
||||
@@ -319,30 +325,36 @@ asynStatus sinqAxis::setVeloFields(double velo, double vbas, double vmax) {
|
||||
// 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);
|
||||
"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_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __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 vmax=%lf.\n",
|
||||
__PRETTY_FUNCTION__, __LINE__, velo, vbas, vmax);
|
||||
"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_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
return asynError;
|
||||
}
|
||||
@@ -350,38 +362,44 @@ asynStatus sinqAxis::setVeloFields(double velo, double vbas, double vmax) {
|
||||
status = pC_->setDoubleParam(axisNo_, pC_->motorVbasFromDriver_, vbas);
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, "motorVbasFromDriver_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
status = pC_->setDoubleParam(axisNo_, pC_->motorVeloFromDriver_, velo);
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, "motorVeloFromDriver_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver_, vmax);
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
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_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
status = pC_->setDoubleParam(axisNo_, pC_->motorVeloFromDriver_, velo);
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, "motorVeloFromDriver_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
status = pC_->setDoubleParam(axisNo_, pC_->motorVmaxFromDriver_, velo);
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, "motorVmaxFromDriver_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
}
|
||||
return status;
|
||||
@@ -393,17 +411,17 @@ asynStatus sinqAxis::setAcclField(double accl) {
|
||||
return asynError;
|
||||
}
|
||||
|
||||
asynStatus status =
|
||||
pC_->setDoubleParam(axisNo_, pC_->motorAcclFromDriver_, accl);
|
||||
asynStatus status = setDoubleParam(pC_->motorAcclFromDriver_, accl);
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, "motorAcclFromDriver_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus sinqAxis::setWatchdogEnabled(bool enable) {
|
||||
return pC_->setIntegerParam(axisNo_, pC_->motorEnableMovWatchdog_, enable);
|
||||
return setIntegerParam(pC_->motorEnableMovWatchdog_, enable);
|
||||
}
|
||||
|
||||
asynStatus sinqAxis::startMovTimeoutWatchdog() {
|
||||
@@ -414,15 +432,14 @@ asynStatus sinqAxis::startMovTimeoutWatchdog() {
|
||||
&enableMovWatchdog);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorEnableMovWatchdog_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __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;
|
||||
@@ -443,14 +460,16 @@ asynStatus sinqAxis::startMovTimeoutWatchdog() {
|
||||
&motorRecResolution);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorPosition_,
|
||||
&motorPositionRec);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorPosition",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
motorPosition = motorPositionRec * motorRecResolution;
|
||||
@@ -477,15 +496,10 @@ asynStatus sinqAxis::startMovTimeoutWatchdog() {
|
||||
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);
|
||||
timeContSpeed = std::ceil(
|
||||
std::fabs(targetPosition_ - motorPosition) / motorVelocity);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -519,7 +533,8 @@ asynStatus sinqAxis::checkMovTimeoutWatchdog(bool moving) {
|
||||
&enableMovWatchdog);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorEnableMovWatchdog_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
// Not moving or watchdog not active / enabled
|
||||
@@ -532,22 +547,23 @@ asynStatus sinqAxis::checkMovTimeoutWatchdog(bool moving) {
|
||||
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));
|
||||
"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_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
||||
pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
@@ -555,4 +571,161 @@ asynStatus sinqAxis::checkMovTimeoutWatchdog(bool moving) {
|
||||
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;
|
||||
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 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
|
||||
|
||||
Reference in New Issue
Block a user