Updated sinqMotor
This commit is contained in:
Submodule sinqMotor updated: 2f8ae23d57...3d984f26bc
@ -196,12 +196,6 @@ asynStatus turboPmacAxis::init() {
|
||||
// Initial motor status is idle
|
||||
setAxisParamChecked(this, motorStatusDone, 1);
|
||||
|
||||
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) {
|
||||
@ -241,10 +235,7 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
|
||||
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;
|
||||
asynStatus status = asynSuccess;
|
||||
|
||||
char command[pC_->MAXBUF_] = {0};
|
||||
char response[pC_->MAXBUF_] = {0};
|
||||
@ -268,9 +259,9 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
|
||||
// =========================================================================
|
||||
|
||||
if (pTurboPmacA_->needInit) {
|
||||
rw_status = init();
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
status = init();
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
@ -294,22 +285,16 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
|
||||
}
|
||||
|
||||
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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorMessageText,
|
||||
"Timed out while waiting for a handshake");
|
||||
pTurboPmacA_->waitForHandshake = false;
|
||||
}
|
||||
|
||||
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;
|
||||
status = pC_->writeRead(axisNo_, command, response, 2);
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
|
||||
nvals = sscanf(response, "%d %d", &handshakePerformed, &error);
|
||||
@ -332,52 +317,30 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
|
||||
// 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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorStatusMoving, *moving);
|
||||
setAxisParamChecked(this, motorStatusDone, !(*moving));
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
}
|
||||
|
||||
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
|
||||
&motorRecResolution);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
getAxisParamChecked(this, motorRecResolution, &motorRecResolution);
|
||||
|
||||
// Read the previous motor position
|
||||
pl_status = motorPosition(&previousPosition);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pl_status;
|
||||
status = motorPosition(&previousPosition);
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
|
||||
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorStatusDone(),
|
||||
&previousStatusDone);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_", axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
getAxisParamChecked(this, motorStatusDone, &previousStatusDone);
|
||||
|
||||
// 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;
|
||||
status = pC_->writeRead(axisNo_, command, response, 5);
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
|
||||
nvals = sscanf(response, "%d %lf %d %lf %lf", &axStatus, ¤tPosition,
|
||||
@ -399,13 +362,7 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
|
||||
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__);
|
||||
}
|
||||
getAxisParamChecked(this, motorLimitsOffset, &limitsOffset);
|
||||
highLimit = highLimit - limitsOffset;
|
||||
lowLimit = lowLimit + limitsOffset;
|
||||
|
||||
@ -413,12 +370,8 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
|
||||
pTurboPmacA_->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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorEnableRBV,
|
||||
(axStatus != -3 && axStatus != -5));
|
||||
|
||||
// Create the unique callsite identifier manually so it can be used later in
|
||||
// the shouldBePrinted calls.
|
||||
@ -450,13 +403,7 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
|
||||
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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorMessageText, "Emergency stop");
|
||||
break;
|
||||
case -3:
|
||||
// Disabled
|
||||
@ -549,12 +496,7 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
|
||||
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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorMessageText, userMessage);
|
||||
}
|
||||
|
||||
if (resetCountStatus) {
|
||||
@ -574,88 +516,44 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
|
||||
|
||||
// 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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorStatusProblem, true);
|
||||
}
|
||||
|
||||
if (*moving == false) {
|
||||
pl_status = setIntegerParam(pC_->motorMoveToHome(), 0);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_",
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
setAxisParamChecked(this, motorMoveToHome, false);
|
||||
}
|
||||
|
||||
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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorStatusMoving, *moving);
|
||||
setAxisParamChecked(this, motorStatusDone, !(*moving));
|
||||
setAxisParamChecked(this, motorStatusDirection, direction);
|
||||
|
||||
int limFromHardware = 0;
|
||||
pl_status =
|
||||
pC_->getIntegerParam(axisNo_, pC_->limFromHardware(), &limFromHardware);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "limFromHardware", axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
getAxisParamChecked(this, limFromHardware, &limFromHardware);
|
||||
|
||||
if (limFromHardware != 0) {
|
||||
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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorHighLimitFromDriver, highLimit);
|
||||
setAxisParamChecked(this, motorLowLimitFromDriver, lowLimit);
|
||||
}
|
||||
|
||||
pl_status = setMotorPosition(currentPosition);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pl_status;
|
||||
status = setMotorPosition(currentPosition);
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
return errorStatus;
|
||||
}
|
||||
|
||||
asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
||||
int sizeUserMessage) {
|
||||
asynStatus status = asynSuccess;
|
||||
asynStatus status = asynError;
|
||||
|
||||
// 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:
|
||||
status = asynSuccess;
|
||||
// No error -> Reset the message repetition watchdog
|
||||
break;
|
||||
case 1:
|
||||
@ -669,17 +567,8 @@ asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
||||
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;
|
||||
setAxisParamChecked(this, motorMessageText,
|
||||
"Target position would exceed software limits");
|
||||
break;
|
||||
case 5:
|
||||
// Command not possible
|
||||
@ -693,18 +582,9 @@ asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
||||
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;
|
||||
setAxisParamChecked(this, motorMessageText,
|
||||
"Axis received move command while it is still "
|
||||
"moving. Please call the support.");
|
||||
break;
|
||||
case 8:
|
||||
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
||||
@ -715,19 +595,11 @@ asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
||||
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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorMessageText, userMessage);
|
||||
break;
|
||||
case 9:
|
||||
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
|
||||
@ -739,18 +611,12 @@ asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
||||
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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorMessageText, userMessage);
|
||||
break;
|
||||
case 10:
|
||||
/*
|
||||
@ -769,23 +635,13 @@ asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
||||
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;
|
||||
setAxisParamChecked(this, motorMessageText, userMessage);
|
||||
break;
|
||||
case 11:
|
||||
// Following error
|
||||
@ -799,21 +655,12 @@ asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
||||
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;
|
||||
setAxisParamChecked(this, motorMessageText, userMessage);
|
||||
break;
|
||||
|
||||
case 12:
|
||||
@ -825,21 +672,12 @@ asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
||||
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;
|
||||
setAxisParamChecked(this, motorMessageText, userMessage);
|
||||
break;
|
||||
|
||||
case 13:
|
||||
@ -853,25 +691,15 @@ asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
||||
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;
|
||||
setAxisParamChecked(this, motorMessageText, userMessage);
|
||||
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,
|
||||
@ -880,20 +708,11 @@ asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
||||
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;
|
||||
setAxisParamChecked(this, motorMessageText, userMessage);
|
||||
break;
|
||||
default:
|
||||
|
||||
@ -906,23 +725,11 @@ asynStatus turboPmacAxis::handleError(int error, char *userMessage,
|
||||
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;
|
||||
setAxisParamChecked(this, motorMessageText, userMessage);
|
||||
break;
|
||||
}
|
||||
|
||||
if (resetError) {
|
||||
if (status == asynSuccess) {
|
||||
pC_->getMsgPrintControl().resetCount(keyError, pC_->pasynUser());
|
||||
}
|
||||
return status;
|
||||
@ -933,10 +740,7 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
|
||||
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;
|
||||
asynStatus status = asynSuccess;
|
||||
|
||||
char command[pC_->MAXBUF_] = {0};
|
||||
char response[pC_->MAXBUF_] = {0};
|
||||
@ -949,19 +753,8 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
|
||||
|
||||
// =========================================================================
|
||||
|
||||
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__);
|
||||
}
|
||||
getAxisParamChecked(this, motorEnableRBV, &enabled);
|
||||
getAxisParamChecked(this, motorRecResolution, &motorRecResolution);
|
||||
|
||||
if (enabled == 0) {
|
||||
asynPrint(
|
||||
@ -981,13 +774,7 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
|
||||
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__);
|
||||
}
|
||||
getAxisParamChecked(this, motorCanSetSpeed, &motorCanSetSpeed);
|
||||
|
||||
// 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
|
||||
@ -1017,8 +804,8 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
|
||||
}
|
||||
|
||||
// We don't expect an answer
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, 0);
|
||||
if (rw_status != asynSuccess) {
|
||||
status = pC_->writeRead(axisNo_, command, response, 0);
|
||||
if (status != asynSuccess) {
|
||||
|
||||
asynPrint(
|
||||
pC_->pasynUser(), ASYN_TRACE_ERROR,
|
||||
@ -1026,13 +813,8 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
|
||||
"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;
|
||||
setAxisParamChecked(this, motorStatusProblem, true);
|
||||
return status;
|
||||
}
|
||||
|
||||
// In the next poll, we will check if the handshake has been performed in a
|
||||
@ -1046,16 +828,13 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
|
||||
return asynError;
|
||||
}
|
||||
|
||||
return rw_status;
|
||||
return 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;
|
||||
asynStatus status = asynSuccess;
|
||||
|
||||
char command[pC_->MAXBUF_] = {0};
|
||||
char response[pC_->MAXBUF_] = {0};
|
||||
@ -1063,37 +842,28 @@ asynStatus turboPmacAxis::stop(double acceleration) {
|
||||
// =========================================================================
|
||||
|
||||
snprintf(command, sizeof(command), "M%2.2d=8", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, 0);
|
||||
status = pC_->writeRead(axisNo_, command, response, 0);
|
||||
|
||||
if (rw_status != asynSuccess) {
|
||||
if (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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorStatusProblem, true);
|
||||
}
|
||||
|
||||
// Reset the driver to idle state and move out of the handshake wait loop,
|
||||
// if we're currently inside it.
|
||||
pTurboPmacA_->waitForHandshake = false;
|
||||
|
||||
return rw_status;
|
||||
return 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;
|
||||
asynStatus status = asynSuccess;
|
||||
|
||||
char command[pC_->MAXBUF_] = {0};
|
||||
char response[pC_->MAXBUF_] = {0};
|
||||
@ -1102,27 +872,21 @@ asynStatus turboPmacAxis::doReset() {
|
||||
|
||||
// Reset the error for this axis manually
|
||||
snprintf(command, sizeof(command), "P%2.2d01=0", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, 0);
|
||||
status = pC_->writeRead(axisNo_, command, response, 0);
|
||||
|
||||
if (rw_status != asynSuccess) {
|
||||
if (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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorStatusProblem, true);
|
||||
}
|
||||
|
||||
// Reset the driver to idle state and move out of the handshake wait loop,
|
||||
// if we're currently inside it.
|
||||
pTurboPmacA_->waitForHandshake = false;
|
||||
|
||||
return rw_status;
|
||||
return status;
|
||||
}
|
||||
|
||||
/*
|
||||
@ -1132,38 +896,25 @@ 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;
|
||||
asynStatus 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__);
|
||||
}
|
||||
getAxisParamChecked(this, encoderType, &response);
|
||||
|
||||
// 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;
|
||||
status = pC_->writeRead(axisNo_, command, response, 0);
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
|
||||
pl_status = setIntegerParam(pC_->motorMoveToHome(), 1);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_",
|
||||
axisNo_, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
setAxisParamChecked(this, motorMoveToHome, true);
|
||||
return callParamCallbacks();
|
||||
}
|
||||
|
||||
@ -1176,10 +927,7 @@ 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;
|
||||
asynStatus status = asynSuccess;
|
||||
|
||||
char command[pC_->MAXBUF_] = {0};
|
||||
char response[pC_->MAXBUF_] = {0};
|
||||
@ -1190,9 +938,9 @@ asynStatus turboPmacAxis::readEncoderType() {
|
||||
|
||||
// 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;
|
||||
status = pC_->writeRead(axisNo_, command, response, 1);
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
|
||||
int reponse_length = strlen(response);
|
||||
@ -1210,22 +958,17 @@ asynStatus turboPmacAxis::readEncoderType() {
|
||||
}
|
||||
|
||||
snprintf(command, sizeof(command), "P46");
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, 1);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
status = pC_->writeRead(axisNo_, command, response, 1);
|
||||
if (status != asynSuccess) {
|
||||
return 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);
|
||||
setAxisParamChecked(this, encoderType, IncrementalEncoder);
|
||||
} else {
|
||||
pl_status = setStringParam(pC_->encoderType(), AbsoluteEncoder);
|
||||
}
|
||||
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "encoderType_", axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
setAxisParamChecked(this, encoderType, AbsoluteEncoder);
|
||||
}
|
||||
return asynSuccess;
|
||||
}
|
||||
@ -1245,24 +988,16 @@ asynStatus turboPmacAxis::rereadEncoder() {
|
||||
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;
|
||||
asynStatus 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__);
|
||||
status = readEncoderType();
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
getAxisParamChecked(this, encoderType, &encoderType);
|
||||
|
||||
// Abort if the axis is incremental
|
||||
if (strcmp(encoderType, IncrementalEncoder) == 0) {
|
||||
@ -1276,25 +1011,16 @@ asynStatus turboPmacAxis::rereadEncoder() {
|
||||
// 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__);
|
||||
}
|
||||
getAxisParamChecked(this, motorEnableRBV, &enabled);
|
||||
|
||||
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(),
|
||||
setAxisParamChecked(
|
||||
this, 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_);
|
||||
@ -1311,15 +1037,9 @@ asynStatus turboPmacAxis::rereadEncoder() {
|
||||
// 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__);
|
||||
}
|
||||
// Turn off parameter as finished rereading, this will only be immediately
|
||||
// noticed in the read back variable though
|
||||
setAxisParamChecked(this, rereadEncoderPosition, false);
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
@ -1331,10 +1051,7 @@ asynStatus turboPmacAxis::enable(bool on) {
|
||||
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;
|
||||
asynStatus status = asynSuccess;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
@ -1361,15 +1078,8 @@ asynStatus turboPmacAxis::enable(bool on) {
|
||||
"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__);
|
||||
}
|
||||
|
||||
setAxisParamChecked(this, motorMessageText,
|
||||
"Axis cannot be disabled while it is moving.");
|
||||
return asynError;
|
||||
}
|
||||
|
||||
@ -1386,9 +1096,9 @@ asynStatus turboPmacAxis::enable(bool on) {
|
||||
|
||||
// Reread the encoder, if the axis is going to be enabled
|
||||
if (on != 0) {
|
||||
rw_status = rereadEncoder();
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
status = rereadEncoder();
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1399,9 +1109,9 @@ asynStatus turboPmacAxis::enable(bool on) {
|
||||
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
|
||||
on ? "Enable" : "Disable");
|
||||
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, 0);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
status = pC_->writeRead(axisNo_, command, response, 0);
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
|
||||
// Query the axis status every few milliseconds until the axis has been
|
||||
@ -1412,9 +1122,9 @@ asynStatus turboPmacAxis::enable(bool on) {
|
||||
|
||||
// Read the axis status
|
||||
usleep(100000);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, 1);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
status = pC_->writeRead(axisNo_, command, response, 1);
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
nvals = sscanf(response, "%d", &pTurboPmacA_->axisStatus);
|
||||
if (nvals != 1) {
|
||||
@ -1441,12 +1151,7 @@ asynStatus turboPmacAxis::enable(bool on) {
|
||||
// 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__);
|
||||
}
|
||||
setAxisParamChecked(this, motorMessageText, command);
|
||||
return asynError;
|
||||
}
|
||||
|
||||
|
||||
@ -206,7 +206,6 @@ asynStatus turboPmacController::writeRead(int axisNo, const char *command,
|
||||
|
||||
// Definition of local variables.
|
||||
asynStatus status = asynSuccess;
|
||||
asynStatus paramLibStatus = asynSuccess;
|
||||
asynStatus timeoutStatus = asynSuccess;
|
||||
// char fullCommand[MAXBUF_] = {0};
|
||||
char drvMessageText[MAXBUF_] = {0};
|
||||
@ -428,41 +427,18 @@ asynStatus turboPmacController::writeRead(int axisNo, const char *command,
|
||||
|
||||
// Log the overall status (communication successfull or not)
|
||||
if (status == asynSuccess) {
|
||||
paramLibStatus = axis->setIntegerParam(this->motorStatusCommsError_, 0);
|
||||
setAxisParamChecked(axis, motorStatusCommsError, false);
|
||||
} else {
|
||||
// Check if the axis already is in an error communication mode. If
|
||||
// it is not, upstream the error. This is done to avoid "flooding"
|
||||
// the user with different error messages if more than one error
|
||||
// ocurred before an error-free communication
|
||||
paramLibStatus =
|
||||
getIntegerParam(axisNo, motorStatusProblem_, &motorStatusProblem);
|
||||
if (paramLibStatus != asynSuccess) {
|
||||
return paramLibAccessFailed(paramLibStatus, "motorStatusProblem",
|
||||
axisNo, __PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
getAxisParamChecked(axis, motorStatusProblem, &motorStatusProblem);
|
||||
|
||||
if (motorStatusProblem == 0) {
|
||||
paramLibStatus =
|
||||
axis->setStringParam(motorMessageText(), drvMessageText);
|
||||
if (paramLibStatus != asynSuccess) {
|
||||
return paramLibAccessFailed(paramLibStatus, "motorMessageText",
|
||||
axisNo, __PRETTY_FUNCTION__,
|
||||
__LINE__);
|
||||
}
|
||||
|
||||
paramLibStatus = axis->setIntegerParam(motorStatusProblem_, 1);
|
||||
if (paramLibStatus != asynSuccess) {
|
||||
return paramLibAccessFailed(paramLibStatus,
|
||||
"motorStatusProblem", axisNo,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
paramLibStatus = axis->setIntegerParam(motorStatusProblem_, 1);
|
||||
if (paramLibStatus != asynSuccess) {
|
||||
return paramLibAccessFailed(paramLibStatus,
|
||||
"motorStatusCommsError", axisNo,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
setAxisParamChecked(axis, motorMessageText, drvMessageText);
|
||||
setAxisParamChecked(axis, motorStatusProblem, true);
|
||||
setAxisParamChecked(axis, motorStatusCommsError, true);
|
||||
}
|
||||
}
|
||||
return status;
|
||||
|
||||
Reference in New Issue
Block a user