From 80877aa6ab18582b86e491a00fa78af3ba65f803 Mon Sep 17 00:00:00 2001 From: brambilla_m Date: Fri, 27 May 2022 15:19:56 +0200 Subject: [PATCH] pmacV3 read speed and evaluate ETA --- Makefile.RHEL7.michele | 1 + sinqEPICSApp/src/pmacAxis.cpp | 113 ++++++++++++++++++++++------------ sinqEPICSApp/src/pmacAxis.h | 5 +- 3 files changed, 79 insertions(+), 40 deletions(-) diff --git a/Makefile.RHEL7.michele b/Makefile.RHEL7.michele index 7b82940..8d6a911 100644 --- a/Makefile.RHEL7.michele +++ b/Makefile.RHEL7.michele @@ -5,6 +5,7 @@ MODULE=sinq LIBVERSION=brambilla_m BUILDCLASSES=Linux EPICS_VERSIONS=3.14.12 7.0.4.1 +ARCH_FILTER=RHEL7-x86_64 # additional module dependencies REQUIRED+=SynApps diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index 3f81966..b2505b9 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -43,6 +43,11 @@ #include #include +#include +#include +#include +#include + #include "pmacController.h" #define MULT 1000. @@ -52,7 +57,7 @@ #define ABS(x) (x < 0 ? -(x) : (x)) -extern "C" void shutdownCallback(void *pPvt) { + extern "C" void shutdownCallback(void *pPvt) { pmacController *pC = static_cast(pPvt); pC->lock(); @@ -868,7 +873,24 @@ asynStatus LiftAxis::stop(double acceleration) { /*-------------------------------------------------------------------------------------------------------------*/ pmacV3Axis::pmacV3Axis(pmacController *pController, int axisNo) - : pmacAxis(pController, axisNo, false){}; + : pmacAxis(pController, axisNo, false){ + + // read max speed + char command[pC_->PMAC_MAXBUF_]; + char response[pC_->PMAC_MAXBUF_]; + + sprintf(command, "Q%2.2d04", axisNo_); + asynStatus cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response); + int nvals = sscanf(response, "%lf", &Speed); + if (cmdStatus || nvals != 1) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "drvPmacAxisGetStatus: Failed to read speed, " + "Status: %d\nCommand :%s\nResponse:%s\n", + cmdStatus, command, response); + updateMsgTxtFromDriver("Cannot read Axis speed"); + } + +}; asynStatus pmacV3Axis::poll(bool *moving) { int status = 0; @@ -906,7 +928,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { char command[pC_->PMAC_MAXBUF_]; char response[pC_->PMAC_MAXBUF_]; asynStatus cmdStatus = asynSuccess; - int done = 0, posChanging = 0; + int done = 0; double position = 0; int nvals = 0; int axisProblemFlag = 0, axStat = 0; @@ -914,7 +936,8 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { epicsUInt32 axErr = 0, highLim = 0, lowLim = 0; char message[132], *axMessage; - static const char *functionName = "pmacV3Axis::getAxisStatus"; + __attribute__((unused)) static const char *functionName = + "pmacV3Axis::getAxisStatus"; sprintf(command, "Q%2.2d10 P%2.2d00 P%2.2d23", axisNo_, axisNo_, axisNo_); cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response); @@ -928,10 +951,8 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { } pmacV3Controller *p3C_ = (pmacV3Controller *)pC_; IsEnable = axStat != -3; - printf("axis %d: position=%lf axStat=%d IsEnable=%d\n", axisNo_, position, - axStat, IsEnable); - asynStatus st = setIntegerParam(p3C_->axisEnabled_, IsEnable); + asynStatus st = setIntegerParam(p3C_->axisEnabled_, axStat > -1); cmdStatus = cmdStatus > st ? cmdStatus : st; int direction = 0; @@ -986,37 +1007,41 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { // /* // 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; - // st = setIntegerParam(pC_->motorStatusMoving_, false); - // cmdStatus = cmdStatus > st ? cmdStatus : st; - // st = setIntegerParam(pC_->motorStatusDone_, true); - // cmdStatus = cmdStatus > st ? cmdStatus : st; - // st = setIntegerParam(pC_->motorStatusProblem_, true); - // cmdStatus = cmdStatus > st ? cmdStatus : st; + int EstimatedTimeOfArrival = 60; + if (axStat == 5 || axStat == 6) { + if (status6Time == 0) { + status6Time = time(NULL); + statusPos = position; + EstimatedTimeOfArrival = + std::ceil(2 * std::fabs(setpointPosition_ - position) / Speed); + + } + else { + if (time(NULL) > status6Time + EstimatedTimeOfArrival) { + /* trigger error only when not moving */ + if (abs(position - statusPos) < .1) { + // done = 1; + // *moving = false; + // st = setIntegerParam(pC_->motorStatusMoving_, false); + // cmdStatus = cmdStatus > st ? cmdStatus : st; + // st = setIntegerParam(pC_->motorStatusDone_, true); + // cmdStatus = cmdStatus > st ? cmdStatus : st; + // st = setIntegerParam(pC_->motorStatusProblem_, true); + // cmdStatus = cmdStatus > st ? cmdStatus : st; - // 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 cmdStatus; - // } else { - // status6Time = time(NULL); - // statusPos = position; - // } - // } - // } - // } + errlogPrintf( + "Axis %d stayed in 5,6 for more than %d seconds BROKEN\n", + axisNo_, EstimatedTimeOfArrival); + updateMsgTxtFromDriver("Axis stayed in 5,6 for more than estimated time: BROKEN"); + status6Time = 0; + return cmdStatus; + } else { + status6Time = time(NULL); + statusPos = position; + } + } + } + } if (!done) { *moving = true; @@ -1041,6 +1066,18 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { return cmdStatus; } + // read max speed + sprintf(command, "Q%2.2d04", axisNo_); + cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response); + nvals = sscanf(response, "%lf", &Speed); + if (cmdStatus || nvals != 1) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "drvPmacAxisGetStatus: Failed to read speed, " + "Status: %d\nCommand :%s\nResponse:%s\n", + cmdStatus, command, response); + updateMsgTxtFromDriver("Cannot read Axis speed"); + } + sprintf(command, "P%2.2d01 P%2.2d37 P%2.2d38", axisNo_, axisNo_, axisNo_); st = pC_->lowLevelWriteRead(axisNo_, command, response); cmdStatus = cmdStatus > st ? cmdStatus : st; @@ -1116,7 +1153,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { cmdStatus = cmdStatus > st ? cmdStatus : st; return cmdStatus; -} + } asynStatus pmacV3Axis::move(double position, int relative, double min_velocity, double max_velocity, double acceleration) { diff --git a/sinqEPICSApp/src/pmacAxis.h b/sinqEPICSApp/src/pmacAxis.h index 275c1c1..a09ee6f 100644 --- a/sinqEPICSApp/src/pmacAxis.h +++ b/sinqEPICSApp/src/pmacAxis.h @@ -148,8 +148,9 @@ public: double max_velocity, double acceleration); asynStatus poll(bool *moving); protected: - int IsEnable = 0; - + int IsEnable; + double Speed; + asynStatus getAxisStatus(bool *moving); friend class pmacController;