lin-epics-modules/sinqMotor
0
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Substantial rRework of 0.2.0 after the CAMEA test showed multiple

Browse Source 
problems. Also improved the documentation.
This commit is contained in:
mathis_s
2024-11-26 16:51:12 +01:00
parent 97e80814e3
commit 682325de7d
7 changed files with 423 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/sinqAxis.cpp
+169 -20
View File
@@ -9,6 +9,8 @@ sinqAxis::sinqAxis(class sinqController *pC, int axisNo)
initial_poll_ = true;
watchdogMovActive_ = false;
init_poll_counter_ = 0;
scaleMovTimeout_ = 2.0;
offsetMovTimeout_ = 30;
}
asynStatus sinqAxis::atFirstPoll() { return asynSuccess; }
@@ -53,6 +55,11 @@ asynStatus sinqAxis::poll(bool *moving) {
// return.
poll_status = doPoll(moving);
// Check and update the watchdog
if (checkMovTimeoutWatchdog(*moving) != asynSuccess) {
return asynError;
}
// If the poll status is ok, reset the error indicators in the parameter
// library
if (poll_status == asynSuccess) {
@@ -74,6 +81,30 @@ asynStatus sinqAxis::poll(bool *moving) {
}
}
// Update the enable RBV
bool axisIsEnabled = false;
pl_status = isEnabled(&axisIsEnabled);
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__);
}
} else {
pl_status = setIntegerParam(pC_->enableMotorRBV_, axisIsEnabled);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_",
__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();
@@ -186,66 +217,183 @@ asynStatus sinqAxis::doHome(double minVelocity, double maxVelocity,
return asynSuccess;
}
asynStatus sinqAxis::setWatchdogEnabled(bool enable) {
watchdogEnabled_ = enable;
asynStatus sinqAxis::enable(bool on) { return asynSuccess; }
asynStatus sinqAxis::isEnabled(bool *on) {
*on = true;
return asynSuccess;
}
asynStatus sinqAxis::setWatchdogEnabled(bool enable) {
return pC_->setIntegerParam(axisNo_, pC_->enableMovWatchdog_, enable);
}
asynStatus sinqAxis::startMovTimeoutWatchdog() {
if (watchdogEnabled_) {
asynStatus pl_status;
int enableMovWatchdog = 0;
pl_status = pC_->getIntegerParam(axisNo_, pC_->enableMovWatchdog_,
&enableMovWatchdog);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "enableMovWatchdog_",
__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 motorVelBase = 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;
asynStatus pl_status;
// Activate the watchdog
watchdogMovActive_ = true;
pl_status =
pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &motorPosition);
/*
The motor record resolution (index motorRecResolution_ in the parameter
library, MRES in the motor record) is NOT a conversion factor between
user units (e.g. mm) and motor units (e.g. encoder steps), but a scaling
factor defining the resolution of the position readback field RRBV. This
is due to an implementation detail inside EPICS described here:
https://epics.anl.gov/tech-talk/2018/msg00089.php
https://github.com/epics-modules/motor/issues/8
Basically, the position value in the parameter library is a double which
is then truncated to an integer in devMotorAsyn.c (because it was
originally meant for converting from engineering units to encoder steps,
which are by definition integer values). Therefore, if we want a
precision of 1 millimeter, we need to set MRES to 1. If we want one of
1 micrometer, we need to set MRES to 0.001. The readback value needs to
be multiplied with MRES to get the actual value.
In the driver, we use user units. Therefore, when we interact with the
parameter library, we need to account for MRES. This means:
- When writing position or speed to the parameter library, we divide the
value by the motor record resolution.
- When reading position or speed from the parameter library, we multiply
the value with the motor record resolution.
Index and motor record field are coupled as follows:
The parameter motorRecResolution_ is coupled to the field MRES of the
motor record in the following manner:
- In sinqMotor.db, the PV (motor_record_pv_name) MOTOR_REC_RESOLUTION
is defined as a copy of the field (motor_record_pv_name).MRES:
record(ao,"$(P)$(M):Resolution") {
field(DESC, "$(M) resolution")
field(DOL, "$(P)$(M).MRES CP MS")
field(OMSL, "closed_loop")
field(DTYP, "asynFloat64")
field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_REC_RESOLUTION")
field(PREC, "$(PREC)")
}
- The PV name MOTOR_REC_RESOLUTION is coupled in asynMotorController.h
to the constant motorRecResolutionString
- ... which in turn is assigned to motorRecResolution_ in
asynMotorController.cpp This way of making the field visible to the
driver is described here:
https://epics.anl.gov/tech-talk/2020/msg00378.php This is a one-way
coupling, changes to the parameter library via setDoubleParam are NOT
transferred to (motor_record_pv_name).MRES or to
(motor_record_pv_name):Resolution.
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__);
}
pl_status =
pC_->getDoubleParam(axisNo_, pC_->motorVelBase_, &motorVelBase);
// Only calculate timeContSpeed if the motorVelBase_ has been populated
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 && motorVelBase > 0.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_,
&motorTargetPosition);
&motorTargetPositionRec);
motorTargetPosition = motorTargetPositionRec * motorRecResolution;
if (pl_status == asynSuccess) {
timeContSpeed =
std::ceil(std::fabs(motorTargetPosition - motorPosition) /
motorVelBase);
motorVelocity);
}
}
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorAccel_, &motorAccel);
if (pl_status == asynSuccess && motorVelBase > 0.0 &&
motorAccel > 0.0) {
timeAccel = 2 * std::ceil(motorVelBase / motorAccel);
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;
// Not moving or watchdog not active
if (!watchdogEnabled_ || !moving) {
pl_status = pC_->getIntegerParam(axisNo_, pC_->enableMovWatchdog_,
&enableMovWatchdog);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "enableMovWatchdog_",
__PRETTY_FUNCTION__, __LINE__);
}
// Not moving or watchdog not active / enabled
if (enableMovWatchdog == 0 || !moving || !watchdogMovActive_) {
watchdogMovActive_ = false;
return asynSuccess;
}
@@ -272,7 +420,8 @@ asynStatus sinqAxis::checkMovTimeoutWatchdog(bool moving) {
__PRETTY_FUNCTION__, __LINE__);
}
return asynError;
// Even if the movement timed out, the rest of the poll should continue.
return asynSuccess;
}
return asynSuccess;
}