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

113
sinqEPICSApp/src/pmacAxis.cpp
View File

@ -43,6 +43,11 @@
#include <string.h>
#include <time.h>
#include<algorithm>
#include<cmath>
#include<utility>
#include <memory>
#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<pmacController *>(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) {

5
sinqEPICSApp/src/pmacAxis.h
View File

@ -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;