From f5da0d54bd506e4d88c15e0440243ab0a2aaa6b2 Mon Sep 17 00:00:00 2001 From: brambilla_m Date: Tue, 22 Mar 2022 11:01:06 +0100 Subject: [PATCH] Reduce polling, se MsgTxt if move when disabled --- sinqEPICSApp/src/pmacAxis.cpp | 339 ++++++++-------------------- sinqEPICSApp/src/pmacAxis.h | 5 +- sinqEPICSApp/src/pmacController.cpp | 15 +- 3 files changed, 100 insertions(+), 259 deletions(-) diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index 5931807..b2ba52e 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -43,13 +43,7 @@ #include #include -#include - #include "pmacController.h" -template -const char *getClassName(T) { - return typeid(T).name(); -} #define MULT 1000. @@ -907,212 +901,16 @@ asynStatus pmacV3Axis::poll(bool *moving) { return status ? asynError : asynSuccess; } -// 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, highLim = 0, lowLim = 0, axDone = 0; -// int axStat=0; -// char message[132], *axMessage; - -// static const char *functionName = "pmacV3Axis::getAxisStatus"; - -// pmacV3Controller* p3C_ = (pmacV3Controller*)pC_; -// sprintf(command, "Q%2.2d10 P%2.2d00 P%2.2d23", axisNo_, axisNo_, axisNo_); -// cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response); -// nvals = sscanf(response, "%lf %d %d", &position, &axStat, &axDone); -// if (cmdStatus || nvals != 3) { -// 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"); -// } - -// int st = setIntegerParam(p3C_->axisEnabled_, axStat >= 0); -// // st = callParamCallbacks(); -// // printf("axStat: callParamCallbacks -> ok: %d\n", st == asynSuccess); - -// 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 ( axDone == 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; -// callParamCallbacks(); -// 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; -// callParamCallbacks(); -// 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, axDone); -// 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 && axStat != -3) || axErr != 0) { -// axisProblemFlag = 1; -// if (axisErrorCount < 10) { -// axMessage = translateAxisError(axErr); -// printf("axErr: %d\taxStat: %d\taxMessage: %s\n", axErr, axStat, -// axMessage); 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); - -// callParamCallbacks(); -// return asynSuccess; -// } asynStatus pmacV3Axis::getAxisStatus(bool *moving) { char command[pC_->PMAC_MAXBUF_]; char response[pC_->PMAC_MAXBUF_]; - int cmdStatus = 0; + asynStatus cmdStatus = asynSuccess; int done = 0, posChanging = 0; double position = 0; int nvals = 0; - int axisProblemFlag = 0, axStat = 0, axDone; + int axisProblemFlag = 0, axStat = 0; + int acknowledge = 0; epicsUInt32 axErr = 0, highLim = 0, lowLim = 0; char message[132], *axMessage; @@ -1120,7 +918,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { sprintf(command, "Q%2.2d10 P%2.2d00 P%2.2d23", axisNo_, axisNo_, axisNo_); cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response); - nvals = sscanf(response, "%lf %d %d", &position, &axStat, &axDone); + nvals = sscanf(response, "%lf %d %d", &position, &axStat, &acknowledge); if (cmdStatus || nvals != 3) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "drvPmacAxisGetStatus: Failed to read position and status, " @@ -1130,25 +928,22 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { } pmacV3Controller *p3C_ = (pmacV3Controller *)pC_; - int st = setIntegerParam(p3C_->axisEnabled_, axStat >= 0); - - 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 != 3) { - 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"); - } + IsEnable = axStat != -3; + asynStatus st = setIntegerParam(p3C_->axisEnabled_, axStat >= 0); + cmdStatus = cmdStatus > st ? cmdStatus : st; int direction = 0; - setDoubleParam(pC_->motorPosition_, position * MULT); - setDoubleParam(pC_->motorEncoderPosition_, position * MULT); + st = setDoubleParam(pC_->motorPosition_, position * MULT); + cmdStatus = cmdStatus > st ? cmdStatus : st; + + st = setDoubleParam(pC_->motorEncoderPosition_, position * MULT); + cmdStatus = cmdStatus > st ? cmdStatus : st; + + if(axisNo_ == 2) { + printf("position(%d): %lf\n", axisNo_, position); + printf("ack(%d): %d\n", axisNo_, acknowledge); + } /* Use previous position and current position to calculate direction.*/ if ((position - previous_position_) > 0) { @@ -1158,15 +953,13 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { } else { direction = 0; } - setIntegerParam(pC_->motorStatusDirection_, direction); + st = setIntegerParam(pC_->motorStatusDirection_, direction); + cmdStatus = cmdStatus > st ? cmdStatus : st; /*Store position to calculate direction for next poll.*/ previous_position_ = position; previous_direction_ = direction; - // errlogPrintf("Polling, axStat = %d, axErr = %d, position = %f\n", axStat, - // axErr, position); - /* are we done? */ if ((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0) { done = 1; @@ -1181,13 +974,16 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { */ done = 1; *moving = false; - setIntegerParam(pC_->motorStatusMoving_, false); - setIntegerParam(pC_->motorStatusDone_, true); - setIntegerParam(pC_->motorStatusProblem_, true); + 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 did not start within 10 seconds!! BROKEN\n", axisNo_); updateMsgTxtFromDriver("Axis did not start within 10 seconds"); starting = 0; - return asynSuccess; + return st; } /* @@ -1203,16 +999,20 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { if (abs(position - statusPos) < .1) { done = 1; *moving = false; - setIntegerParam(pC_->motorStatusMoving_, false); - setIntegerParam(pC_->motorStatusDone_, true); - setIntegerParam(pC_->motorStatusProblem_, true); + 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 asynSuccess; + return cmdStatus; } else { status6Time = time(NULL); statusPos = position; @@ -1223,40 +1023,73 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { if (!done) { *moving = true; - setIntegerParam(pC_->motorStatusMoving_, true); - setIntegerParam(pC_->motorStatusDone_, false); + st = setIntegerParam(pC_->motorStatusMoving_, true); + cmdStatus = cmdStatus > st ? cmdStatus : st; + st = setIntegerParam(pC_->motorStatusDone_, false); + cmdStatus = cmdStatus > st ? cmdStatus : st; } else { *moving = false; - setIntegerParam(pC_->motorStatusMoving_, false); - setIntegerParam(pC_->motorStatusDone_, true); + st = setIntegerParam(pC_->motorStatusMoving_, false); + cmdStatus = cmdStatus > st ? cmdStatus : st; + st = setIntegerParam(pC_->motorStatusDone_, true); + cmdStatus = cmdStatus > st ? cmdStatus : st; if (homing) { - setIntegerParam(pC_->motorStatusHomed_, done); + st = setIntegerParam(pC_->motorStatusHomed_, done); + cmdStatus = cmdStatus > st ? cmdStatus : st; homing = 0; } } + if (*moving) { + return cmdStatus; + } + + 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; + + nvals = sscanf(response, "%d %d %d", &axErr, &highLim, &lowLim); + if (st || nvals != 3) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "drvPmacAxisGetStatus: Failed to read axis Error, high and low limit " + "flags Status: %d\nCommand :%s\nResponse:%s\n", + st, command, response); + updateMsgTxtFromDriver( + "Cannot read Axis Error, high and low limit flag Status"); + } + + // errlogPrintf("Polling, axStat = %d, axErr = %d, position = %f\n", axStat, + // axErr, position); + sprintf(message, "poll results: axis %d, status %d, axErr %d, done = %d", axisNo_, axStat, axErr, done); pC_->debugFlow(message); /* search for trouble */ if (highLim) { - setIntegerParam(pC_->motorStatusHighLimit_, true); + st = setIntegerParam(pC_->motorStatusHighLimit_, true); + cmdStatus = cmdStatus > st ? cmdStatus : st; updateMsgTxtFromDriver("High Limit Hit"); } else { - setIntegerParam(pC_->motorStatusHighLimit_, false); + st = setIntegerParam(pC_->motorStatusHighLimit_, false); + cmdStatus = cmdStatus > st ? cmdStatus : st; } if (lowLim) { - setIntegerParam(pC_->motorStatusLowLimit_, true); + st = setIntegerParam(pC_->motorStatusLowLimit_, true); + cmdStatus = cmdStatus > st ? cmdStatus : st; updateMsgTxtFromDriver("Low Limit Hit"); } else { - setIntegerParam(pC_->motorStatusLowLimit_, false); + st = setIntegerParam(pC_->motorStatusLowLimit_, false); + cmdStatus = cmdStatus > st ? cmdStatus : st; } if (axErr == 11) { - setIntegerParam(pC_->motorStatusFollowingError_, true); + st = setIntegerParam(pC_->motorStatusFollowingError_, true); + cmdStatus = cmdStatus > st ? cmdStatus : st; updateMsgTxtFromDriver("Following error triggered"); } else { - setIntegerParam(pC_->motorStatusFollowingError_, false); + st = setIntegerParam(pC_->motorStatusFollowingError_, false); + cmdStatus = cmdStatus > st ? cmdStatus : st; } /* Set any axis specific general problem bits. */ if (axErr != 0) { @@ -1282,7 +1115,19 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { axisProblemFlag = 0; axisErrorCount = 0; } - setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag); + st = setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag); + cmdStatus = cmdStatus > st ? cmdStatus : st; - return asynSuccess; + return cmdStatus; } + +asynStatus pmacV3Axis::move(double position, int relative, double min_velocity, + double max_velocity, double acceleration) { + + if(!IsEnable) { + updateMsgTxtFromDriver("Error: axis disabled"); + return asynSuccess; + } + updateMsgTxtFromDriver(""); + return pmacAxis::move(position, relative, min_velocity, max_velocity, acceleration); +} \ No newline at end of file diff --git a/sinqEPICSApp/src/pmacAxis.h b/sinqEPICSApp/src/pmacAxis.h index 90b530f..275c1c1 100644 --- a/sinqEPICSApp/src/pmacAxis.h +++ b/sinqEPICSApp/src/pmacAxis.h @@ -144,9 +144,12 @@ public: pmacV3Axis(pmacController *pController, int axisNo); + asynStatus move(double position, int relative, double min_velocity, + double max_velocity, double acceleration); asynStatus poll(bool *moving); protected: - + int IsEnable = 0; + asynStatus getAxisStatus(bool *moving); friend class pmacController; diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index 514db21..0b8f0c6 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -364,11 +364,8 @@ asynStatus pmacController::writeFloat64(asynUser *pasynUser, epicsFloat64 value) /* Set the parameter and readback in the parameter library. */ status = pAxis->setDoubleParam(function, value); - printf("%s: pAxis->setDoubleParam(%d, %lf)\n", functionName, function, value); - if (function == motorPosition_) { - printf("%s: pAxis->setDoubleParam(%d, %lf)\n", functionName, function, value); - } + // if (function == motorPosition_) { // /*Set position on motor axis.*/ // epicsInt32 position = (epicsInt32) floor(value*32/pAxis->scale_ + 0.5); @@ -841,19 +838,16 @@ asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, "%s: Enable axis on controller %s, axis %d enable=%d\n", functionName, portName, pAxis->axisNo_, value); - //errlogPrintf("Enable axis %d: %d, command = %s\n", pAxis->axisNo_, value, command); + if(value){ + pAxis->updateMsgTxtFromDriver(""); + } } // Execute the command. if (command[0] != 0 && status == asynSuccess) { status = lowLevelWriteRead(pAxis->axisNo_, command, response); - printf("%s: lowLevelWriteRead: success=%d\n (function == enableAxis_? %d)\n",functionName, status==asynSuccess, function == enableAxis_); } - // Call base class method - // This will handle callCallbacks even if the function was handled here. - // status = asynMotorController::writeInt32(pasynUser, value); - return pmacController::writeInt32(pasynUser, value); } @@ -878,7 +872,6 @@ asynStatus pmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) { *value = strtol(response, NULL, 10); int st = setIntegerParam(axisEnabled_, *value); - // printf("%s: setIntegerParam(%d, %d) -> success=%d\n", functionName,axisEnabled_, *value, st==asynSuccess); callParamCallbacks(); } return pmacController::readInt32(pasynUser, value);