From 929f9f600ddb5aeea9ed4c79a4226c09bb4f9235 Mon Sep 17 00:00:00 2001 From: brambilla_m Date: Fri, 25 Feb 2022 14:56:21 +0100 Subject: [PATCH] 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 --- sinqEPICSApp/src/pmacAxis.cpp | 218 ++++++++++++++++++++++++++++ sinqEPICSApp/src/pmacAxis.h | 12 ++ sinqEPICSApp/src/pmacController.cpp | 12 ++ sinqEPICSApp/src/pmacController.h | 2 +- 4 files changed, 243 insertions(+), 1 deletion(-) diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index 6e3d611..4affc4a 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -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 @@ -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; +} \ No newline at end of file diff --git a/sinqEPICSApp/src/pmacAxis.h b/sinqEPICSApp/src/pmacAxis.h index 3577581..eb9ebe6 100644 --- a/sinqEPICSApp/src/pmacAxis.h +++ b/sinqEPICSApp/src/pmacAxis.h @@ -131,4 +131,16 @@ class LiftAxis : public pmacAxis int waitStart; }; + /******************************************** + * Protocol version 3 requires just some minor change + * + * Michele Brambilla, February 2022 + ********************************************/ + +class pmacV3Axis : public pmacAxis { +public: + using pmacAxis::pmacAxis; + friend class pmacController; + }; + #endif /* pmacAxis_H */ diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index 34abe89..ead47fb 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -843,6 +843,18 @@ static void configLiftAxisCallFunc(const iocshArgBuf *args) LiftCreateAxis(args[0].sval, args[1].ival); } +/* pmacV3CreateAxis */ +static const iocshArg pmacV3CreateAxisArg0 = {"Controller port name", + iocshArgString}; +static const iocshArg pmacV3CreateAxisArg1 = {"Axis number", iocshArgInt}; +static const iocshArg *const pmacV3CreateAxisArgs[] = {&pmacV3CreateAxisArg0, + &pmacV3CreateAxisArg1}; +static const iocshFuncDef configpmacV3Axis = {"pmacV3CreateAxis", 2, + pmacV3CreateAxisArgs}; + +static void configpmacV3AxisCallFunc(const iocshArgBuf *args) { + pmacV3CreateAxis(args[0].sval, args[1].ival); +} /* pmacCreateAxes */ static const iocshArg pmacCreateAxesArg0 = {"Controller port name", iocshArgString}; diff --git a/sinqEPICSApp/src/pmacController.h b/sinqEPICSApp/src/pmacController.h index 2f67fcf..0898577 100644 --- a/sinqEPICSApp/src/pmacController.h +++ b/sinqEPICSApp/src/pmacController.h @@ -139,6 +139,7 @@ class pmacController : public SINQController { friend class pmacHRPTAxis; friend class SeleneAxis; friend class LiftAxis; + friend class pmacV3Axis; }; #define NUM_PMAC_PARAMS (&LAST_PMAC_PARAM - &FIRST_PMAC_PARAM + 1) @@ -163,5 +164,4 @@ class SeleneController : public pmacController { }; - #endif /* pmacController_H */