lin-epics-modules/sinqepicsapp
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

pmacV3 read speed and evaluate ETA

Browse Source
This commit is contained in:
brambilla_m
2022-05-27 15:19:56 +02:00
parent afc92bde3f
commit 80877aa6ab
3 changed files with 79 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
Makefile.RHEL7.michele
View File

@ -5,6 +5,7 @@ MODULE=sinq
LIBVERSION=brambilla_m LIBVERSION=brambilla_m
BUILDCLASSES=Linux BUILDCLASSES=Linux
EPICS_VERSIONS=3.14.12 7.0.4.1 EPICS_VERSIONS=3.14.12 7.0.4.1
ARCH_FILTER=RHEL7-x86_64
# additional module dependencies # additional module dependencies
REQUIRED+=SynApps REQUIRED+=SynApps

113
sinqEPICSApp/src/pmacAxis.cpp
View File

@ -43,6 +43,11 @@
#include <string.h> #include <string.h>
#include <time.h> #include <time.h>
#include<algorithm>
#include<cmath>
#include<utility>
#include <memory>
#include "pmacController.h" #include "pmacController.h"
#define MULT 1000. #define MULT 1000.
@ -52,7 +57,7 @@
#define ABS(x) (x < 0 ? -(x) : (x)) #define ABS(x) (x < 0 ? -(x) : (x))
extern "C" void shutdownCallback(void *pPvt) { extern "C" void shutdownCallback(void *pPvt) {
pmacController *pC = static_cast<pmacController *>(pPvt); pmacController *pC = static_cast<pmacController *>(pPvt);
pC->lock(); pC->lock();
@ -868,7 +873,24 @@ asynStatus LiftAxis::stop(double acceleration) {
/*-------------------------------------------------------------------------------------------------------------*/ /*-------------------------------------------------------------------------------------------------------------*/
pmacV3Axis::pmacV3Axis(pmacController *pController, int axisNo) 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) { asynStatus pmacV3Axis::poll(bool *moving) {
int status = 0; int status = 0;
@ -906,7 +928,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
char command[pC_->PMAC_MAXBUF_]; char command[pC_->PMAC_MAXBUF_];
char response[pC_->PMAC_MAXBUF_]; char response[pC_->PMAC_MAXBUF_];
asynStatus cmdStatus = asynSuccess; asynStatus cmdStatus = asynSuccess;
int done = 0, posChanging = 0; int done = 0;
double position = 0; double position = 0;
int nvals = 0; int nvals = 0;
int axisProblemFlag = 0, axStat = 0; int axisProblemFlag = 0, axStat = 0;
@ -914,7 +936,8 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
epicsUInt32 axErr = 0, highLim = 0, lowLim = 0; epicsUInt32 axErr = 0, highLim = 0, lowLim = 0;
char message[132], *axMessage; 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_); sprintf(command, "Q%2.2d10 P%2.2d00 P%2.2d23", axisNo_, axisNo_, axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response); cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
@ -928,10 +951,8 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
} }
pmacV3Controller *p3C_ = (pmacV3Controller *)pC_; pmacV3Controller *p3C_ = (pmacV3Controller *)pC_;
IsEnable = axStat != -3; 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; cmdStatus = cmdStatus > st ? cmdStatus : st;
int direction = 0; int direction = 0;
@ -986,37 +1007,41 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
// /* // /*
// code to test against too long status 5 or 6 // code to test against too long status 5 or 6
// */ // */
// if (axStat == 5 || axStat == 6) { int EstimatedTimeOfArrival = 60;
// if (status6Time == 0) { if (axStat == 5 || axStat == 6) {
// status6Time = time(NULL); if (status6Time == 0) {
// statusPos = position; status6Time = time(NULL);
// } else { statusPos = position;
// if (time(NULL) > status6Time + 60) { EstimatedTimeOfArrival =
// /* trigger error only when not moving */ std::ceil(2 * std::fabs(setpointPosition_ - position) / Speed);
// if (abs(position - statusPos) < .1) {
// done = 1; }
// *moving = false; else {
// st = setIntegerParam(pC_->motorStatusMoving_, false); if (time(NULL) > status6Time + EstimatedTimeOfArrival) {
// cmdStatus = cmdStatus > st ? cmdStatus : st; /* trigger error only when not moving */
// st = setIntegerParam(pC_->motorStatusDone_, true); if (abs(position - statusPos) < .1) {
// cmdStatus = cmdStatus > st ? cmdStatus : st; // done = 1;
// st = setIntegerParam(pC_->motorStatusProblem_, true); // *moving = false;
// cmdStatus = cmdStatus > st ? cmdStatus : st; // 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( errlogPrintf(
// "Axis %d stayed in 5,6 for more then 60 seconds BROKEN\n", "Axis %d stayed in 5,6 for more than %d seconds BROKEN\n",
// axisNo_); axisNo_, EstimatedTimeOfArrival);
// updateMsgTxtFromDriver( updateMsgTxtFromDriver("Axis stayed in 5,6 for more than estimated time: BROKEN");
// "Axis stayed in 5,6 for more then 60 seconds: BROKEN"); status6Time = 0;
// status6Time = 0; return cmdStatus;
// return cmdStatus; } else {
// } else { status6Time = time(NULL);
// status6Time = time(NULL); statusPos = position;
// statusPos = position; }
// } }
// } }
// } }
// }
if (!done) { if (!done) {
*moving = true; *moving = true;
@ -1041,6 +1066,18 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
return cmdStatus; 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_); sprintf(command, "P%2.2d01 P%2.2d37 P%2.2d38", axisNo_, axisNo_, axisNo_);
st = pC_->lowLevelWriteRead(axisNo_, command, response); st = pC_->lowLevelWriteRead(axisNo_, command, response);
cmdStatus = cmdStatus > st ? cmdStatus : st; cmdStatus = cmdStatus > st ? cmdStatus : st;
@ -1116,7 +1153,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
cmdStatus = cmdStatus > st ? cmdStatus : st; cmdStatus = cmdStatus > st ? cmdStatus : st;
return cmdStatus; return cmdStatus;
} }
asynStatus pmacV3Axis::move(double position, int relative, double min_velocity, asynStatus pmacV3Axis::move(double position, int relative, double min_velocity,
double max_velocity, double acceleration) { double max_velocity, double acceleration) {

5
sinqEPICSApp/src/pmacAxis.h
View File

@ -148,8 +148,9 @@ public:
double max_velocity, double acceleration); double max_velocity, double acceleration);
asynStatus poll(bool *moving); asynStatus poll(bool *moving);
protected: protected:
int IsEnable = 0; int IsEnable;
double Speed;
asynStatus getAxisStatus(bool *moving); asynStatus getAxisStatus(bool *moving);
friend class pmacController; friend class pmacController;