1164 lines
45 KiB
C++
1164 lines
45 KiB
C++
#include "pmacV3Axis.h"
|
|
#include "asynOctetSyncIO.h"
|
|
#include "pmacV3Controller.h"
|
|
#include <cmath>
|
|
#include <errlog.h>
|
|
#include <limits>
|
|
#include <math.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
|
|
pmacV3Axis::pmacV3Axis(pmacV3Controller *pC, int axisNo, double offsetLimit)
|
|
: sinqAxis(pC, axisNo), pC_(pC) {
|
|
|
|
asynStatus status = asynSuccess;
|
|
|
|
/*
|
|
The superclass constructor sinqAxis calls in turn its superclass constructor
|
|
asynMotorAxis. In the latter, a pointer to the constructed object this is
|
|
stored inside the array pAxes_:
|
|
|
|
pC->pAxes_[axisNo] = this;
|
|
|
|
Therefore, the axes are managed by the controller pC. If axisNo is out of
|
|
bounds, asynMotorAxis prints an error (see
|
|
https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorAxis.cpp,
|
|
line 40). However, we want the IOC creation to stop completely, since this
|
|
is a configuration error.
|
|
*/
|
|
if (axisNo >= pC->numAxes_) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:: FATAL ERROR: Axis index %d must be smaller "
|
|
"than the total number of axes %d. Call the support.",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, pC->numAxes_);
|
|
exit(-1);
|
|
}
|
|
|
|
// Initialize all member variables
|
|
initial_poll_ = true;
|
|
waitForHandshake_ = false;
|
|
axisStatus_ = 0;
|
|
offsetLimits_ = offsetLimit;
|
|
|
|
// Provide initial values for some parameter library entries
|
|
status = pC_->setIntegerParam(axisNo_, pC_->rereadEncoderPosition_, 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));
|
|
exit(-1);
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, 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));
|
|
exit(-1);
|
|
}
|
|
|
|
// Default values for the watchdog timeout mechanism
|
|
offsetMovTimeout_ = 30; // seconds
|
|
scaleMovTimeout_ = 2.0;
|
|
}
|
|
|
|
pmacV3Axis::~pmacV3Axis(void) {
|
|
// Since the controller memory is managed somewhere else, we don't need to
|
|
// clean up the pointer pC here.
|
|
}
|
|
|
|
/**
|
|
Read the configuration at the first poll
|
|
*/
|
|
asynStatus pmacV3Axis::atFirstPoll() { return readConfig(); }
|
|
|
|
/*
|
|
Read the configuration from the motor control unit and the parameter library.
|
|
*/
|
|
asynStatus pmacV3Axis::readConfig() {
|
|
|
|
// Local variable declaration
|
|
asynStatus status = asynSuccess;
|
|
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
|
int nvals = 0;
|
|
double motorRecResolution = 0.0;
|
|
double motorPosition = 0.0;
|
|
double motorVelBase = 0.0;
|
|
double motorAccel = 0.0;
|
|
int axStatus = 0;
|
|
|
|
/*
|
|
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. 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!
|
|
*/
|
|
status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
|
|
&motorRecResolution);
|
|
if (status == asynParamUndefined) {
|
|
return asynParamUndefined;
|
|
} else if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorRecResolution_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Software limits and current position
|
|
snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10 Q%2.2d04 Q%2.2d06",
|
|
axisNo_, axisNo_, axisNo_, axisNo_);
|
|
status = pC_->writeRead(axisNo_, command, response, 4);
|
|
nvals = sscanf(response, "%d %lf %lf %lf", &axStatus, &motorPosition,
|
|
&motorVelBase, &motorAccel);
|
|
if (nvals != 4) {
|
|
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Transform from motor to parameter library coordinates
|
|
motorPosition = motorPosition / motorRecResolution;
|
|
motorVelBase = motorVelBase / motorRecResolution;
|
|
motorAccel = motorAccel / motorRecResolution;
|
|
|
|
// Store these values in the parameter library
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorPosition_, motorPosition);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorPosition_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorVelBase_, motorVelBase);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorVelBase_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
status = pC_->setDoubleParam(axisNo_, pC_->motorAccel_, motorAccel);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorAccel_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Set the initial enable based on the motor status value
|
|
status =
|
|
setIntegerParam(pC_->enableMotor_, (axStatus != -3 && axStatus != -5));
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "enableMotor_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Update the parameter library immediately
|
|
status = callParamCallbacks();
|
|
if (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.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status),
|
|
axisNo_);
|
|
return status;
|
|
}
|
|
|
|
return this->readEncoderType();
|
|
}
|
|
|
|
// Perform the actual poll
|
|
asynStatus pmacV3Axis::doPoll(bool *moving) {
|
|
|
|
// Return value for the poll
|
|
asynStatus poll_status = asynSuccess;
|
|
|
|
// Status of read-write-operations of ASCII commands to the controller
|
|
asynStatus rw_status = asynSuccess;
|
|
|
|
// Status of parameter library operations
|
|
asynStatus pl_status = asynSuccess;
|
|
|
|
char command[pC_->MAXBUF_], response[pC_->MAXBUF_],
|
|
userMessage[pC_->MAXBUF_];
|
|
int nvals = 0;
|
|
|
|
int direction = 0;
|
|
int error = 0;
|
|
int axStatus = 0;
|
|
double currentPosition = 0.0;
|
|
double previousPosition = 0.0;
|
|
double motorRecResolution = 0.0;
|
|
int handshakePerformed = 0;
|
|
double highLimit = 0.0;
|
|
double lowLimit = 0.0;
|
|
|
|
// =========================================================================
|
|
|
|
// Are we currently waiting for a handshake?
|
|
if (waitForHandshake_) {
|
|
|
|
snprintf(command, sizeof(command), "P%2.2d23 P%2.2d01", axisNo_,
|
|
axisNo_);
|
|
rw_status = pC_->writeRead(axisNo_, command, response, 2);
|
|
if (rw_status != asynSuccess) {
|
|
return rw_status;
|
|
}
|
|
|
|
nvals = sscanf(response, "%d %d", &handshakePerformed, &error);
|
|
if (nvals != 2) {
|
|
return pC_->errMsgCouldNotParseResponse(
|
|
command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
if (error != 0) {
|
|
// If an error occurred while waiting for the handshake, abort the
|
|
// waiting procedure and continue with the poll in order to handle
|
|
// the error.
|
|
waitForHandshake_ = false;
|
|
} else if (handshakePerformed == 1) {
|
|
// Handshake has been performed successfully -> Continue with the
|
|
// poll
|
|
waitForHandshake_ = false;
|
|
} else {
|
|
// Still waiting for the handshake. This is already part of the
|
|
// movement procedure!
|
|
*moving = true;
|
|
return asynSuccess;
|
|
}
|
|
}
|
|
|
|
// Motor resolution from parameter library
|
|
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
|
|
&motorRecResolution);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Read the previous motor position
|
|
pl_status =
|
|
pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &previousPosition);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Transform from EPICS to motor coordinates (see comment in
|
|
// pmacV3Axis::readConfig())
|
|
previousPosition = previousPosition * motorRecResolution;
|
|
|
|
// Query the axis status
|
|
snprintf(command, sizeof(command),
|
|
"P%2.2d00 Q%2.2d10 P%2.2d01 Q%2.2d13 Q%2.2d14", axisNo_, axisNo_,
|
|
axisNo_, axisNo_, axisNo_);
|
|
rw_status = pC_->writeRead(axisNo_, command, response, 5);
|
|
if (rw_status != asynSuccess) {
|
|
return rw_status;
|
|
}
|
|
|
|
nvals = sscanf(response, "%d %lf %d %lf %lf", &axStatus, ¤tPosition,
|
|
&error, &highLimit, &lowLimit);
|
|
if (nvals != 5) {
|
|
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
/*
|
|
The axis limits are set as: ({[]})
|
|
where [] are the positive and negative limits set in EPICS/NICOS, {} are the
|
|
software limits set on the MCU and () are the hardware limit switches. In
|
|
other words, the EPICS/NICOS limits must be stricter than the software
|
|
limits on the MCU which in turn should be stricter than the hardware limit
|
|
switches. For example, if the hardware limit switches are at [-10, 10], the
|
|
software limits could be at [-9, 9] and the EPICS / NICOS limits could be at
|
|
[-8, 8]. Therefore, we cannot use the software limits read from the MCU
|
|
directly, but need to shrink them a bit. In this case, we're shrinking them
|
|
by 0.1 mm or 0.1 degree (depending on the axis type) on both sides.
|
|
*/
|
|
highLimit = highLimit - offsetLimits_;
|
|
lowLimit = lowLimit + offsetLimits_;
|
|
|
|
// Store the axis status
|
|
axisStatus_ = axStatus;
|
|
|
|
// Intepret the status
|
|
switch (axStatus) {
|
|
case -6:
|
|
*moving = true;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nAxis %d is stopping\n", __PRETTY_FUNCTION__,
|
|
__LINE__, axisNo_);
|
|
break;
|
|
case -5:
|
|
*moving = false;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nAxis %d is deactivated\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
|
|
pl_status = setStringParam(pC_->motorMessageText_, "Deactivated");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// No further evaluation of the axis status is necessary
|
|
return asynSuccess;
|
|
case -4:
|
|
*moving = false;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nEmergency stop activated\n",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
|
|
pl_status = setStringParam(pC_->motorMessageText_, "Emergency stop");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// No further evaluation of the axis status is necessary
|
|
return asynSuccess;
|
|
case -3:
|
|
*moving = false;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nAxis %d is disabled\n", __PRETTY_FUNCTION__,
|
|
__LINE__, axisNo_);
|
|
|
|
pl_status = setStringParam(pC_->motorMessageText_, "Disabled");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// No further evaluation of the axis status is necessary
|
|
return asynSuccess;
|
|
case 0:
|
|
*moving = false;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nAxis %d is idle\n", __PRETTY_FUNCTION__,
|
|
__LINE__, axisNo_);
|
|
break;
|
|
case 1:
|
|
*moving = true;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nMove order for axis %d acknowledged\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
break;
|
|
case 2:
|
|
*moving = true;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nMove order for axis %d is possible\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
break;
|
|
case 3:
|
|
*moving = true;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nAxis %d in Air Cushion Output status\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
break;
|
|
case 4:
|
|
*moving = true;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nAxis %d in Air Cushion Input status\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
break;
|
|
case 5:
|
|
*moving = true;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nAxis %d is moving\n", __PRETTY_FUNCTION__,
|
|
__LINE__, axisNo_);
|
|
|
|
break;
|
|
default:
|
|
*moving = false;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nReached unreachable state P%2.2d00 = %d\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, axStatus);
|
|
pl_status = setStringParam(
|
|
pC_->motorMessageText_,
|
|
"Unreachable state has been reached. Please call the support.");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
}
|
|
|
|
if (*moving) {
|
|
// If the axis is moving, evaluate the movement direction
|
|
if ((currentPosition - previousPosition) > 0) {
|
|
direction = 1;
|
|
} else {
|
|
direction = 0;
|
|
}
|
|
}
|
|
|
|
// Check and update the watchdog
|
|
if (checkMovTimeoutWatchdog(*moving) != asynSuccess) {
|
|
return asynError;
|
|
}
|
|
|
|
// Error handling
|
|
switch (error) {
|
|
case 0:
|
|
// No error
|
|
break;
|
|
case 1:
|
|
// EPICS should already prevent this issue in the first place,
|
|
// since it contains the user limits
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nTarget position would exceed user limits in "
|
|
"axis %d\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
|
|
pl_status = setStringParam(pC_->motorMessageText_,
|
|
"Target position would exceed software "
|
|
"limits. Please call the support.");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
poll_status = asynError;
|
|
break;
|
|
case 5:
|
|
// Command not possible
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nAxis %d is still moving, but received "
|
|
"another move command. EPICS should prevent this, check if "
|
|
"*moving is set correctly.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
|
|
pl_status = setStringParam(pC_->motorMessageText_,
|
|
"Axis received move command while it is "
|
|
"still moving. Please call the support.");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
poll_status = asynError;
|
|
break;
|
|
case 8:
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nAir cushion feedback stopped during "
|
|
"movement (P%2.2d01 = %d).\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, error);
|
|
|
|
snprintf(userMessage, sizeof(userMessage),
|
|
"Air cushion feedback stopped during movement (P%2.2d01 = "
|
|
"%d). Please call the support.",
|
|
axisNo_, error);
|
|
|
|
pl_status = setStringParam(pC_->motorMessageText_, userMessage);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
break;
|
|
case 9:
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nNo air cushion feedback before movement "
|
|
"start (P%2.2d01 = %d).\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, error);
|
|
|
|
snprintf(userMessage, sizeof(userMessage),
|
|
"No air cushion feedback before movement start (P%2.2d01 = "
|
|
"%d). Please call the support.",
|
|
axisNo_, error);
|
|
pl_status = setStringParam(pC_->motorMessageText_, userMessage);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
break;
|
|
case 10:
|
|
/*
|
|
Software limits of the controller have been hit. Since the EPICS limits
|
|
are derived from the software limits and are a little bit smaller, this
|
|
error case can only happen if either the axis has an incremental encoder
|
|
which is not properly homed or if a bug occured.
|
|
*/
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nAxis %d hit the controller limits.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
|
|
snprintf(userMessage, sizeof(userMessage),
|
|
"Software limits or end switch hit (P%2.2d01 = %d). Try "
|
|
"homing the motor, moving in the opposite direction or check "
|
|
"the SPS for errors (if available). "
|
|
"Otherwise please call the support.",
|
|
axisNo_, error);
|
|
|
|
pl_status = setStringParam(pC_->motorMessageText_, userMessage);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
poll_status = asynError;
|
|
break;
|
|
case 11:
|
|
// Following error
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nMaximum allowed following error exceeded "
|
|
"for axis %d.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
|
|
snprintf(command, sizeof(command),
|
|
"Maximum allowed following error exceeded (P%2.2d01 = %d). "
|
|
"Check if movement range is blocked."
|
|
"Otherwise please call the support.",
|
|
axisNo_, error);
|
|
pl_status = setStringParam(pC_->motorMessageText_, command);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
poll_status = asynError;
|
|
break;
|
|
|
|
case 12:
|
|
snprintf(command, sizeof(command),
|
|
"Security input is triggered (P%2.2d01 = %d). Check the SPS "
|
|
"for errors (if available). Otherwise please call "
|
|
"the support.",
|
|
axisNo_, error);
|
|
break;
|
|
|
|
case 13:
|
|
// Driver hardware error triggered
|
|
asynPrint(
|
|
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nDriver hardware error triggered for axis %d.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
snprintf(command, sizeof(command),
|
|
"Driver hardware error (P%2.2d01 = 13). "
|
|
"Please call the support.",
|
|
axisNo_);
|
|
pl_status = setStringParam(pC_->motorMessageText_, command);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
poll_status = asynError;
|
|
break;
|
|
case 14:
|
|
// EPICS should already prevent this issue in the first place,
|
|
// since it contains the user limits
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nMove command exceeds hardware limits "
|
|
"(P%2.2d01 = %d).\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, error);
|
|
|
|
snprintf(command, sizeof(command),
|
|
"Move command exceeds hardware limits (P%2.2d01 = %d). Please "
|
|
"call the support.",
|
|
axisNo_, error);
|
|
pl_status = setStringParam(pC_->motorMessageText_, command);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
poll_status = asynError;
|
|
break;
|
|
default:
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nAxis %d reached an unreachable state "
|
|
"(P%2.2d01 = %d).\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, axisNo_, error);
|
|
|
|
pl_status = setStringParam(
|
|
pC_->motorMessageText_,
|
|
"Axis reached an unreachable state (P%2.2d01 = %d). Please "
|
|
"call the support.");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
poll_status = asynError;
|
|
break;
|
|
}
|
|
|
|
// Update the parameter library
|
|
if (error != 0) {
|
|
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
}
|
|
|
|
if (*moving == false) {
|
|
pl_status = setIntegerParam(pC_->motorMoveToHome_, 0);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
}
|
|
|
|
pl_status =
|
|
setIntegerParam(pC_->motorEnabled_, (axStatus != -3 && axStatus != -5));
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorEnabled_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusMoving_, *moving);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusDone_, !(*moving));
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusDirection_, direction);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusDirection_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status =
|
|
pC_->setDoubleParam(axisNo_, pC_->motorHighLimitFromDriver_, highLimit);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status =
|
|
pC_->setDoubleParam(axisNo_, pC_->motorLowLimitFromDriver_, lowLimit);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Transform from motor to EPICS coordinates (see comment in
|
|
// pmacV3Axis::readConfig())
|
|
currentPosition = currentPosition / motorRecResolution;
|
|
|
|
pl_status = setDoubleParam(pC_->motorPosition_, currentPosition);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
return poll_status;
|
|
}
|
|
|
|
asynStatus pmacV3Axis::doMove(double position, int relative, double minVelocity,
|
|
double maxVelocity, double acceleration) {
|
|
|
|
// Status of read-write-operations of ASCII commands to the controller
|
|
asynStatus rw_status = asynSuccess;
|
|
|
|
// Status of parameter library operations
|
|
asynStatus pl_status = asynSuccess;
|
|
|
|
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
|
double motorCoordinatesPosition = 0.0;
|
|
int enabled = 0;
|
|
double motorRecResolution = 0.0;
|
|
|
|
// =========================================================================
|
|
|
|
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnabled_, &enabled);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorEnabled_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
|
|
&motorRecResolution);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
if (enabled == 0) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nAxis %d is disabled.\n", __PRETTY_FUNCTION__,
|
|
__LINE__, axisNo_);
|
|
return asynSuccess;
|
|
}
|
|
|
|
// Convert from EPICS to user / motor units
|
|
motorCoordinatesPosition = position * motorRecResolution;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nStart of axis %d to position %lf.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, position);
|
|
|
|
// Perform handshake, Set target position and start the move command
|
|
if (relative) {
|
|
snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d02=%lf M%2.2d=2",
|
|
axisNo_, axisNo_, motorCoordinatesPosition, axisNo_);
|
|
} else {
|
|
snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d01=%lf M%2.2d=1",
|
|
axisNo_, axisNo_, motorCoordinatesPosition, axisNo_);
|
|
}
|
|
|
|
// We don't expect an answer
|
|
rw_status = pC_->writeRead(axisNo_, command, response, 0);
|
|
if (rw_status != asynSuccess) {
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nStarting movement to target position %lf "
|
|
"failed for axis %d.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, motorCoordinatesPosition,
|
|
axisNo_);
|
|
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
return rw_status;
|
|
}
|
|
|
|
// In the next poll, we will check if the handshake has been performed in a
|
|
// reasonable time
|
|
waitForHandshake_ = true;
|
|
timeAtHandshake_ = time(NULL);
|
|
|
|
// Waiting for a handshake is already part of the movement procedure =>
|
|
// Start the watchdog
|
|
if (startMovTimeoutWatchdog() != asynSuccess) {
|
|
return asynError;
|
|
}
|
|
|
|
return rw_status;
|
|
}
|
|
|
|
asynStatus pmacV3Axis::stop(double acceleration) {
|
|
|
|
// Status of read-write-operations of ASCII commands to the controller
|
|
asynStatus rw_status = asynSuccess;
|
|
|
|
// Status of parameter library operations
|
|
asynStatus pl_status = asynSuccess;
|
|
|
|
bool moving = false;
|
|
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
|
|
|
// =========================================================================
|
|
|
|
pl_status = doPoll(&moving);
|
|
if (pl_status != asynSuccess) {
|
|
return pl_status;
|
|
}
|
|
|
|
if (moving) {
|
|
// Only send a stop when actually moving
|
|
snprintf(command, sizeof(command), "M%2.2d=8", axisNo_);
|
|
rw_status = pC_->writeRead(axisNo_, command, response, 0);
|
|
|
|
if (rw_status != asynSuccess) {
|
|
asynPrint(
|
|
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nStopping the movement failed for axis %d.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status,
|
|
"motorStatusProblem_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
}
|
|
}
|
|
return rw_status;
|
|
}
|
|
|
|
/*
|
|
Home the axis. On absolute encoder systems, this is a no-op
|
|
*/
|
|
asynStatus pmacV3Axis::doHome(double min_velocity, double max_velocity,
|
|
double acceleration, int forwards) {
|
|
|
|
// Status of read-write-operations of ASCII commands to the controller
|
|
asynStatus rw_status = asynSuccess;
|
|
|
|
// Status of parameter library operations
|
|
asynStatus pl_status = asynSuccess;
|
|
|
|
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
|
|
|
// =========================================================================
|
|
|
|
pl_status = pC_->getStringParam(axisNo_, pC_->encoderType_,
|
|
sizeof(response), response);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "encoderType_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Only send the home command if the axis has an incremental encoder
|
|
if (strcmp(response, IncrementalEncoder) == 0) {
|
|
snprintf(command, sizeof(command), "M%2.2d=9", axisNo_);
|
|
rw_status = pC_->writeRead(axisNo_, command, response, 0);
|
|
if (rw_status != asynSuccess) {
|
|
return rw_status;
|
|
}
|
|
|
|
pl_status = setIntegerParam(pC_->motorMoveToHome_, 1);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = setStringParam(pC_->motorMessageText_, "Homing");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
} else {
|
|
pl_status = setStringParam(pC_->motorMessageText_,
|
|
"Can't home a motor with absolute encoder");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
}
|
|
|
|
return rw_status;
|
|
}
|
|
|
|
/*
|
|
Read the encoder type and update the parameter library accordingly
|
|
*/
|
|
asynStatus pmacV3Axis::readEncoderType() {
|
|
|
|
// Status of read-write-operations of ASCII commands to the controller
|
|
asynStatus rw_status = asynSuccess;
|
|
|
|
// Status of parameter library operations
|
|
asynStatus pl_status = asynSuccess;
|
|
|
|
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
|
int nvals = 0;
|
|
int encoder_id = 0;
|
|
|
|
// =========================================================================
|
|
|
|
// Check if this is an absolute encoder
|
|
snprintf(command, sizeof(command), "I%2.2X04", axisNo_);
|
|
rw_status = pC_->writeRead(axisNo_, command, response, 1);
|
|
if (rw_status != asynSuccess) {
|
|
return rw_status;
|
|
}
|
|
|
|
int reponse_length = strlen(response);
|
|
if (reponse_length < 3) {
|
|
asynPrint(
|
|
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s => line %d:\nUnexpected reponse '%s' from axis %d on "
|
|
"controller %s while reading the encoder type. Aborting....\n",
|
|
__PRETTY_FUNCTION__, __LINE__, response, axisNo_, pC_->portName);
|
|
return asynError;
|
|
}
|
|
|
|
// We are only interested in the last two digits and the last value in
|
|
// the string before the terminator is \r
|
|
nvals = sscanf(response + (reponse_length - 3), "%2X", &encoder_id);
|
|
if (nvals != 1) {
|
|
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
snprintf(command, sizeof(command), "P46");
|
|
rw_status = pC_->writeRead(axisNo_, command, response, 1);
|
|
if (rw_status != asynSuccess) {
|
|
return rw_status;
|
|
}
|
|
int number_of_axes = strtol(response, NULL, 10);
|
|
|
|
// If true, the encoder is incremental
|
|
if (encoder_id <= number_of_axes) {
|
|
pl_status = setStringParam(pC_->encoderType_, IncrementalEncoder);
|
|
} else {
|
|
pl_status = setStringParam(pC_->encoderType_, AbsoluteEncoder);
|
|
}
|
|
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "encoderType_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
return asynSuccess;
|
|
}
|
|
|
|
/*
|
|
This is not a command that can always be run when enabling a motor as it also
|
|
causes relative encoders to reread a position necessitating recalibration. We
|
|
only want it to run on absolute encoders. We also want it to be clear to
|
|
instrument scientists, that power has to be cut to the motor, in order to reread
|
|
the encoder as not all motors have breaks and they may start to move when
|
|
disabled. For that reason, we don't automatically disable the motors to run the
|
|
command and instead require that the scientists first disable the motor.
|
|
*/
|
|
asynStatus pmacV3Axis::rereadEncoder() {
|
|
char command[pC_->MAXBUF_] = {0};
|
|
char response[pC_->MAXBUF_] = {0};
|
|
char encoderType[pC_->MAXBUF_] = {0};
|
|
|
|
// Status of read-write-operations of ASCII commands to the controller
|
|
asynStatus rw_status = asynSuccess;
|
|
|
|
// Status of parameter library operations
|
|
asynStatus pl_status = asynSuccess;
|
|
|
|
// =========================================================================
|
|
|
|
// Check if this is an absolute encoder
|
|
rw_status = readEncoderType();
|
|
if (rw_status != asynSuccess) {
|
|
return rw_status;
|
|
}
|
|
pl_status = pC_->getStringParam(axisNo_, pC_->encoderType_,
|
|
sizeof(encoderType), encoderType);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "encoderType_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Abort if the axis is incremental
|
|
if (strcmp(encoderType, IncrementalEncoder) == 1) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING,
|
|
"%s => line %d:\nTrying to reread absolute encoder of "
|
|
"axis %d on controller %s, but it is a relative encoder.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, pC_->portName);
|
|
pl_status = setStringParam(pC_->motorMessageText_,
|
|
"Cannot reread an incremental encoder.");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
return asynError;
|
|
}
|
|
|
|
// Check if the axis is disabled. If not, inform the user that this
|
|
// is necessary
|
|
int enabled = 0;
|
|
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnabled_, &enabled);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorEnabled_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
if (enabled == 1) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING,
|
|
"%s => line %d:\nAxis %d on controller %s must be "
|
|
"disabled before rereading the encoder.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, pC_->portName);
|
|
pl_status = setStringParam(
|
|
pC_->motorMessageText_,
|
|
"Axis must be disabled before rereading the encoder.");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
return asynError;
|
|
} else {
|
|
snprintf(command, sizeof(command), "M%2.2d=15", axisNo_);
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nRereading absolute encoder of axis %d on "
|
|
"controller %s via command %s.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, pC_->portName,
|
|
command);
|
|
pC_->writeRead(axisNo_, command, response, 0);
|
|
}
|
|
|
|
// Switching on the axis while the rereading process is still
|
|
// ongoing causes it to fail. We currently have no way to check if
|
|
// it is actually finished, so we instead wait for 0.5 seconds.
|
|
usleep(500000);
|
|
|
|
// turn off parameter as finished rereading
|
|
// this will only be immediately noticed in the read back variable
|
|
// though
|
|
pl_status = pC_->setIntegerParam(pC_->rereadEncoderPosition_, 0);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "rereadEncoderPosition_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus pmacV3Axis::enable(bool on) {
|
|
|
|
int timeout_enable_disable = 2;
|
|
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
|
int nvals = 0;
|
|
|
|
// Status of read-write-operations of ASCII commands to the controller
|
|
asynStatus rw_status = asynSuccess;
|
|
|
|
// Status of parameter library operations
|
|
asynStatus pl_status = asynSuccess;
|
|
|
|
bool moving = false;
|
|
|
|
// =========================================================================
|
|
|
|
// If the axis is currently moving, it cannot be disabled. Ignore the
|
|
// command and inform the user
|
|
doPoll(&moving); // Muss gehen wenn Status -6
|
|
if (moving) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING,
|
|
"%s => line %d:\nAxis %d is not idle and can therefore not "
|
|
"be enabled / disabled.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_);
|
|
|
|
pl_status =
|
|
setStringParam(pC_->motorMessageText_,
|
|
"Axis cannot be disabled while it is moving.");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Reset the value in the param lib.
|
|
pl_status = setIntegerParam(pC_->enableMotor_, 1);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "enableMotor_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
return asynError;
|
|
}
|
|
|
|
// Axis is already enabled / disabled and a new enable / disable command
|
|
// was sent => Do nothing
|
|
if ((axisStatus_ != -3) == on) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING,
|
|
"%s => line %d:\nAxis %d on controller %s is already %s.\n",
|
|
__PRETTY_FUNCTION__, __LINE__, axisNo_, pC_->portName,
|
|
on ? "enabled" : "disabled");
|
|
return asynSuccess;
|
|
}
|
|
|
|
// Enable / disable the axis if it is not moving
|
|
snprintf(command, sizeof(command), "M%2.2d14=%d", axisNo_, on);
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\n%s axis %d on controller %s\n",
|
|
__PRETTY_FUNCTION__, __LINE__, on ? "Enable" : "Disable", axisNo_,
|
|
pC_->portName);
|
|
if (on == 0) {
|
|
pl_status = setStringParam(pC_->motorMessageText_, "Disabling ...");
|
|
} else {
|
|
pl_status = setStringParam(pC_->motorMessageText_, "Enabling ...");
|
|
}
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
rw_status = pC_->writeRead(axisNo_, command, response, 0);
|
|
if (rw_status != asynSuccess) {
|
|
return rw_status;
|
|
}
|
|
|
|
// Query the axis status every few milliseconds until the axis has been
|
|
// enabled or until the timeout has been reached
|
|
snprintf(command, sizeof(command), "P%2.2d00", axisNo_);
|
|
int startTime = time(NULL);
|
|
while (time(NULL) < startTime + timeout_enable_disable) {
|
|
|
|
// Read the axis status
|
|
usleep(100000);
|
|
rw_status = pC_->writeRead(axisNo_, command, response, 1);
|
|
if (rw_status != asynSuccess) {
|
|
return rw_status;
|
|
}
|
|
nvals = sscanf(response, "%d", &axisStatus_);
|
|
if (nvals != 1) {
|
|
return pC_->errMsgCouldNotParseResponse(
|
|
command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
if ((axisStatus_ != -3) == on) {
|
|
bool moving = false;
|
|
// Perform a poll to update the parameter library
|
|
poll(&moving);
|
|
return asynSuccess;
|
|
}
|
|
}
|
|
|
|
// Failed to change axis status within timeout_enable_disable => Send a
|
|
// corresponding message
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s => line %d:\nFailed to %s axis %d on controller %s within %d "
|
|
"seconds\n",
|
|
__PRETTY_FUNCTION__, __LINE__, on ? "enable" : "disable", axisNo_,
|
|
pC_->portName, timeout_enable_disable);
|
|
|
|
// Output message to user
|
|
snprintf(command, sizeof(command), "Failed to %s within %d seconds",
|
|
on ? "enable" : "disable", timeout_enable_disable);
|
|
pl_status = setStringParam(pC_->motorMessageText_, "Enabling ...");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
return asynError;
|
|
}
|