Add PMAC V3
- limit switches: use PXX37, PXX38 insteam of MXX21, MXX22 - if the motor is running query for axis position (QXX10) and status (PXX00) - else query also error status (PXX01) and limit switches (PXX37-8) - make the requests in 1+1 transactions
This commit is contained in:
brambilla_m
@@ -23,6 +23,11 @@
|
||||
* Modified to use the MsgTxt field for SINQ
|
||||
*
|
||||
* Mark Koennecke, January 2019
|
||||
*
|
||||
* Add driver for V3 protocol
|
||||
*
|
||||
* Michele Brambilla, February 2022
|
||||
*
|
||||
********************************************/
|
||||
|
||||
#include <stdlib.h>
|
||||
@@ -363,6 +368,34 @@ static char *translateAxisError(int axErr)
|
||||
}
|
||||
}
|
||||
|
||||
asynStatus pmacV3Axis::getAxisStatus(bool *moving)
|
||||
{
|
||||
char command[pC_->PMAC_MAXBUF_];
|
||||
char response[pC_->PMAC_MAXBUF_];
|
||||
int cmdStatus = 0;
|
||||
int done = 0, posChanging = 0;
|
||||
double position = 0;
|
||||
int nvals = 0;
|
||||
int axisProblemFlag = 0;
|
||||
epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim = 0;
|
||||
char message[132], *axMessage;
|
||||
|
||||
/* read our status items one by one: our PMAC does not seem to give multiple
|
||||
* responses ..*/
|
||||
sprintf(command, "P%2.2d01", axisNo_);
|
||||
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
|
||||
nvals = sscanf(response, "%d", &axErr);
|
||||
if (cmdStatus || nvals != 1) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"drvPmacAxisGetStatus: Failed to read axis Error Status: "
|
||||
"%d\nCommand :%s\nResponse:%s\n",
|
||||
cmdStatus, command, response);
|
||||
updateMsgTxtFromDriver("Cannot read Axis Error Status");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
asynStatus pmacAxis::getAxisStatus(bool *moving)
|
||||
{
|
||||
char command[pC_->PMAC_MAXBUF_];
|
||||
@@ -847,3 +880,188 @@ asynStatus LiftAxis::stop(double acceleration)
|
||||
return pmacAxis::stop(acceleration);
|
||||
}
|
||||
/*-------------------------------------------------------------------------------------------------------------*/
|
||||
|
||||
asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
|
||||
char command[pC_->PMAC_MAXBUF_];
|
||||
char response[pC_->PMAC_MAXBUF_];
|
||||
int cmdStatus = 0;
|
||||
|
||||
int done = 0, posChanging = 0;
|
||||
double position = 0;
|
||||
int nvals = 0;
|
||||
int axisProblemFlag = 0;
|
||||
epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim = 0;
|
||||
char message[132], *axMessage;
|
||||
|
||||
sprintf(command, "Q%2.2d10 P%2.2d00", axisNo_, axisNo_);
|
||||
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
|
||||
nvals = sscanf(response, "%lf %d", &position, &axStat);
|
||||
if (cmdStatus || nvals != 2) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"drvPmacAxisGetStatus: Failed to read position and status, "
|
||||
"Status: %d\nCommand :%s\nResponse:%s\n",
|
||||
cmdStatus, command, response);
|
||||
updateMsgTxtFromDriver("Cannot read Axis position and status");
|
||||
}
|
||||
|
||||
setDoubleParam(pC_->motorPosition_, position * MULT);
|
||||
setDoubleParam(pC_->motorEncoderPosition_, position * MULT);
|
||||
|
||||
/* Use previous position and current position to calculate direction.*/
|
||||
int direction = previous_direction_;
|
||||
if ((position - previous_position_) > 0) {
|
||||
direction = 1;
|
||||
}
|
||||
if ((position - previous_position_) < 0) {
|
||||
direction = 0;
|
||||
}
|
||||
setIntegerParam(pC_->motorStatusDirection_, direction);
|
||||
|
||||
/*Store position to calculate direction for next poll.*/
|
||||
previous_position_ = position;
|
||||
previous_direction_ = direction;
|
||||
|
||||
// errlogPrintf("Polling, axStat = %d, position = %f\n", axStat, position);
|
||||
|
||||
/* are we done? */
|
||||
if ((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0) {
|
||||
done = 1;
|
||||
} else {
|
||||
starting = 0;
|
||||
done = 0;
|
||||
}
|
||||
|
||||
if (starting && time(NULL) > startTime + 10) {
|
||||
/*
|
||||
did not start in 10 seconds: messed up: cause an alarm
|
||||
*/
|
||||
done = 1;
|
||||
*moving = false;
|
||||
setIntegerParam(pC_->motorStatusMoving_, false);
|
||||
setIntegerParam(pC_->motorStatusDone_, true);
|
||||
setIntegerParam(pC_->motorStatusProblem_, true);
|
||||
errlogPrintf("Axis %d did not start within 10 seconds!! BROKEN\n", axisNo_);
|
||||
updateMsgTxtFromDriver("Axis did not start within 10 seconds");
|
||||
starting = 0;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
/*
|
||||
code to test against too long status 5 or 6
|
||||
*/
|
||||
if (axStat == 5 || axStat == 6) {
|
||||
if (status6Time == 0) {
|
||||
status6Time = time(NULL);
|
||||
statusPos = position;
|
||||
} else {
|
||||
if (time(NULL) > status6Time + 60) {
|
||||
/* trigger error only when not moving */
|
||||
if (abs(position - statusPos) < .1) {
|
||||
done = 1;
|
||||
*moving = false;
|
||||
setIntegerParam(pC_->motorStatusMoving_, false);
|
||||
setIntegerParam(pC_->motorStatusDone_, true);
|
||||
setIntegerParam(pC_->motorStatusProblem_, true);
|
||||
errlogPrintf(
|
||||
"Axis %d stayed in 5,6 for more then 60 seconds BROKEN\n",
|
||||
axisNo_);
|
||||
updateMsgTxtFromDriver(
|
||||
"Axis stayed in 5,6 for more then 60 seconds: BROKEN");
|
||||
status6Time = 0;
|
||||
return asynSuccess;
|
||||
} else {
|
||||
status6Time = time(NULL);
|
||||
statusPos = position;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!done) {
|
||||
*moving = true;
|
||||
setIntegerParam(pC_->motorStatusMoving_, true);
|
||||
setIntegerParam(pC_->motorStatusDone_, false);
|
||||
} else {
|
||||
*moving = false;
|
||||
setIntegerParam(pC_->motorStatusMoving_, false);
|
||||
setIntegerParam(pC_->motorStatusDone_, true);
|
||||
if (homing) {
|
||||
setIntegerParam(pC_->motorStatusHomed_, done);
|
||||
homing = 0;
|
||||
}
|
||||
}
|
||||
|
||||
sprintf(message, "poll results: axis %d, status %d, done = %d",
|
||||
axisNo_, axStat, done);
|
||||
pC_->debugFlow(message);
|
||||
|
||||
if (*moving == false) {
|
||||
|
||||
sprintf(command, "P%2.2d01 P%2.2d37 P%2.2d38", axisNo_, axisNo_, axisNo_);
|
||||
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
|
||||
nvals = sscanf(response, "%d %d %d", &axErr, &highLim, &lowLim);
|
||||
if (cmdStatus || nvals != 1) {
|
||||
asynPrint(
|
||||
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"drvPmacAxisGetStatus: Failed to read axis Error, high and low limit "
|
||||
"flags Status: %d\nCommand :%s\nResponse:%s\n",
|
||||
cmdStatus, command, response);
|
||||
updateMsgTxtFromDriver(
|
||||
"Cannot read Axis Error, high and low limit flag Status");
|
||||
}
|
||||
|
||||
// errlogPrintf("Polling, axErr = %d, highLim = %d, lowLim = %d\n", axStat,
|
||||
// axErr, highLim, lowLim);
|
||||
|
||||
sprintf(message, "poll results: axis %d, axErr %d", axisNo_, axErr);
|
||||
pC_->debugFlow(message);
|
||||
|
||||
/* search for trouble */
|
||||
if (highLim) {
|
||||
setIntegerParam(pC_->motorStatusHighLimit_, true);
|
||||
updateMsgTxtFromDriver("High Limit Hit");
|
||||
} else {
|
||||
setIntegerParam(pC_->motorStatusHighLimit_, false);
|
||||
}
|
||||
if (lowLim) {
|
||||
setIntegerParam(pC_->motorStatusLowLimit_, true);
|
||||
updateMsgTxtFromDriver("Low Limit Hit");
|
||||
} else {
|
||||
setIntegerParam(pC_->motorStatusLowLimit_, false);
|
||||
}
|
||||
if (axErr == 11) {
|
||||
setIntegerParam(pC_->motorStatusFollowingError_, true);
|
||||
updateMsgTxtFromDriver("Following error triggered");
|
||||
} else {
|
||||
setIntegerParam(pC_->motorStatusFollowingError_, false);
|
||||
}
|
||||
/* Set any axis specific general problem bits. */
|
||||
}
|
||||
|
||||
if (axStat < 0 || axErr != 0) {
|
||||
axisProblemFlag = 1;
|
||||
if (axisErrorCount < 10) {
|
||||
axMessage = translateAxisError(axErr);
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"drvPmacAxisGetStatus: Axis %d is in deep trouble: axis error "
|
||||
"code %d, translated: %s:, status code = %d\n",
|
||||
axisNo_, axErr, axMessage, axStat);
|
||||
snprintf(message, sizeof(message), "PMAC Axis error: %s", axMessage);
|
||||
updateMsgTxtFromDriver(message);
|
||||
if (axMessage != NULL) {
|
||||
free(axMessage);
|
||||
}
|
||||
axisErrorCount++;
|
||||
} else if (axisErrorCount == 10) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"Suppressing further axis error messages\n");
|
||||
axisErrorCount++;
|
||||
}
|
||||
} else {
|
||||
axisProblemFlag = 0;
|
||||
axisErrorCount = 0;
|
||||
}
|
||||
setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag);
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
Reference in New Issue
Block a user