From f1a17bc2950fedbec90eeebc63885688e92a1303 Mon Sep 17 00:00:00 2001 From: koennecke Date: Fri, 7 Jul 2023 13:54:21 +0200 Subject: [PATCH] Added AMOR detector tower special pmac motors: working now --- sinqEPICSApp/src/NanotecDriver.cpp | 14 +- sinqEPICSApp/src/pmacAxis.cpp | 313 +++++++++++++++++++++++++++- sinqEPICSApp/src/pmacAxis.h | 31 +++ sinqEPICSApp/src/pmacController.cpp | 40 ++++ sinqEPICSApp/src/pmacController.h | 1 + 5 files changed, 396 insertions(+), 3 deletions(-) diff --git a/sinqEPICSApp/src/NanotecDriver.cpp b/sinqEPICSApp/src/NanotecDriver.cpp index 395325d..e564be3 100644 --- a/sinqEPICSApp/src/NanotecDriver.cpp +++ b/sinqEPICSApp/src/NanotecDriver.cpp @@ -394,7 +394,12 @@ asynStatus NanotecAxis::poll(bool *moving) pPtr = strchr(reply,'C'); pPtr++; - posVal = atoi(pPtr); + if(pPtr){ + posVal = atoi(pPtr); + } else { + errlogPrintf("Invalid response %s for #C received for axis %d\n", reply, axisNo_); + return asynError; + } //errlogPrintf("Axis %d, reply %s, position %d\n", axisNo_, reply, posVal); setDoubleParam(pC_->motorPosition_, (double)posVal); @@ -408,7 +413,12 @@ asynStatus NanotecAxis::poll(bool *moving) pPtr = strchr(reply,'$'); pPtr++; - statVal = atoi(pPtr); + if(pPtr) { + statVal = atoi(pPtr); + } else { + errlogPrintf("Invalid response %s for #$ received for axis %d\n", reply, axisNo_); + return asynError; + } //errlogPrintf("Axis %d, reply %s, statVal = %d\n", // axisNo_, reply, statVal); diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index c69e8a4..3b19af2 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -1,4 +1,4 @@ -/******************************************** +/******************************************* * pmacAxis.cpp * * PMAC Asyn motor based on the @@ -28,6 +28,10 @@ * * Michele Brambilla, February 2022 * + * Added driver for special AMOR detector axes + * + * Mark Koennecke, June 2023 + * ********************************************/ #include @@ -42,6 +46,7 @@ #include #include #include +#include #include #include @@ -1181,3 +1186,309 @@ asynStatus pmacV3Axis::move(double position, int relative, double min_velocity, updateMsgTxtFromDriver(""); return pmacAxis::move(position, relative, min_velocity, max_velocity, acceleration); } + +/*========================= Amor Detector Motor code ==============*/ +AmorDetectorAxis::AmorDetectorAxis(pmacController *pC, int axisNo, int function) + : pmacAxis(pC, axisNo, false) { + static const char *functionName = "pmacAxis::pmacAxis"; + + pC_->debugFlow(functionName); + + _function = function; + + callParamCallbacks(); + + /* Wake up the poller task which will make it do a poll, + * updating values for this axis to use the new resolution (stepSize_) */ + pC_->wakeupPoller(); +} +/*-----------------------------------------------------------------*/ +asynStatus AmorDetectorAxis::moveVelocity(double min_velocity, + double max_velocity, + double acceleration) +{ + updateMsgTxtFromDriver("Function moveVelocity not allowed"); + return asynError; +} + +/*------------------------------------------------------------------*/ +asynStatus AmorDetectorAxis::home(double min_velocity, + double max_velocity, + double acceleration, + int forwards) +{ + updateMsgTxtFromDriver("Cannot home AMOR detector motors"); + return asynError; +} +/*-------------------------------------------------------------------*/ +asynStatus AmorDetectorAxis::setPosition(double position) +{ + updateMsgTxtFromDriver("Cannot setPosition() AMOR detector motors"); + return asynError; +} +/*-------------------------------------------------------------------*/ +asynStatus AmorDetectorAxis::move(double position, int relative, + double min_velocity, double max_velocity, + double acceleration) +{ + asynStatus status = asynError; + static const char *functionName = "AmorDetectorAxis::move"; + double realPosition; + int parkStatus; + time_t errorWait; + + updateMsgTxtFromDriver(""); + + pC_->debugFlow(functionName); + + char command[128] = {0}; + char response[32] = {0}; + + pC_->debugFlow(functionName); + + /* test if we are not in the change position */ + sprintf(command,"P358"); + pC_->lowLevelWriteRead(axisNo_, command, response); + sscanf(response, "%d", &parkStatus); + if(_function != ADPARK && parkStatus == 2) { + updateMsgTxtFromDriver("Cannot run motor when in park position"); + errlogPrintf("Cannot run motor when in park position"); + return asynError; + } + + if (relative) { + realPosition = previous_position_ + position / MULT; + } else { + realPosition = position / MULT; + } + + /* set target */ + errlogPrintf("function %d, sending position %f\n", _function, realPosition); + + switch(_function){ + case ADCOM: + sprintf(command, "Q451=%f", realPosition); + break; + case ADCOZ: + sprintf(command, "Q454=%f", realPosition); + break; + case ADPARK: + // test if a reset is required first + sprintf(command, "P359"); + pC_->lowLevelWriteRead(axisNo_, command, response); + sscanf(response, "%d", &parkStatus); + errlogPrintf("park status %d\n", parkStatus); + if(parkStatus != 0){ + sprintf(command,"P352=2"); + pC_->lowLevelWriteRead(axisNo_, command, response); + errlogPrintf("Park status %d, error reset: %s\n", parkStatus, command); + } + errorWait = time(NULL); + while(true){ + usleep(5000); + sprintf(command, "P359"); + pC_->lowLevelWriteRead(axisNo_, command, response); + sscanf(response, "%d", &parkStatus); + errlogPrintf("P359 = %d\n", parkStatus); + if(parkStatus == 0){ + break; + } + if(time(NULL) > errorWait + 3){ + errlogPrintf("Failed to clear error status after 3 seconds\n"); + updateMsgTxtFromDriver("Failed to clear errorState in 3 seconds"); + return asynError; + } + } + // now drive to the real place + if(realPosition == 0) { + sprintf(command, "P350=3"); + } else if(realPosition < -90){ + sprintf(command, "P350=2"); + } else { + updateMsgTxtFromDriver("Can only reach -100, 0, nothing else"); + return asynError; + } + errlogPrintf("Park position %f, park command: %s\n", realPosition, command); + + } + status = pC_->lowLevelWriteRead(axisNo_, command, response); + if(status != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "AmorDetectorAxis::move failed to send command %s", command); + updateMsgTxtFromDriver("Cannot send Amor Detector Command"); + return status; + } + + /* send move command */ + if(_function != ADPARK) { + sprintf(command, "P350=1"); + status = pC_->lowLevelWriteRead(axisNo_, command, response); + if(status != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "AmorDetectorAxis::move failed to send command %s", command); + updateMsgTxtFromDriver("Cannot send Amor Detector Command"); + } + } + + det_starting = true; + det_startTime = time(NULL); + + next_poll = BUSYPOLL; + + return status; +} +/*-----------------------------------------------------------------------------*/ +asynStatus AmorDetectorAxis::stop(double acceleration) { + asynStatus status = asynError; + static const char *functionName = "AmorDetectorAxis::stopAxis"; + bool moving = false; + + pC_->debugFlow(functionName); + + char command[128] = {0}; + char response[32] = {0}; + + this->poll(&moving); + det_starting = false; + + if(moving) { + // only send a stop when actually moving + sprintf(command, "P350=8"); + status = pC_->lowLevelWriteRead(axisNo_, command, response); + } + return status; +} +/*-------------------------------------------------------------------------------*/ +asynStatus AmorDetectorAxis::poll(bool *moving) +{ + asynStatus status = asynError; + static const char *functionName = "AmorDetectorAxis::poll"; + int targetReached, errorCode, parkCode; + double position; + + pC_->debugFlow(functionName); + + char command[128] = {0}; + char response[32] = {0}; + char errorMessage[128] = {0}; + + /* read in position flag*/ + sprintf(command, "P354"); + status = pC_->lowLevelWriteRead(axisNo_, command, response); + if(status != asynSuccess){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "AmorDetectorAxis::poll failed to read in position"); + updateMsgTxtFromDriver("Failed to read Amor Detector in position"); + return status; + } + sscanf(response, "%d", &targetReached); + + //if(_function == ADCOM){ + // errlogPrintf("startTime: %ld, current time: %ld, starting flag %d, targetReached %d\n", det_startTime, time(NULL), + // det_starting, targetReached); + //} + + // The thing is sometimes slow to start, allow 7 seconds + // Especially coz is really slow to indicate + if(det_starting && time(NULL) > (det_startTime + 7)) { + det_starting = false; + //if(_function == ADCOM){ + // errlogPrintf("Resetting starting flag after timeout\n"); + //} + } + + setIntegerParam(pC_->motorStatusProblem_, false); + if(targetReached == 0 && det_starting == false){ + *moving = false; + setIntegerParam(pC_->motorStatusDone_, true); + next_poll = IDLEPOLL; + } else { + *moving = true; + if(targetReached == 1){ + det_starting = false; + //if(_function == ADCOM){ + // errlogPrintf("Resetting starting flag following targetReached flag\n"); + //} + } + setIntegerParam(pC_->motorStatusDone_, false); + } + + // read position + if(_function == ADCOM) { + strcpy(command,"Q0510"); + } else if(_function == ADCOZ) { + strcpy(command, "Q454"); + } else if(_function == ADPARK){ + strcpy(command,"P358"); + } + status = pC_->lowLevelWriteRead(axisNo_, command, response); + if(status != asynSuccess){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "AmorDetectorAxis::poll failed to read value"); + updateMsgTxtFromDriver("Failed to read Amor Detector value"); + return status; + } + if(_function != ADPARK){ + sscanf(response, "%lf", &position); + } else { + // analyse the park code to check where the thing is. This does + // not show good values while driving. But that is a limitation + // of the hardware. + sscanf(response, "%d", &parkCode); + if(parkCode == 1) { + position = 0; + } else { + position = -100; + } + // errlogPrintf("parkCode %d , position %f,targetREached = %d\n", parkCode, position, targetReached); + } + setDoubleParam(pC_->motorPosition_, position * 1000.); + setDoubleParam(pC_->motorEncoderPosition_, position * 1000.); + + //if(_function == ADCOM){ + // errlogPrintf("polling for function %d, position %f, pos*10^3 %f, targetREached = %d\n", _function, position, + // position*MULT, targetReached); + // } + + // read error status + sprintf(command, "P359"); + status = pC_->lowLevelWriteRead(axisNo_, command, response); + if(status == asynSuccess) { + sscanf(response, "%d", &errorCode); + switch(errorCode) { + case 0: + break; + case 1: + strcpy(errorMessage,"COZ not released"); + break; + case 2: + strcpy(errorMessage,"Motor is in wrong state"); + break; + case 3: + strcpy(errorMessage,"FTZ error while moving"); + break; + case 4: + strcpy(errorMessage, "Break while moving"); + break; + case 5: + strcpy(errorMessage, "No freigabe during move"); + break; + default: + strcpy(errorMessage, "Unknown error code from: "); + strcat(errorMessage, response); + break; + } + if(strlen(errorMessage) > 1) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "AmorDetectorAxis::poll: %s", errorMessage); + updateMsgTxtFromDriver(errorMessage); + setIntegerParam(pC_->motorStatusProblem_, true); + status = asynError; + } + } + + callParamCallbacks(); + return status; +} + + diff --git a/sinqEPICSApp/src/pmacAxis.h b/sinqEPICSApp/src/pmacAxis.h index a09ee6f..83712c2 100644 --- a/sinqEPICSApp/src/pmacAxis.h +++ b/sinqEPICSApp/src/pmacAxis.h @@ -157,4 +157,35 @@ public: friend class pmacV3Controller; }; +/* + * Special motors for the AMOR detector movement. The whole + * command set is different but on a pmac controller. This implements + * a coordinated movement of cox, coz and ftz in order not to break + * the flight tube which may have been mounted. This is mapped to three + * motors selected via the function code: com, the detector omega, coz, + * the detector z offset and park, for parking the flightpath. + */ + +#define ADCOM 1 +#define ADCOZ 2 +#define ADPARK 3 + +class AmorDetectorAxis: public pmacAxis { +public: + AmorDetectorAxis(pmacController *pController, int axisNo, int function); + + asynStatus move(double position, int relative, double min_velocity, + double max_velocity, double acceleration); + asynStatus poll(bool *moving); + asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration); + asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards); + asynStatus stop(double acceleration); + asynStatus setPosition(double position); + +protected: + int _function; + int det_starting; + time_t det_startTime; +}; + #endif /* pmacAxis_H */ diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index 3083e29..94c48ac 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -751,7 +751,33 @@ asynStatus pmacCreateAxes(const char *pmacName, return asynSuccess; } +/** + * C wrapper for the pmacAmorDetectorAxis constructor. + * See pmacAmorDetectorAxis::pmacAmorDetectorAxis. + * + */ +asynStatus pmacCreateAmorDetectorAxis(const char *pmacName, /* specify which controller by port name */ + int axis, /* axis number (start from 1). */ + int function_code) /* AMOR detector axis function */ +{ + pmacController *pC; + pmacAxis *pAxis; + static const char *functionName = "pmacCreateHRPTAxis"; + + pC = (pmacController*) findAsynPortDriver(pmacName); + if (!pC) { + printf("%s:%s: Error port %s not found\n", + driverName, functionName, pmacName); + return asynError; + } + + pC->lock(); + pAxis = new AmorDetectorAxis(pC, axis, function_code); + pAxis = NULL; + pC->unlock(); + return asynSuccess; +} /*================================ SeleneController ===============================================*/ @@ -996,6 +1022,19 @@ static void configpmacAxesCallFunc(const iocshArgBuf *args) pmacCreateAxes(args[0].sval, args[1].ival); } +/* pmacCraeteAmorDetectorAxis */ +static const iocshArg pmacCreateAmorDetectorAxisArg0 = {"Controller port name", iocshArgString}; +static const iocshArg pmacCreateAmorDetectorAxisArg1 = {"Axis number", iocshArgInt}; +static const iocshArg pmacCreateAmorDetectorAxisArg2 = {"Function code", iocshArgInt}; +static const iocshArg * const pmacCreateAmorDetectorAxisArgs[] = {&pmacCreateAmorDetectorAxisArg0, + &pmacCreateAmorDetectorAxisArg1, &pmacCreateAmorDetectorAxisArg2}; +static const iocshFuncDef configpmacAmorDetectorAxis = {"pmacCreateAmorDetectorAxis", 3, pmacCreateAmorDetectorAxisArgs}; + +static void configpmacAmorDetectorAxisCallFunc(const iocshArgBuf *args) +{ + pmacCreateAmorDetectorAxis(args[0].sval, args[1].ival, args[2].ival); +} + static void pmacControllerRegister(void) @@ -1005,6 +1044,7 @@ static void pmacControllerRegister(void) iocshRegister(&configpmacV3CreateController, configpmacV3CreateControllerCallFunc); iocshRegister(&configpmacAxis, configpmacAxisCallFunc); iocshRegister(&configpmacHRPTAxis, configpmacHRPTAxisCallFunc); + iocshRegister(&configpmacAmorDetectorAxis, configpmacAmorDetectorAxisCallFunc); iocshRegister(&configSeleneCreateAxis, configSeleneCreateAxisCallFunc); iocshRegister(&configLiftAxis, configLiftAxisCallFunc); iocshRegister(&configpmacV3Axis, configpmacV3AxisCallFunc); diff --git a/sinqEPICSApp/src/pmacController.h b/sinqEPICSApp/src/pmacController.h index 5703011..559ef60 100644 --- a/sinqEPICSApp/src/pmacController.h +++ b/sinqEPICSApp/src/pmacController.h @@ -140,6 +140,7 @@ class pmacController : public SINQController { friend class SeleneAxis; friend class LiftAxis; friend class pmacV3Axis; + friend class AmorDetectorAxis; }; #define NUM_PMAC_PARAMS (&LAST_PMAC_PARAM - &FIRST_PMAC_PARAM + 1)