From c972cce072ca9abec8ccf4be010dc69d14608659 Mon Sep 17 00:00:00 2001 From: Mathis Stefan Date: Thu, 18 Jul 2024 13:43:12 +0200 Subject: [PATCH 1/2] Added girder translation axis drivers --- Makefile.RHEL8 | 3 +- sinqEPICSApp/src/pmacAxis.cpp | 275 ++++++++++++++++++++++++++++ sinqEPICSApp/src/pmacAxis.h | 17 ++ sinqEPICSApp/src/pmacController.cpp | 40 ++++ sinqEPICSApp/src/pmacController.h | 2 + 5 files changed, 335 insertions(+), 2 deletions(-) diff --git a/Makefile.RHEL8 b/Makefile.RHEL8 index bd514aa..0d24e3d 100644 --- a/Makefile.RHEL8 +++ b/Makefile.RHEL8 @@ -15,7 +15,7 @@ REQUIRED+=asynMotor # using a test version #scaler_VERSION=2024 -LIBVERSION=2024 +LIBVERSION=2024-dev TEMPLATES += sinqEPICSApp/Db/dimetix.db TEMPLATES += sinqEPICSApp/Db/slsvme.db @@ -35,6 +35,5 @@ SOURCES += sinqEPICSApp/src/EuroMoveDriver.cpp SOURCES += sinqEPICSApp/src/pmacAsynIPPort.c SOURCES += sinqEPICSApp/src/pmacAxis.cpp SOURCES += sinqEPICSApp/src/pmacController.cpp -SOURCES += sinqEPICSApp/src/drvAsynMasterMACSPort.c SOURCES += sinqEPICSApp/src/MasterMACSDriver.cpp # MISCS would be the place to keep the stream device template files diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index 79cd046..d83248a 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -32,6 +32,10 @@ * * Mark Koennecke, June 2023 * + * Added driver for GirderAxis + * + * Stefan Mathis, July 2024 + * ********************************************/ #include @@ -1479,4 +1483,275 @@ asynStatus AmorDetectorAxis::poll(bool *moving) return status; } +/*-------------------------------------------------------------------------------*/ +GirderAxis::GirderAxis(pmacController *pController, int axisNo) + : pmacV3Axis(pController, axisNo){ + + // GirderAxis expects a pmacV3Controller. Therefore it is checked whether the pointer pController can be cast + // to this object type. If this is not possible, the user made an error in the configuration files. This is documented + // in a corresponding error; after that, an exception is thrown to avoid returning an "illegal" instance of GirderAxis. + pmacV3Controller* pV3Controller = dynamic_cast(pController); + if (pV3Controller == nullptr) { + errlogPrintf("A GirderAxis instance needs a pmacV3Controller. Please check the configuration files."); + throw std::invalid_argument("A GirderAxis instance needs a pmacV3Controller. Please check the configuration files."); + } + + // Initial values for member variables + next_poll = -1; + previous_position_ = 0.0; + previous_direction_ = 0; + status6Time = 0; + statusPos = 0.0; + homing = 0; + axisErrorCount = 0; +}; + +/*-------------------------------------------------------------------------------*/ +asynStatus GirderAxis::move(double position, int relative, double min_velocity, double max_velocity, double acceleration) { + + // If the axis is not enabled, do nothing + if(!IsEnable) { + updateMsgTxtFromDriver("Error: axis disabled"); + return asynError; + } + + // ======================================= + // Local variable declaration + + asynStatus status = asynError; + char command[pC_->PMAC_MAXBUF_] = {0}; + char response[pC_->PMAC_MAXBUF_] = {0}; + double realPosition = 0; + + // ======================================= + + updateMsgTxtFromDriver(""); + static const char *functionName = "GirderAxis::move"; + pC_->debugFlow(functionName); + + if (relative) { + realPosition = previous_position_ + position / MULT; + } else { + realPosition = position / MULT; + } + startTime = time(NULL); + status6Time = 0; + starting = 1; + + // Set target position + snprintf(command, sizeof(command), "Q251=%f", realPosition); + status = pC_->lowLevelWriteRead(axisNo_, command, response); + if (status == asynError) { + updateMsgTxtFromDriver("Error: Could not set target position"); + return asynError; + } + + // Start motion + snprintf(command, sizeof(command), "P150=1"); + status = pC_->lowLevelWriteRead(axisNo_, command, response); + if (status == asynError) { + updateMsgTxtFromDriver("Error: Could not start motion"); + return asynError; + } + + next_poll = -1; + return status; +} + +asynStatus GirderAxis::stop(double acceleration) { + + // ======================================= + // Local variable declaration + + asynStatus status = asynSuccess; + static const char *functionName = "GirderAxis::stop"; + bool moving = false; + char command[pC_->PMAC_MAXBUF_] = {0}; + char response[pC_->PMAC_MAXBUF_] = {0}; + + // ======================================= + + pC_->debugFlow(functionName); + this->poll(&moving); + + if(moving) { + // only send a stop when actually moving + snprintf(command, sizeof(command), "P150=8"); + status = pC_->lowLevelWriteRead(axisNo_, command, response); + } + return status; +} + +asynStatus GirderAxis::poll(bool *moving) { + + // ======================================= + // Local variable declaration + + asynStatus status = asynSuccess; + static const char *functionName = "GirderAxis::poll"; + char command[pC_->PMAC_MAXBUF_] = {0}; + char response[pC_->PMAC_MAXBUF_] = {0}; + char message[132], *axMessage; + double position = 0; + int moving_to_position = 0, nvals = 0, axisProblemFlag = 0, axStat = 0, axError = 0, isEnabled = 1; + asynStatus st; + + // ======================================= + + // Check for girder axis specific errors + snprintf(command, sizeof(command), "P159"); + status = pC_->lowLevelWriteRead(axisNo_, command, response); + sscanf(response, "P159=%d", &axError); + switch(axError) { + case 1: + snprintf(message, sizeof(message), "PMAC: %s on %d", + "Axis not switched on", axisNo_); + updateMsgTxtFromDriver(message); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message); + axisProblemFlag = 1; + isEnabled = 0; + break; + case 2: + snprintf(message, sizeof(message), "PMAC: %s on %d", + "Axis not ready for new command", axisNo_); + updateMsgTxtFromDriver(message); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message); + axisProblemFlag = 1; + break; + case 3: + snprintf(message, sizeof(message), "PMAC: %s on %d", + "Axis 1 ERROR during motion", axisNo_); + updateMsgTxtFromDriver(message); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message); + axisProblemFlag = 1; + break; + case 4: + snprintf(message, sizeof(message), "PMAC: %s on %d", + "Axis 2 ERROR during motion", axisNo_); + updateMsgTxtFromDriver(message); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message); + axisProblemFlag = 1; + break; + } + st = setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag); + status = status > st ? status : st; + + // Downcast the pointer pmacController to pmacV3Controller in a typesafe manner + pmacV3Controller* p3C_ = dynamic_cast(pC_); + if (p3C_ == nullptr) { + errlogPrintf("A GirderAxis instance needs a pmacV3Controller. Please check the configuration files."); + throw std::invalid_argument("A GirderAxis instance needs a pmacV3Controller. Please check the configuration files."); + } + setIntegerParam(p3C_->axisEnabled_, isEnabled); + + st = setIntegerParam(p3C_->enableAxis_, isEnabled); + status = status > st ? status : st; + + int direction = 0; + + /* + In GirderAxis::move, the user input is scaled by /MULT. Hence, the output of + Qxx10 needs to be scaled by *MULT + */ + st = setDoubleParam(pC_->motorPosition_, position * MULT); + status = status > st ? status : st; + st = setDoubleParam(pC_->motorEncoderPosition_, position * MULT); + status = status > st ? status : st; + + // Calculate the current (or last) movement direction + if ((position - previous_position_) > 0) { + direction = 1; + } else if (position - previous_position_ == 0.0) { + direction = previous_direction_; + } else { + direction = 0; + } + st = setIntegerParam(pC_->motorStatusDirection_, direction); + status = status > st ? status : st; + + // Store the position which was read out from the hardware together with the calculated direction for the next poll + previous_position_ = position; + previous_direction_ = direction; + + // Is the axis currently moving? + snprintf(command, sizeof(command), "P154"); + status = pC_->lowLevelWriteRead(axisNo_, command, response); + sscanf(response, "P154=%d", &moving_to_position); + + /* + This code tests whether the axis is too long in status 5 or 6. + If the axis is in status 5 or 6, a time counter (status6Time) is started and updated during subsequent polls. + If status6Time exceeds the estimated arrival time of 120 seconds, a corresponding error is returned. + */ + int EstimatedTimeOfArrival = 120; // seconds + if (axStat == 5 || axStat == 6) { + if (status6Time == 0) { + status6Time = time(nullptr); + statusPos = position; + } else { + if (time(nullptr) > status6Time + EstimatedTimeOfArrival) { + /* trigger error only when not moving */ + if (abs(position - statusPos) < .1) { + moving_to_position = 0; + errlogPrintf( + "Axis %d stayed in status 5 or 6 for more than %d seconds BROKEN\n", + axisNo_, EstimatedTimeOfArrival); + updateMsgTxtFromDriver("Axis stayed in status 5 or 6 for more than estimated time: BROKEN"); + status6Time = 0; + return status; + } else { + status6Time = time(nullptr); + statusPos = position; + } + } + } + } + + /* + If the axis is moving, set the corresponding flags in the pmacController (pC_) and end the poll. + */ + if (!moving_to_position) { + *moving = true; + st = setIntegerParam(pC_->motorStatusMoving_, true); + status = status > st ? status : st; + st = setIntegerParam(pC_->motorStatusDone_, false); + return status > st ? status : st; + } else { + *moving = false; + st = setIntegerParam(pC_->motorStatusMoving_, false); + status = status > st ? status : st; + st = setIntegerParam(pC_->motorStatusDone_, true); + status = status > st ? status : st; + } + + /* Set any axis specific general problem bits. */ + if (axError != 0) { + axisProblemFlag = 1; + if (axisErrorCount < 10) { + axMessage = translateAxisError(axError); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "drvGirderAxisGetStatus: Axis %d is in deep trouble: axis error " + "code %d, translated: %s:, status code = %d\n", + axisNo_, axError, axMessage, axStat); + snprintf(message, sizeof(message), "GirderAxis 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; + } + + st = setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag); + status = status > st ? status : st; + + return status; +} \ No newline at end of file diff --git a/sinqEPICSApp/src/pmacAxis.h b/sinqEPICSApp/src/pmacAxis.h index 658a2ec..1fc6454 100644 --- a/sinqEPICSApp/src/pmacAxis.h +++ b/sinqEPICSApp/src/pmacAxis.h @@ -189,4 +189,21 @@ protected: time_t det_startTime; }; +/*----------------------------------------------------------------------------------------------*/ + + +class GirderAxis: public pmacV3Axis { + public: + GirderAxis(pmacController *pController, int axisNo); + asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); + asynStatus stop(double acceleration); + asynStatus poll(bool *moving); + + protected: + int IsEnable; + + friend class pmacController; + friend class pmacV3Controller; +}; + #endif /* pmacAxis_H */ diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index 8548e32..71c7e2f 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -781,6 +781,34 @@ pC->unlock(); return asynSuccess; } +/** +* C wrapper for the GirderAxis constructor. +* See GirderAxis::GirderAxis. +* +*/ +asynStatus pmacCreateGirderAxis( + const char *pmacName, /* specify which controller by port name */ + int axis) /* axis number (start from 1). */ +{ + pmacController *pC; + pmacAxis *pAxis; + + static const char *functionName = "pmacCreateGirderAxis"; + + pC = (pmacController *)findAsynPortDriver(pmacName); + if (!pC) { + printf("%s:%s: Error port %s not found\n", driverName, functionName, + pmacName); + return asynError; + } + + pC->lock(); + pAxis = new GirderAxis(pC, axis); + pAxis = NULL; + pC->unlock(); + return asynSuccess; +} + /*================================ SeleneController ===============================================*/ asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 value) @@ -1057,6 +1085,18 @@ static void configpmacAmorDetectorAxisCallFunc(const iocshArgBuf *args) pmacCreateAmorDetectorAxis(args[0].sval, args[1].ival, args[2].ival); } +/* GirderCreateAxis */ +static const iocshArg pmacCreateGirderAxisArg0 = {"Controller port name", + iocshArgString}; +static const iocshArg pmacCreateGirderAxisArg1 = {"Axis number", iocshArgInt}; +static const iocshArg *const pmacCreateGirderAxisArgs[] = {&pmacCreateGirderAxisArg0, + &pmacCreateGirderAxisArg1}; +static const iocshFuncDef configpmacGirderAxis = {"pmacGirderCreateAxis", 2, + pmacCreateGirderAxisArgs}; + +static void configpmacGirderAxisCallFunc(const iocshArgBuf *args) { + pmacCreateGirderAxis(args[0].sval, args[1].ival); +} static void pmacControllerRegister(void) diff --git a/sinqEPICSApp/src/pmacController.h b/sinqEPICSApp/src/pmacController.h index 559ef60..b87d846 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 GirderAxis; friend class AmorDetectorAxis; }; @@ -182,6 +183,7 @@ public: friend class pmacV3Axis; friend class pmacAxis; + friend class GirderAxis; protected: pmacV3Axis **pAxes_; /**< Array of pointers to axis objects */ From a6a8f14b26677b79d6cde2cd7e4c862e5ab73c30 Mon Sep 17 00:00:00 2001 From: smathis Date: Tue, 10 Sep 2024 08:58:59 +0200 Subject: [PATCH 2/2] The member variable oredMSR of EL734Axis contains the axis status from the previous poll. Therefore, it needs to initialized with a sensible value (i.e. 1 which means that the axis is standing still and has no errors. --- sinqEPICSApp/src/EL734Driver.cpp | 438 +++++++++++++++++-------------- sinqEPICSApp/src/EL734Driver.h | 25 +- 2 files changed, 257 insertions(+), 206 deletions(-) diff --git a/sinqEPICSApp/src/EL734Driver.cpp b/sinqEPICSApp/src/EL734Driver.cpp index 73cb3bd..4d04ceb 100644 --- a/sinqEPICSApp/src/EL734Driver.cpp +++ b/sinqEPICSApp/src/EL734Driver.cpp @@ -11,7 +11,6 @@ Mark Koennecke, May, August 2017 */ - #include #include #include @@ -30,84 +29,82 @@ Mark Koennecke, May, August 2017 #define IDLEPOLL 60 - /** Creates a new EL734Controller object. - * \param[in] portName The name of the asyn port that will be created for this driver - * \param[in] EL734PortName The name of the drvAsynSerialPort that was created previously to connect to the EL734 controller - * \param[in] numAxes The number of axes that this controller supports - */ + * \param[in] portName The name of the asyn port that will be created for this driver + * \param[in] EL734PortName The name of the drvAsynSerialPort that was created previously to connect to the EL734 controller + * \param[in] numAxes The number of axes that this controller supports + */ EL734Controller::EL734Controller(const char *portName, const char *EL734PortName, int numAxes) - : SINQController(portName, EL734PortName, numAxes) + : SINQController(portName, EL734PortName, numAxes) { int axis; asynStatus status; static const char *functionName = "EL734Controller::EL734Controller"; - /* Connect to EL734 controller */ status = pasynOctetSyncIO->connect(EL734PortName, 0, &pasynUserController_, NULL); - if (status) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: cannot connect to EL734 controller\n", - functionName); + if (status) + { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: cannot connect to EL734 controller\n", + functionName); } - pasynOctetSyncIO->setOutputEos(pasynUserController_,"\r",strlen("\r")); - pasynOctetSyncIO->setInputEos(pasynUserController_,"\r",strlen("\r")); + pasynOctetSyncIO->setOutputEos(pasynUserController_, "\r", strlen("\r")); + pasynOctetSyncIO->setInputEos(pasynUserController_, "\r", strlen("\r")); switchRemote(); - for (axis=0; axis 0 then information is printed about each axis. - * After printing controller-specific information it calls asynMotorController::report() - */ + * \param[in] fp The file pointer on which report information will be written + * \param[in] level The level of report detail desired + * + * If details > 0 then information is printed about each axis. + * After printing controller-specific information it calls asynMotorController::report() + */ void EL734Controller::report(FILE *fp, int level) { - fprintf(fp, "EL734 motor driver %s, numAxes=%d\n", - this->portName, numAxes_); + fprintf(fp, "EL734 motor driver %s, numAxes=%d\n", + this->portName, numAxes_); // Call the base class method asynMotorController::report(fp, level); } /** Returns a pointer to an EL734Axis object. - * Returns NULL if the axis number encoded in pasynUser is invalid. - * \param[in] pasynUser asynUser structure that encodes the axis index number. */ -EL734Axis* EL734Controller::getAxis(asynUser *pasynUser) + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] pasynUser asynUser structure that encodes the axis index number. */ +EL734Axis *EL734Controller::getAxis(asynUser *pasynUser) { - return static_cast(asynMotorController::getAxis(pasynUser)); + return static_cast(asynMotorController::getAxis(pasynUser)); } /** Returns a pointer to an EL734Axis object. - * Returns NULL if the axis number encoded in pasynUser is invalid. - * \param[in] axisNo Axis index number. */ -EL734Axis* EL734Controller::getAxis(int axisNo) + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] axisNo Axis index number. */ +EL734Axis *EL734Controller::getAxis(int axisNo) { - return static_cast(asynMotorController::getAxis(axisNo)); + return static_cast(asynMotorController::getAxis(axisNo)); } void EL734Controller::switchRemote() @@ -116,27 +113,27 @@ void EL734Controller::switchRemote() size_t in, out; int reason; - strcpy(command,"RMT 1"); + strcpy(command, "RMT 1"); pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), - reply,COMLEN, 1.,&out,&in,&reason); - strcpy(command,"ECHO 0"); + reply, COMLEN, 1., &out, &in, &reason); + strcpy(command, "ECHO 0"); pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), - reply,COMLEN, 1.,&out,&in,&reason); - strcpy(command,"RMT 1"); + reply, COMLEN, 1., &out, &in, &reason); + strcpy(command, "RMT 1"); pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), - reply,COMLEN, 1.,&out,&in,&reason); - strcpy(command,"ECHO 0"); + reply, COMLEN, 1., &out, &in, &reason); + strcpy(command, "ECHO 0"); pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), - reply,COMLEN, 1.,&out,&in,&reason); + reply, COMLEN, 1., &out, &in, &reason); } /** - * send a command to the EL734 and read the reply. Do some error and controller + * send a command to the EL734 and read the reply. Do some error and controller * issue fixing on the way * \param[in] command The command to send * \param[out] reply The controllers reply */ -asynStatus EL734Controller::transactController(int axisNo,char command[COMLEN], char reply[COMLEN]) +asynStatus EL734Controller::transactController(int axisNo, char command[COMLEN], char reply[COMLEN]) { asynStatus status; size_t in, out, i; @@ -145,58 +142,71 @@ asynStatus EL734Controller::transactController(int axisNo,char command[COMLEN], SINQAxis *axis = getAxis(axisNo); pasynOctetSyncIO->flush(pasynUserController_); - + status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), - reply,COMLEN, 2.,&out,&in,&reason); - if(status != asynSuccess){ - if(axis!= NULL){ + reply, COMLEN, 2., &out, &in, &reason); + if (status != asynSuccess) + { + if (axis != NULL) + { axis->updateMsgTxtFromDriver("Lost connection to motor controller"); } return status; } - /* + /* check for the offline reply */ - if(strstr(reply,"?LOC") != NULL){ + if (strstr(reply, "?LOC") != NULL) + { switchRemote(); return pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), - reply,COMLEN, 1.,&out,&in,&reason); + reply, COMLEN, 1., &out, &in, &reason); } /* check for echos. This means that the thing is offline. */ - if(strstr(reply,"p ") != NULL || strstr(reply,"u ") != NULL || strstr(reply,"msr ") != NULL){ + if (strstr(reply, "p ") != NULL || strstr(reply, "u ") != NULL || strstr(reply, "msr ") != NULL) + { switchRemote(); return pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), - reply,COMLEN, 1.,&out,&in,&reason); + reply, COMLEN, 1., &out, &in, &reason); } /* check for EL734 errors */ strcpy(myReply, reply); - for(i = 0; i < strlen(reply); i++){ + for (i = 0; i < strlen(reply); i++) + { myReply[i] = (char)tolower((int)reply[i]); } - if(strstr(myReply,"?cmd") != NULL){ - snprintf(errTxt,sizeof(errTxt), "Bad command %s at axis %d", command, axisNo); + if (strstr(myReply, "?cmd") != NULL) + { + snprintf(errTxt, sizeof(errTxt), "Bad command %s at axis %d", command, axisNo); asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt); - if(axis!= NULL){ + if (axis != NULL) + { axis->updateMsgTxtFromDriver(errTxt); } return asynError; - } else if(strstr(myReply,"?par") != NULL){ - snprintf(errTxt,sizeof(errTxt), "Bad parameter in command %s", command); + } + else if (strstr(myReply, "?par") != NULL) + { + snprintf(errTxt, sizeof(errTxt), "Bad parameter in command %s", command); asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt); - if(axis!= NULL){ + if (axis != NULL) + { axis->updateMsgTxtFromDriver(errTxt); } return asynError; - } else if(strstr(myReply,"?rng") != NULL){ - snprintf(errTxt,sizeof(errTxt), "Parameter out of range in command %s", command); + } + else if (strstr(myReply, "?rng") != NULL) + { + snprintf(errTxt, sizeof(errTxt), "Parameter out of range in command %s", command); asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt); - if(axis!= NULL){ + if (axis != NULL) + { axis->updateMsgTxtFromDriver(errTxt); } return asynError; @@ -208,14 +218,14 @@ asynStatus EL734Controller::transactController(int axisNo,char command[COMLEN], // These are the EL734Axis methods /** Creates a new EL734Axis object. - * \param[in] pC Pointer to the EL734Controller to which this axis belongs. - * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. - * - * Initializes register numbers, etc. - */ + * \param[in] pC Pointer to the EL734Controller to which this axis belongs. + * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. + * + * Initializes register numbers, etc. + */ EL734Axis::EL734Axis(EL734Controller *pC, int axisNo) - : SINQAxis(pC, axisNo), pC_(pC) -{ + : SINQAxis(pC, axisNo), pC_(pC) +{ char command[COMLEN], reply[COMLEN]; asynStatus status; int count = 0; @@ -224,52 +234,62 @@ EL734Axis::EL734Axis(EL734Controller *pC, int axisNo) /* get the hardware limits from the controller */ - sprintf(command,"H %d",axisNo_); - status = pC_->transactController(axisNo_,command,reply); - if(status == asynSuccess){ - count = sscanf(reply,"%f %f",&low, &high); - if(count >= 2){ - pC_->setDoubleParam(axisNo_,pC_->motorLowLimit_,low); - pC_->setDoubleParam(axisNo_,pC_->motorHighLimit_,high); - callParamCallbacks(); - } else { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "Bad response - %s - requesting limits at axis %d", reply, axisNo_); + sprintf(command, "H %d", axisNo_); + status = pC_->transactController(axisNo_, command, reply); + if (status == asynSuccess) + { + count = sscanf(reply, "%f %f", &low, &high); + if (count >= 2) + { + pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, low); + pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, high); + callParamCallbacks(); + } + else + { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Bad response - %s - requesting limits at axis %d", reply, axisNo_); } - } else { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "Failed to read limits at axis %d", axisNo_); } + else + { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Failed to read limits at axis %d", axisNo_); + } + + /* + oredMSR contains the axis status from the previous poll. The initial value + 1 means that the axis is not moving and has no errors. + */ + oredMSR = 1; } - - /** Reports on status of the axis - * \param[in] fp The file pointer on which report information will be written - * \param[in] level The level of report detail desired - * - * After printing device-specific information calls asynMotorAxis::report() - */ + * \param[in] fp The file pointer on which report information will be written + * \param[in] level The level of report detail desired + * + * After printing device-specific information calls asynMotorAxis::report() + */ void EL734Axis::report(FILE *fp, int level) { - if (level > 0) { + if (level > 0) + { fprintf(fp, " axis %d\n", axisNo_); } // Call the base class method - //asynMotorAxis::report(fp, level); + // asynMotorAxis::report(fp, level); } - asynStatus EL734Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) { asynStatus status; - //static const char *functionName = "EL734Axis::move"; + // static const char *functionName = "EL734Axis::move"; char command[COMLEN], reply[COMLEN]; // status = sendAccelAndVelocity(acceleration, maxVelocity); - + errlogPrintf("minVelocity = %f, maxVelocity = %f\n", minVelocity, maxVelocity); /* @@ -278,16 +298,16 @@ asynStatus EL734Axis::move(double position, int relative, double minVelocity, do sprintf(command, "J %d %d", axisNo_, (int)maxVelocity); status = pC_->transactController(axisNo_, command, reply); - if (relative) { + if (relative) + { position += this->position; } - oredMSR = 0; homing = 0; errorReported = 0; - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "Starting axis %d with destination %f", axisNo_,position/1000); - sprintf(command, "p %d %.3f", axisNo_, position/1000.); - status = pC_->transactController(axisNo_,command,reply); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Starting axis %d with destination %f", axisNo_, position / 1000); + sprintf(command, "p %d %.3f", axisNo_, position / 1000.); + status = pC_->transactController(axisNo_, command, reply); setIntegerParam(pC_->motorStatusProblem_, false); updateMsgTxtFromDriver(""); next_poll = -1; @@ -297,7 +317,7 @@ asynStatus EL734Axis::move(double position, int relative, double minVelocity, do asynStatus EL734Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) { asynStatus status; - //static const char *functionName = "EL734Axis::home"; + // static const char *functionName = "EL734Axis::home"; char command[COMLEN], reply[COMLEN]; // status = sendAccelAndVelocity(acceleration, maxVelocity); @@ -308,52 +328,55 @@ asynStatus EL734Axis::home(double minVelocity, double maxVelocity, double accele sprintf(command, "R %d", axisNo_); homing = 1; - next_poll= -1; - status = pC_->transactController(axisNo_,command,reply); + next_poll = -1; + status = pC_->transactController(axisNo_, command, reply); return status; } asynStatus EL734Axis::moveVelocity(double minVelocity, double maxVelocity, double acceleration) { asynStatus status; - //static const char *functionName = "EL734Axis::moveVelocity"; + // static const char *functionName = "EL734Axis::moveVelocity"; char command[COMLEN], reply[COMLEN]; // asynPrint(pasynUser_, ASYN_TRACE_FLOW, // "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n", // functionName, minVelocity, maxVelocity, acceleration); - errorReported = 0; - if (maxVelocity > 0.) { + if (maxVelocity > 0.) + { /* This is a positive move */ sprintf(command, "FF %d", axisNo_); - } else { - /* This is a negative move */ - sprintf(command, "FB %d", axisNo_); } - status = pC_->transactController(axisNo_,command,reply); + else + { + /* This is a negative move */ + sprintf(command, "FB %d", axisNo_); + } + status = pC_->transactController(axisNo_, command, reply); setIntegerParam(pC_->motorStatusProblem_, false); updateMsgTxtFromDriver(""); next_poll = -1; return status; } -asynStatus EL734Axis::stop(double acceleration ) +asynStatus EL734Axis::stop(double acceleration) { asynStatus status = asynSuccess; - //static const char *functionName = "EL734Axis::stop"; + // static const char *functionName = "EL734Axis::stop"; char command[COMLEN], reply[COMLEN]; bool moving = false; this->poll(&moving); - if(moving && errorReported == 0){ - sprintf(command, "S %d", axisNo_); - status = pC_->transactController(axisNo_,command,reply); - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Sent STOP on Axis %d\n", axisNo_); - updateMsgTxtFromDriver("Axis interrupted"); - errorReported = 1; - } + if (moving && errorReported == 0) + { + sprintf(command, "S %d", axisNo_); + status = pC_->transactController(axisNo_, command, reply); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Sent STOP on Axis %d\n", axisNo_); + updateMsgTxtFromDriver("Axis interrupted"); + errorReported = 1; + } return status; } @@ -361,11 +384,11 @@ asynStatus EL734Axis::stop(double acceleration ) asynStatus EL734Axis::setPosition(double position) { asynStatus status; - //static const char *functionName = "EL734Axis::setPosition"; + // static const char *functionName = "EL734Axis::setPosition"; char command[COMLEN], reply[COMLEN]; - sprintf(command, "U %d %f", axisNo_, position/1000.); - status = pC_->transactController(axisNo_,command,reply); + sprintf(command, "U %d %f", axisNo_, position / 1000.); + status = pC_->transactController(axisNo_, command, reply); next_poll = -1; return status; @@ -373,8 +396,8 @@ asynStatus EL734Axis::setPosition(double position) asynStatus EL734Axis::setClosedLoop(bool closedLoop) { - //static const char *functionName = "EL734Axis::setClosedLoop"; - + // static const char *functionName = "EL734Axis::setClosedLoop"; + /* This belongs into the Kingdom of Electronics. We do not do this. @@ -384,45 +407,50 @@ asynStatus EL734Axis::setClosedLoop(bool closedLoop) } /** Polls the axis. - * This function reads the motor position, the limit status, the home status, the moving status, - * and the drive power-on status. - * It calls setIntegerParam() and setDoubleParam() for each item that it polls, - * and then calls callParamCallbacks() at the end. - * \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */ + * This function reads the motor position, the limit status, the home status, the moving status, + * and the drive power-on status. + * It calls setIntegerParam() and setDoubleParam() for each item that it polls, + * and then calls callParamCallbacks() at the end. + * \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */ asynStatus EL734Axis::poll(bool *moving) -{ +{ int msr, count; float low, high; asynStatus comStatus = asynSuccess; char command[COMLEN], reply[COMLEN], errTxt[256]; // protect against excessive polling - if(time(NULL) < next_poll){ + if (time(NULL) < next_poll) + { *moving = false; return asynSuccess; } // read hardware limits - sprintf(command,"H %d",axisNo_); - comStatus = pC_->transactController(axisNo_,command,reply); - if(comStatus == asynSuccess){ - count = sscanf(reply,"%f %f",&low, &high); - if(count >= 2){ - pC_->setDoubleParam(axisNo_,pC_->motorLowLimit_,low); - pC_->setDoubleParam(axisNo_,pC_->motorHighLimit_,high); - callParamCallbacks(); + sprintf(command, "H %d", axisNo_); + comStatus = pC_->transactController(axisNo_, command, reply); + if (comStatus == asynSuccess) + { + count = sscanf(reply, "%f %f", &low, &high); + if (count >= 2) + { + pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, low); + pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, high); + callParamCallbacks(); } } - + // Read the current motor position - setIntegerParam(pC_->motorStatusProblem_,false); - sprintf(command,"u %d", axisNo_); - comStatus = pC_->transactController(axisNo_,command,reply); - if(comStatus == asynError){ - setIntegerParam(pC_->motorStatusProblem_,true); + setIntegerParam(pC_->motorStatusProblem_, false); + sprintf(command, "u %d", axisNo_); + comStatus = pC_->transactController(axisNo_, command, reply); + if (comStatus == asynError) + { + setIntegerParam(pC_->motorStatusProblem_, true); goto skip; } - if(strstr(reply,"*ES") != NULL){ + if (strstr(reply, "*ES") != NULL) + { *moving = false; setIntegerParam(pC_->motorStatusDone_, true); setIntegerParam(pC_->motorStatusProblem_, true); @@ -430,70 +458,86 @@ asynStatus EL734Axis::poll(bool *moving) updateMsgTxtFromDriver("Emergency Stop Engaged"); comStatus = asynError; goto skip; - } else if(strstr(reply,"?BSY") != NULL){ + } + else if (strstr(reply, "?BSY") != NULL) + { *moving = true; setIntegerParam(pC_->motorStatusDone_, false); goto skip; } - count = sscanf(reply,"%lf", &position); - if(count != 1) { - if(!homing) { - snprintf(errTxt,sizeof(errTxt),"Bad reply %s when reading position for %d", reply, axisNo_); + count = sscanf(reply, "%lf", &position); + if (count != 1) + { + if (!homing) + { + snprintf(errTxt, sizeof(errTxt), "Bad reply %s when reading position for %d", reply, axisNo_); setIntegerParam(pC_->motorStatusProblem_, true); errorReported = 1; updateMsgTxtFromDriver(errTxt); comStatus = asynError; goto skip; } - } else { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "Axis %d, reply %s, position %lf\n", axisNo_, reply, position); - setDoubleParam(pC_->motorPosition_, position*1000); - //setDoubleParam(pC_->motorEncoderPosition_, position); + } + else + { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "Axis %d, reply %s, position %lf\n", axisNo_, reply, position); + setDoubleParam(pC_->motorPosition_, position * 1000); + // setDoubleParam(pC_->motorEncoderPosition_, position); } // Read the moving status of this motor - sprintf(command,"msr %d",axisNo_); - comStatus = pC_->transactController(axisNo_,command,reply); - if(comStatus == asynError){ - setIntegerParam(pC_->motorStatusProblem_,true); + sprintf(command, "msr %d", axisNo_); + comStatus = pC_->transactController(axisNo_, command, reply); + if (comStatus == asynError) + { + setIntegerParam(pC_->motorStatusProblem_, true); goto skip; } - sscanf(reply,"%x",&msr); + sscanf(reply, "%x", &msr); - //errlogPrintf("Axis %d, reply %s, msr %d, oredmsr = %d, position = %lf\n", + // errlogPrintf("Axis %d, reply %s, msr %d, oredmsr = %d, position = %lf\n", // axisNo_, reply, msr, oredMSR, position); oredMSR |= msr; - if( (msr & 0x1) == 0){ + if ((msr & 0x1) == 0) + { // done: check for trouble - //errlogPrintf("Axis %d finished\n", axisNo_); + // errlogPrintf("Axis %d finished\n", axisNo_); *moving = false; setIntegerParam(pC_->motorStatusDone_, true); - next_poll = time(NULL)+IDLEPOLL; - if(oredMSR & 0x10){ + next_poll = time(NULL) + IDLEPOLL; + if (oredMSR & 0x10) + { setIntegerParam(pC_->motorStatusLowLimit_, true); updateMsgTxtFromDriver("Lower Limit Hit"); errorReported = 1; comStatus = asynError; goto skip; - } else { + } + else + { setIntegerParam(pC_->motorStatusLowLimit_, false); } - if(oredMSR & 0x20){ + if (oredMSR & 0x20) + { setIntegerParam(pC_->motorStatusHighLimit_, true); updateMsgTxtFromDriver("Upper Limit Hit"); errorReported = 1; comStatus = asynError; goto skip; - } else { + } + else + { setIntegerParam(pC_->motorStatusHighLimit_, false); } - if(homing){ + if (homing) + { setIntegerParam(pC_->motorStatusAtHome_, true); } - if(oredMSR & 0x1000){ + if (oredMSR & 0x1000) + { setIntegerParam(pC_->motorStatusProblem_, true); // errlogPrintf("Detected air cushion error on %d", axisNo_); asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Air cushion problem on %d", axisNo_); @@ -502,7 +546,8 @@ asynStatus EL734Axis::poll(bool *moving) comStatus = asynError; goto skip; } - if(oredMSR & 0x100){ + if (oredMSR & 0x100) + { setIntegerParam(pC_->motorStatusProblem_, true); asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Run failure at %d", axisNo_); updateMsgTxtFromDriver("Run failure"); @@ -510,7 +555,8 @@ asynStatus EL734Axis::poll(bool *moving) errorReported = 1; goto skip; } - if(oredMSR & 0x400){ + if (oredMSR & 0x400) + { setIntegerParam(pC_->motorStatusProblem_, true); asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Positioning failure at %d", axisNo_); updateMsgTxtFromDriver("Positioning failure"); @@ -518,17 +564,20 @@ asynStatus EL734Axis::poll(bool *moving) errorReported = 1; goto skip; } - if(oredMSR & 0x200 || oredMSR & 0x80) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, "Positioning fault at %d", axisNo_); - } + if (oredMSR & 0x200 || oredMSR & 0x80) + { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, "Positioning fault at %d", axisNo_); + } setIntegerParam(pC_->motorStatusProblem_, false); -} else { + } + else + { *moving = true; next_poll = -1; setIntegerParam(pC_->motorStatusDone_, false); -} + } - skip: +skip: callParamCallbacks(); return comStatus; } @@ -537,9 +586,9 @@ asynStatus EL734Axis::poll(bool *moving) static const iocshArg EL734CreateControllerArg0 = {"Port name", iocshArgString}; static const iocshArg EL734CreateControllerArg1 = {"EL734 port name", iocshArgString}; static const iocshArg EL734CreateControllerArg2 = {"Number of axes", iocshArgInt}; -static const iocshArg * const EL734CreateControllerArgs[] = {&EL734CreateControllerArg0, - &EL734CreateControllerArg1, - &EL734CreateControllerArg2}; +static const iocshArg *const EL734CreateControllerArgs[] = {&EL734CreateControllerArg0, + &EL734CreateControllerArg1, + &EL734CreateControllerArg2}; static const iocshFuncDef EL734CreateControllerDef = {"EL734CreateController", 3, EL734CreateControllerArgs}; static void EL734CreateContollerCallFunc(const iocshArgBuf *args) { @@ -551,6 +600,7 @@ static void EL734Register(void) iocshRegister(&EL734CreateControllerDef, EL734CreateContollerCallFunc); } -extern "C" { -epicsExportRegistrar(EL734Register); +extern "C" +{ + epicsExportRegistrar(EL734Register); } diff --git a/sinqEPICSApp/src/EL734Driver.h b/sinqEPICSApp/src/EL734Driver.h index 3e1f16c..f48af7d 100644 --- a/sinqEPICSApp/src/EL734Driver.h +++ b/sinqEPICSApp/src/EL734Driver.h @@ -28,31 +28,32 @@ public: asynStatus setClosedLoop(bool closedLoop); private: - EL734Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. - * Abbreviated because it is used very frequently */ + EL734Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. + * Abbreviated because it is used very frequently */ int oredMSR; double position; int homing; time_t next_poll; int errorReported; - -friend class EL734Controller; + + friend class EL734Controller; }; -class EL734Controller : public SINQController { +class EL734Controller : public SINQController +{ public: EL734Controller(const char *portName, const char *EL734PortName, int numAxes); void report(FILE *fp, int level); - EL734Axis* getAxis(asynUser *pasynUser); - EL734Axis* getAxis(int axisNo); + EL734Axis *getAxis(asynUser *pasynUser); + EL734Axis *getAxis(int axisNo); -friend class EL734Axis; - private: - asynUser *pasynUserController_; + friend class EL734Axis; - asynStatus transactController(int axis, char command[COMLEN], char reply[COMLEN]); +private: + asynUser *pasynUserController_; - void switchRemote(); + asynStatus transactController(int axis, char command[COMLEN], char reply[COMLEN]); + void switchRemote(); };