1465 lines
56 KiB
C++
1465 lines
56 KiB
C++
#include "turboPmacAxis.h"
|
|
#include "asynOctetSyncIO.h"
|
|
#include "epicsExport.h"
|
|
#include "iocsh.h"
|
|
#include "pmacAsynIPPort.h"
|
|
#include "turboPmacController.h"
|
|
#include <cmath>
|
|
#include <errlog.h>
|
|
#include <initHooks.h>
|
|
#include <limits>
|
|
#include <math.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <vector>
|
|
|
|
/*
|
|
Contains all instances of turboPmacAxis which have been created and is used in
|
|
the initialization hook function.
|
|
*/
|
|
static std::vector<turboPmacAxis *> axes;
|
|
|
|
/**
|
|
* @brief Hook function to perform certain actions during the IOC initialization
|
|
*
|
|
* @param iState
|
|
*/
|
|
static void epicsInithookFunction(initHookState iState) {
|
|
if (iState == initHookAfterDatabaseRunning) {
|
|
// Iterate through all axes of each and call the initialization method
|
|
// on each one of them.
|
|
for (std::vector<turboPmacAxis *>::iterator itA = axes.begin();
|
|
itA != axes.end(); ++itA) {
|
|
turboPmacAxis *axis = *itA;
|
|
axis->init();
|
|
}
|
|
}
|
|
}
|
|
|
|
turboPmacAxis::turboPmacAxis(turboPmacController *pC, int axisNo,
|
|
bool initialize)
|
|
: sinqAxis(pC, axisNo), pC_(pC) {
|
|
|
|
asynStatus status = asynSuccess;
|
|
|
|
if (initialize) {
|
|
// Register the hook function during construction of the first axis
|
|
// object
|
|
if (axes.empty()) {
|
|
initHookRegister(&epicsInithookFunction);
|
|
}
|
|
|
|
// Collect all axes into this list which will be used in the hook
|
|
// function
|
|
axes.push_back(this);
|
|
}
|
|
|
|
// Initialize all member variables
|
|
waitForHandshake_ = false;
|
|
timeAtHandshake_ = 0;
|
|
axisStatus_ = 0;
|
|
|
|
// Provide initial values for some parameter library entries
|
|
status = pC_->setIntegerParam(axisNo_, pC_->rereadEncoderPosition(), 0);
|
|
if (status != asynSuccess) {
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed with %s)\n. Terminating IOC",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
// Even though this happens already in sinqAxis, a default value for
|
|
// motorMessageText is set here again, because apparently the sinqAxis
|
|
// constructor is not run before the string is accessed?
|
|
status = setStringParam(pC_->motorMessageText(), "");
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
|
|
"(setting a parameter value failed "
|
|
"with %s)\n. Terminating IOC",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
exit(-1);
|
|
}
|
|
|
|
// turboPmac motors can always be disabled
|
|
status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable(), 1);
|
|
if (status != asynSuccess) {
|
|
pC_->paramLibAccessFailed(status, "motorCanDisable", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Default values for the watchdog timeout mechanism
|
|
offsetMovTimeout_ = 30; // seconds
|
|
scaleMovTimeout_ = 2.0;
|
|
}
|
|
|
|
turboPmacAxis::~turboPmacAxis(void) {
|
|
// Since the controller memory is managed somewhere else, we don't need to
|
|
// clean up the pointer pC here.
|
|
}
|
|
|
|
asynStatus turboPmacAxis::init() {
|
|
|
|
// Local variable declaration
|
|
asynStatus status = asynSuccess;
|
|
char command[pC_->MAXBUF_] = {0};
|
|
char response[pC_->MAXBUF_] = {0};
|
|
int nvals = 0;
|
|
double motorRecResolution = 0.0;
|
|
double motorPos = 0.0;
|
|
double motorVelocity = 0.0;
|
|
double motorVmax = 0.0;
|
|
double motorAccel = 0.0;
|
|
int acoDelay = 0.0; // Offset time for the movement watchdog caused by
|
|
// the air cushions in milliseconds.
|
|
int axStatus = 0;
|
|
|
|
// The parameter library takes some time to be initialized. Therefore we
|
|
// wait until the status is not asynParamUndefined anymore.
|
|
time_t now = time(NULL);
|
|
time_t maxInitTime = 60;
|
|
while (1) {
|
|
status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
|
|
&motorRecResolution);
|
|
if (status == asynParamUndefined) {
|
|
if (now + maxInitTime < time(NULL)) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line "
|
|
"%d\nInitializing the parameter library failed.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
return asynError;
|
|
}
|
|
} else if (status == asynSuccess) {
|
|
break;
|
|
} else if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorRecResolution_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
}
|
|
|
|
/*
|
|
Read out the axis status, the current position, current and maximum speed,
|
|
acceleration and the air cushion delay.
|
|
*/
|
|
snprintf(command, sizeof(command),
|
|
"P%2.2d00 Q%2.2d10 Q%2.2d03 Q%2.2d04 Q%2.2d06 P%2.2d22", axisNo_,
|
|
axisNo_, axisNo_, axisNo_, axisNo_, axisNo_);
|
|
status = pC_->writeRead(axisNo_, command, response, 6);
|
|
if (status != asynSuccess) {
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nCould not communicate "
|
|
"with controller during IOC initialization. Check if you used "
|
|
"\"pmacAsynIPPortConfigure\" instead of the standard "
|
|
"\"drvAsynIPPortConfigure\" function in the .cmd file in order to "
|
|
"create the port driver.\nTerminating IOC.\n",
|
|
pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__);
|
|
exit(-1);
|
|
}
|
|
nvals = sscanf(response, "%d %lf %lf %lf %lf %d", &axStatus, &motorPos,
|
|
&motorVmax, &motorVelocity, &motorAccel, &acoDelay);
|
|
|
|
// The acoDelay is given in milliseconds -> Convert to seconds, rounded up
|
|
offsetMovTimeout_ = std::ceil(acoDelay / 1000.0);
|
|
|
|
// The PMAC electronic specifies the acceleration in m/s^2. Since we
|
|
// otherwise work with the base length mm, the acceleration is converted
|
|
// here to mm/s^2.
|
|
motorAccel = motorAccel * 1000;
|
|
|
|
if (nvals != 6) {
|
|
return pC_->couldNotParseResponse(command, response, axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Store these values in the parameter library
|
|
status = setMotorPosition(motorPos);
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
|
|
// Initial motor status is idle
|
|
status = pC_->setIntegerParam(axisNo_, pC_->motorStatusDone(), 1);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorStatusDone_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Write to the motor record fields
|
|
status = setVeloFields(motorVelocity, 0.0, motorVmax);
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
status = setAcclField(motorAccel);
|
|
if (status != asynSuccess) {
|
|
return status;
|
|
}
|
|
|
|
// 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_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\ncallParamCallbacks "
|
|
"failed with %s.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->stringifyAsynStatus(status));
|
|
return status;
|
|
}
|
|
|
|
return this->readEncoderType();
|
|
}
|
|
|
|
// Perform the actual poll
|
|
asynStatus turboPmacAxis::doPoll(bool *moving) {
|
|
|
|
// Return value for the poll
|
|
asynStatus errorStatus = 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_] = {0};
|
|
char response[pC_->MAXBUF_] = {0};
|
|
char userMessage[pC_->MAXBUF_] = {0};
|
|
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;
|
|
double limitsOffset = 0.0;
|
|
|
|
// Was the axis idle during the previous poll?
|
|
int previousStatusDone = 1;
|
|
|
|
// =========================================================================
|
|
|
|
// Are we currently waiting for a handshake?
|
|
if (waitForHandshake_) {
|
|
|
|
// Check if the handshake takes too long and communicate an error in
|
|
// this case. A handshake should not take more than 5 seconds.
|
|
time_t currentTime = time(NULL);
|
|
bool timedOut = (currentTime > timeAtHandshake_ + 5);
|
|
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, timedOut,
|
|
pC_->pasynUser())) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nAsked for a "
|
|
"handshake at %ld s and didn't get a positive reply yet "
|
|
"(current time is %ld s).\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
timeAtHandshake_, currentTime);
|
|
}
|
|
|
|
if (timedOut) {
|
|
pl_status =
|
|
setStringParam(pC_->motorMessageText(),
|
|
"Timed out while waiting for a handshake");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
}
|
|
|
|
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_->couldNotParseResponse(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 - try again in the next busy
|
|
// poll. This is already part of the movement procedure.
|
|
*moving = true;
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusMoving(), *moving);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status,
|
|
"motorStatusMoving_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusDone(), !(*moving));
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
return asynSuccess;
|
|
}
|
|
}
|
|
|
|
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
|
|
&motorRecResolution);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
// Read the previous motor position
|
|
pl_status = motorPosition(&previousPosition);
|
|
if (pl_status != asynSuccess) {
|
|
return pl_status;
|
|
}
|
|
|
|
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorStatusDone(),
|
|
&previousStatusDone);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// 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_->couldNotParseResponse(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 should 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 limitsOffset on both sides.
|
|
*/
|
|
pl_status =
|
|
pC_->getDoubleParam(axisNo_, pC_->motorLimitsOffset(), &limitsOffset);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorLimitsOffset_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
highLimit = highLimit - limitsOffset;
|
|
lowLimit = lowLimit + limitsOffset;
|
|
|
|
// Store the axis status
|
|
axisStatus_ = axStatus;
|
|
|
|
// Update the enablement PV
|
|
pl_status = setIntegerParam(pC_->motorEnableRBV(),
|
|
(axStatus != -3 && axStatus != -5));
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorEnableRBV_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Create the unique callsite identifier manually so it can be used later in
|
|
// the shouldBePrinted calls.
|
|
msgPrintControlKey keyStatus = msgPrintControlKey(
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
|
bool resetCountStatus = true;
|
|
|
|
// Interpret the status
|
|
switch (axStatus) {
|
|
case -6:
|
|
// Axis is stopping
|
|
*moving = true;
|
|
break;
|
|
case -5:
|
|
// Axis is deactivated
|
|
*moving = false;
|
|
break;
|
|
case -4:
|
|
// Emergency stop
|
|
*moving = false;
|
|
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyStatus, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nEmergency stop "
|
|
"activated.%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetCountStatus = false;
|
|
|
|
pl_status = setStringParam(pC_->motorMessageText(), "Emergency stop");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
break;
|
|
case -3:
|
|
// Disabled
|
|
*moving = false;
|
|
break;
|
|
case 0:
|
|
// Idle
|
|
*moving = false;
|
|
break;
|
|
case 1:
|
|
// Move order acknowledged
|
|
*moving = true;
|
|
break;
|
|
case 2:
|
|
// Move order confirmed possible
|
|
*moving = true;
|
|
break;
|
|
case 3:
|
|
// Axis in Air Cushion Output status
|
|
*moving = true;
|
|
break;
|
|
case 4:
|
|
// Axis in Air Cushion Input status
|
|
*moving = true;
|
|
break;
|
|
case 5:
|
|
*moving = true;
|
|
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_FLOW,
|
|
"Controller \"%s\", axis %d => %s, line %d\nAxis is moving\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
|
|
|
break;
|
|
default:
|
|
*moving = false;
|
|
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyStatus, true,
|
|
pC_->pasynUser())) {
|
|
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nUnknown "
|
|
"state P%2.2d00 = %d.%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
axisNo_, axStatus, pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetCountStatus = false;
|
|
|
|
snprintf(
|
|
userMessage, sizeof(userMessage),
|
|
"Reached unknown state P%2.2d00 = %d. Please call the support.",
|
|
axisNo_, error);
|
|
pl_status = setStringParam(pC_->motorMessageText(), userMessage);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
}
|
|
|
|
if (resetCountStatus) {
|
|
pC_->getMsgPrintControl().resetCount(keyStatus, pC_->pasynUser());
|
|
}
|
|
|
|
if (*moving) {
|
|
// If the axis is moving, evaluate the movement direction
|
|
if ((currentPosition - previousPosition) > 0) {
|
|
direction = 1;
|
|
} else {
|
|
direction = 0;
|
|
}
|
|
}
|
|
|
|
errorStatus = handleError(error, userMessage, sizeof(userMessage));
|
|
|
|
// Update the parameter library
|
|
if (error != 0) {
|
|
pl_status = setIntegerParam(pC_->motorStatusProblem(), true);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
}
|
|
|
|
if (*moving == false) {
|
|
pl_status = setIntegerParam(pC_->motorMoveToHome(), 0);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
}
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusMoving(), *moving);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusDone(), !(*moving));
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusDirection(), direction);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusDirection_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
pl_status = pC_->setDoubleParam(axisNo_, pC_->motorHighLimitFromDriver(),
|
|
highLimit);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
pl_status =
|
|
pC_->setDoubleParam(axisNo_, pC_->motorLowLimitFromDriver(), lowLimit);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
pl_status = setMotorPosition(currentPosition);
|
|
if (pl_status != asynSuccess) {
|
|
return pl_status;
|
|
}
|
|
return errorStatus;
|
|
}
|
|
|
|
asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
|
int sizeUserMessage) {
|
|
asynStatus status = asynSuccess;
|
|
|
|
// Create the unique callsite identifier manually so it can be used later in
|
|
// the shouldBePrinted calls.
|
|
msgPrintControlKey keyError = msgPrintControlKey(
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
|
bool resetError = true;
|
|
|
|
switch (error) {
|
|
case 0:
|
|
// No error -> Reset the message repetition watchdog
|
|
break;
|
|
case 1:
|
|
// EPICS should already prevent this issue in the first place,
|
|
// since it contains the user limits
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nTarget "
|
|
"position would exceed user limits.%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetError = false;
|
|
|
|
status = setStringParam(pC_->motorMessageText(),
|
|
"Target position would exceed software limits");
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = asynError;
|
|
break;
|
|
case 5:
|
|
// Command not possible
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nAxis is "
|
|
"still moving, but received another move command. EPICS "
|
|
"should prevent this, check if *moving is set correctly.%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetError = false;
|
|
|
|
status = setStringParam(pC_->motorMessageText(),
|
|
"Axis received move command while it is "
|
|
"still moving. Please call the support.");
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = asynError;
|
|
break;
|
|
case 8:
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nAir cushion "
|
|
"feedback stopped during movement (P%2.2d01 = %d).%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
axisNo_, error, pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetError = false;
|
|
|
|
snprintf(userMessage, sizeUserMessage,
|
|
"Air cushion feedback stopped during movement (P%2.2d01 = "
|
|
"%d). Please call the support.",
|
|
axisNo_, error);
|
|
|
|
status = setStringParam(pC_->motorMessageText(), userMessage);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
break;
|
|
case 9:
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nNo air cushion "
|
|
"feedback before movement start (P%2.2d01 = %d).%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, axisNo_,
|
|
error, pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetError = false;
|
|
|
|
snprintf(userMessage, sizeUserMessage,
|
|
"No air cushion feedback before movement start (P%2.2d01 = "
|
|
"%d). Please call the support.",
|
|
axisNo_, error);
|
|
status = setStringParam(pC_->motorMessageText(), userMessage);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __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.
|
|
*/
|
|
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nAxis hit the "
|
|
"controller limits.%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetError = false;
|
|
|
|
snprintf(userMessage, sizeUserMessage,
|
|
"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);
|
|
|
|
status = setStringParam(pC_->motorMessageText(), userMessage);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = asynError;
|
|
break;
|
|
case 11:
|
|
// Following error
|
|
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nMaximum allowed "
|
|
"following error exceeded.%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetError = false;
|
|
|
|
snprintf(userMessage, sizeUserMessage,
|
|
"Maximum allowed following error exceeded (P%2.2d01 = %d). "
|
|
"Check if movement range is blocked. "
|
|
"Otherwise please call the support.",
|
|
axisNo_, error);
|
|
status = setStringParam(pC_->motorMessageText(), userMessage);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = asynError;
|
|
break;
|
|
|
|
case 12:
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nSecurity "
|
|
"input is triggered (P%2.2d01 = %d).%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
axisNo_, error, pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetError = false;
|
|
|
|
snprintf(userMessage, sizeUserMessage,
|
|
"Security input is triggered (P%2.2d01 = %d). Check the SPS "
|
|
"for errors (if available). Otherwise please call "
|
|
"the support.",
|
|
axisNo_, error);
|
|
status = setStringParam(pC_->motorMessageText(), userMessage);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = asynError;
|
|
break;
|
|
|
|
case 13:
|
|
// Driver hardware error triggered
|
|
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nDriver "
|
|
"hardware error triggered.%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetError = false;
|
|
|
|
snprintf(
|
|
userMessage, sizeUserMessage,
|
|
"Driver hardware error (P%2.2d01 = 13). Please call the support.",
|
|
axisNo_);
|
|
status = setStringParam(pC_->motorMessageText(), userMessage);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = asynError;
|
|
break;
|
|
case 14:
|
|
// EPICS should already prevent this issue in the first place,
|
|
// since it contains the user limits
|
|
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nMove "
|
|
"command exceeds hardware limits (P%2.2d01 = %d).%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
axisNo_, error, pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetError = false;
|
|
|
|
snprintf(userMessage, sizeUserMessage,
|
|
"Move command exceeds hardware limits (P%2.2d01 = %d). Please "
|
|
"call the support.",
|
|
axisNo_, error);
|
|
status = setStringParam(pC_->motorMessageText(), userMessage);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = asynError;
|
|
break;
|
|
default:
|
|
|
|
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
|
pC_->pasynUser())) {
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nUnknown error "
|
|
"P%2.2d01 = %d.%s\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, axisNo_,
|
|
error, pC_->getMsgPrintControl().getSuffix());
|
|
}
|
|
resetError = false;
|
|
|
|
snprintf(userMessage, sizeUserMessage,
|
|
"Unknown error P%2.2d01 = %d. Please call the support.",
|
|
axisNo_, error);
|
|
status = setStringParam(pC_->motorMessageText(), userMessage);
|
|
if (status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
status = asynError;
|
|
break;
|
|
}
|
|
|
|
if (resetError) {
|
|
pC_->getMsgPrintControl().resetCount(keyError, pC_->pasynUser());
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus turboPmacAxis::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_] = {0};
|
|
char response[pC_->MAXBUF_] = {0};
|
|
double motorCoordinatesPosition = 0.0;
|
|
double motorRecResolution = 0.0;
|
|
double motorVelocity = 0.0;
|
|
int enabled = 0;
|
|
int motorCanSetSpeed = 0;
|
|
int writeOffset = 0;
|
|
|
|
// =========================================================================
|
|
|
|
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableRBV(), &enabled);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_", 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__);
|
|
}
|
|
|
|
if (enabled == 0) {
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nAxis is disabled.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
|
return asynSuccess;
|
|
}
|
|
|
|
// Convert from EPICS to user / motor units
|
|
motorCoordinatesPosition = position * motorRecResolution;
|
|
motorVelocity = maxVelocity * motorRecResolution;
|
|
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW,
|
|
"Controller \"%s\", axis %d => %s, line %d\nStart of axis to "
|
|
"position %lf.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, position);
|
|
|
|
// Check if the speed is allowed to be changed
|
|
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorCanSetSpeed(),
|
|
&motorCanSetSpeed);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorCanSetSpeed_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
|
|
// Prepend the new motor speed, if the user is allowed to set the speed.
|
|
// Mind the " " (space) before the closing "", as the command created here
|
|
// is prepended to the one down below.
|
|
if (motorCanSetSpeed != 0) {
|
|
snprintf(command, sizeof(command), "Q%2.2d04=%lf ", axisNo_,
|
|
motorVelocity);
|
|
writeOffset = strlen(command);
|
|
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW,
|
|
"Controller \"%s\", axis %d => %s, line %d\nSetting speed "
|
|
"to %lf.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
motorVelocity);
|
|
}
|
|
|
|
// Perform handshake, Set target position (and speed, if allowed) and start
|
|
// the move command
|
|
if (relative) {
|
|
snprintf(&command[writeOffset], sizeof(command) - writeOffset,
|
|
"P%2.2d23=0 Q%2.2d02=%lf M%2.2d=2", axisNo_, axisNo_,
|
|
motorCoordinatesPosition, axisNo_);
|
|
} else {
|
|
snprintf(&command[writeOffset], sizeof(command) - writeOffset,
|
|
"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_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nStarting movement to "
|
|
"target position %lf failed.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
motorCoordinatesPosition);
|
|
pl_status = setIntegerParam(pC_->motorStatusProblem(), true);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
|
axisNo_, __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 turboPmacAxis::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;
|
|
|
|
char command[pC_->MAXBUF_] = {0};
|
|
char response[pC_->MAXBUF_] = {0};
|
|
|
|
// =========================================================================
|
|
|
|
snprintf(command, sizeof(command), "M%2.2d=8", axisNo_);
|
|
rw_status = pC_->writeRead(axisNo_, command, response, 0);
|
|
|
|
if (rw_status != asynSuccess) {
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nStopping the movement "
|
|
"failed.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusProblem(), true);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
}
|
|
|
|
return rw_status;
|
|
}
|
|
|
|
asynStatus turboPmacAxis::doReset() {
|
|
|
|
// 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_] = {0};
|
|
char response[pC_->MAXBUF_] = {0};
|
|
|
|
// =========================================================================
|
|
|
|
// Reset the error for this axis manually
|
|
snprintf(command, sizeof(command), "P%2.2d01=0", axisNo_);
|
|
rw_status = pC_->writeRead(axisNo_, command, response, 0);
|
|
|
|
if (rw_status != asynSuccess) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nResetting the "
|
|
"error failed\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
|
|
|
pl_status = setIntegerParam(pC_->motorStatusProblem(), true);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
}
|
|
|
|
return rw_status;
|
|
}
|
|
|
|
/*
|
|
Home the axis. On absolute encoder systems, this is a no-op
|
|
*/
|
|
asynStatus turboPmacAxis::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_] = {0};
|
|
char response[pC_->MAXBUF_] = {0};
|
|
|
|
// =========================================================================
|
|
|
|
pl_status = pC_->getStringParam(axisNo_, pC_->encoderType(),
|
|
sizeof(response), response);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "encoderType_", axisNo_,
|
|
__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_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
return callParamCallbacks();
|
|
}
|
|
|
|
return asynSuccess;
|
|
}
|
|
|
|
/*
|
|
Read the encoder type and update the parameter library accordingly
|
|
*/
|
|
asynStatus turboPmacAxis::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_] = {0};
|
|
char response[pC_->MAXBUF_] = {0};
|
|
int nvals = 0;
|
|
int encoder_id = 0;
|
|
|
|
// =========================================================================
|
|
|
|
// Check if this is an absolute encoder
|
|
snprintf(command, sizeof(command), "I%2.2d04", 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) {
|
|
return pC_->couldNotParseResponse(command, response, axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// 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_->couldNotParseResponse(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_", axisNo_,
|
|
__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 turboPmacAxis::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_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
// Abort if the axis is incremental
|
|
if (strcmp(encoderType, IncrementalEncoder) == 0) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW,
|
|
"Controller \"%s\", axis %d => %s, line %d\nEncoder is "
|
|
"not reread because it is incremental.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
|
return asynSuccess;
|
|
}
|
|
|
|
// Check if the axis is disabled. If not, inform the user that this
|
|
// is necessary
|
|
int enabled = 0;
|
|
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableRBV(), &enabled);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_", axisNo_,
|
|
__PRETTY_FUNCTION__, __LINE__);
|
|
}
|
|
|
|
if (enabled == 1) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_WARNING,
|
|
"Controller \"%s\", axis %d => %s, line %d\nAxis must be "
|
|
"disabled before rereading the encoder.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
|
pl_status = setStringParam(
|
|
pC_->motorMessageText(),
|
|
"Axis must be disabled before rereading the encoder.");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
return asynError;
|
|
} else {
|
|
snprintf(command, sizeof(command), "M%2.2d=15", axisNo_);
|
|
asynPrint(
|
|
pC_->pasynUser(), ASYN_TRACE_FLOW,
|
|
"Controller \"%s\", axis %d => %s, line %d\nRereading absolute "
|
|
"encoder via command %s.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, 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_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
return asynSuccess;
|
|
}
|
|
|
|
asynStatus turboPmacAxis::enable(bool on) {
|
|
|
|
int timeout_enable_disable = 2;
|
|
char command[pC_->MAXBUF_] = {0};
|
|
char response[pC_->MAXBUF_] = {0};
|
|
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;
|
|
|
|
// =========================================================================
|
|
|
|
/*
|
|
Continue regardless of the status returned by the poll; we just want to
|
|
find out whether the motor is currently moving or not. If the poll
|
|
function fails before it can determine that, it is assumed that the motor
|
|
is not moving.
|
|
*/
|
|
bool moving = false;
|
|
doPoll(&moving);
|
|
|
|
// If the axis is currently moving, it cannot be disabled. Ignore the
|
|
// command and inform the user. We check the last known status of the axis
|
|
// instead of "moving", since status -6 is also moving, but the motor can
|
|
// actually be disabled in this state!
|
|
if (axisStatus_ == 1 || axisStatus_ == 2 || axisStatus_ == 3 ||
|
|
axisStatus_ == 4 || axisStatus_ == 5) {
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
|
|
"Controller \"%s\", axis %d => %s, line %d\nAxis is not "
|
|
"idle and can therefore not be enabled / disabled.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
|
|
|
pl_status =
|
|
setStringParam(pC_->motorMessageText(),
|
|
"Axis cannot be disabled while it is moving.");
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
axisNo_, __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_->pasynUser(), ASYN_TRACE_WARNING,
|
|
"Controller \"%s\", axis %d => %s, line %d\nAxis is already %s.\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
on ? "enabled" : "disabled");
|
|
return asynSuccess;
|
|
}
|
|
|
|
// Reread the encoder, if the axis is going to be enabled
|
|
if (on != 0) {
|
|
rw_status = rereadEncoder();
|
|
if (rw_status != asynSuccess) {
|
|
return rw_status;
|
|
}
|
|
}
|
|
|
|
// Enable / disable the axis if it is not moving
|
|
snprintf(command, sizeof(command), "M%2.2d14=%d", axisNo_, on);
|
|
asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW,
|
|
"Controller \"%s\", axis %d => %s, line %d\n%s axis\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
on ? "Enable" : "Disable");
|
|
|
|
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_->couldNotParseResponse(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_->pasynUser(), ASYN_TRACE_FLOW,
|
|
"Controller \"%s\", axis %d => %s, line %d\nFailed to %s axis "
|
|
"within %d seconds\n",
|
|
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
|
on ? "enable" : "disable", 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(), command);
|
|
if (pl_status != asynSuccess) {
|
|
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
|
|
axisNo_, __PRETTY_FUNCTION__,
|
|
__LINE__);
|
|
}
|
|
return asynError;
|
|
}
|
|
|
|
/*************************************************************************************/
|
|
/** The following functions are C-wrappers, and can be called directly from
|
|
* iocsh */
|
|
|
|
extern "C" {
|
|
|
|
/*
|
|
C wrapper for the axis constructor. Please refer to the turboPmacAxis
|
|
constructor documentation. The controller is read from the portName.
|
|
*/
|
|
asynStatus turboPmacCreateAxis(const char *portName, int axis) {
|
|
|
|
/*
|
|
findAsynPortDriver is a asyn library FFI function which uses the C ABI.
|
|
Therefore it returns a void pointer instead of e.g. a pointer to a
|
|
superclass of the controller such as asynPortDriver. Type-safe upcasting
|
|
via dynamic_cast is therefore not possible directly. However, we do know
|
|
that the void pointer is either a pointer to asynPortDriver (if a driver
|
|
with the specified name exists) or a nullptr. Therefore, we first do a
|
|
nullptr check, then a cast to asynPortDriver and lastly a (typesafe)
|
|
dynamic_upcast to Controller
|
|
https://stackoverflow.com/questions/70906749/is-there-a-safe-way-to-cast-void-to-class-pointer-in-c
|
|
*/
|
|
void *ptr = findAsynPortDriver(portName);
|
|
if (ptr == nullptr) {
|
|
/*
|
|
We can't use asynPrint here since this macro would require us
|
|
to get an asynUser from a pointer to an asynPortDriver.
|
|
However, the given pointer is a nullptr and therefore doesn't
|
|
have an asynUser! printf is an EPICS alternative which
|
|
works w/o that, but doesn't offer the comfort provided
|
|
by the asynTrace-facility
|
|
*/
|
|
errlogPrintf("Controller \"%s\" => %s, line %d\nPort not found.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__);
|
|
return asynError;
|
|
}
|
|
// Unsafe cast of the pointer to an asynPortDriver
|
|
asynPortDriver *apd = (asynPortDriver *)(ptr);
|
|
|
|
// Safe downcast
|
|
turboPmacController *pC = dynamic_cast<turboPmacController *>(apd);
|
|
if (pC == nullptr) {
|
|
errlogPrintf("Controller \"%s\" => %s, line %d\nController "
|
|
"is not a turboPmacController.",
|
|
portName, __PRETTY_FUNCTION__, __LINE__);
|
|
return asynError;
|
|
}
|
|
|
|
// Prevent manipulation of the controller from other threads while we
|
|
// create the new axis.
|
|
pC->lock();
|
|
|
|
/*
|
|
We create a new instance of the axis, using the "new" keyword to
|
|
allocate it on the heap while avoiding RAII.
|
|
https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorController.cpp
|
|
https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/asynPortDriver.cpp
|
|
|
|
The created object is registered in EPICS in its constructor and can safely
|
|
be "leaked" here.
|
|
*/
|
|
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
|
|
#pragma GCC diagnostic ignored "-Wunused-variable"
|
|
turboPmacAxis *pAxis = new turboPmacAxis(pC, axis);
|
|
|
|
// Allow manipulation of the controller again
|
|
pC->unlock();
|
|
return asynSuccess;
|
|
}
|
|
|
|
/*
|
|
Same procedure as for the CreateController function, but for the axis
|
|
itself.
|
|
*/
|
|
static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mcu1)",
|
|
iocshArgString};
|
|
static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt};
|
|
static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0,
|
|
&CreateAxisArg1};
|
|
static const iocshFuncDef configTurboPmacCreateAxis = {
|
|
"turboPmacAxis", 2, CreateAxisArgs,
|
|
"Create an instance of a turboPmac axis. The first argument is the "
|
|
"controller this axis should be attached to, the second argument is the "
|
|
"axis number."};
|
|
static void configTurboPmacCreateAxisCallFunc(const iocshArgBuf *args) {
|
|
turboPmacCreateAxis(args[0].sval, args[1].ival);
|
|
}
|
|
|
|
// This function is made known to EPICS in turboPmac.dbd and is called by EPICS
|
|
// in order to register both functions in the IOC shell
|
|
static void turboPmacAxisRegister(void) {
|
|
iocshRegister(&configTurboPmacCreateAxis,
|
|
configTurboPmacCreateAxisCallFunc);
|
|
}
|
|
epicsExportRegistrar(turboPmacAxisRegister);
|
|
|
|
} // extern "C"
|