diff --git a/Makefile.RHEL8 b/Makefile.RHEL8 index 9f0713d..62ede3e 100644 --- a/Makefile.RHEL8 +++ b/Makefile.RHEL8 @@ -13,7 +13,7 @@ REQUIRED+=scaler REQUIRED+=asynMotor # Release version -LIBVERSION=2024-dev +LIBVERSION=2024-newPmacV3 # DB files to include in the release TEMPLATES += sinqEPICSApp/Db/dimetix.db @@ -38,6 +38,9 @@ SOURCES += sinqEPICSApp/src/pmacController.cpp SOURCES += sinqEPICSApp/src/MasterMACSDriver.cpp SOURCES += sinqEPICSApp/src/C804Axis.cpp SOURCES += sinqEPICSApp/src/C804Controller.cpp +SOURCES += sinqEPICSApp/src/newPmacV3Axis.cpp +SOURCES += sinqEPICSApp/src/newPmacV3Controller.cpp +SOURCES += sinqEPICSApp/src/pmacController.cpp USR_CFLAGS += -Wall -Wextra # -Werror diff --git a/sinqEPICSApp/src/C804Axis.cpp b/sinqEPICSApp/src/C804Axis.cpp index acbd396..03fda0b 100644 --- a/sinqEPICSApp/src/C804Axis.cpp +++ b/sinqEPICSApp/src/C804Axis.cpp @@ -46,9 +46,9 @@ asynStatus C804Axis::poll(bool *moving) { // Local variable declaration static const char *functionName = "C804Axis::poll"; - // The poll function is just a wrapper around poll_no_param_lib_update and + // The poll function is just a wrapper around pollNoUpdate and // handles mainly the callParamCallbacks() function - asynStatus status_poll = C804Axis::poll_no_param_lib_update(moving); + asynStatus status_poll = C804Axis::pollNoUpdate(moving); // According to the function documentation of asynMotorAxis::poll, this // function should be called at the end of a poll implementation. @@ -64,7 +64,7 @@ asynStatus C804Axis::poll(bool *moving) { } // Perform the actual poll -asynStatus C804Axis::poll_no_param_lib_update(bool *moving) { +asynStatus C804Axis::pollNoUpdate(bool *moving) { // Local variable declaration static const char *functionName = "C804Axis::poll"; asynStatus status; diff --git a/sinqEPICSApp/src/C804Axis.h b/sinqEPICSApp/src/C804Axis.h index 6e5bdc9..0e4eddd 100644 --- a/sinqEPICSApp/src/C804Axis.h +++ b/sinqEPICSApp/src/C804Axis.h @@ -1,40 +1,43 @@ #ifndef C804Axis_H #define C804Axis_H -#include "SINQController.h" #include "SINQAxis.h" +#include "SINQController.h" // Forward declaration of the controller class to resolve the cyclic dependency -// between C804Controller.h and C804Axis.h. See https://en.cppreference.com/w/cpp/language/class. +// between C804Controller.h and C804Axis.h. See +// https://en.cppreference.com/w/cpp/language/class. class C804Controller; -class C804Axis : public SINQAxis -{ -public: - /* These are the methods we override from the base class */ - C804Axis(C804Controller *pController, int axisNo); - virtual ~C804Axis(); - asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); - asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration); - asynStatus stop(double acceleration); - asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards); - asynStatus poll(bool *moving); - asynStatus poll_no_param_lib_update(bool *moving); - asynStatus enable(int on); +class C804Axis : public SINQAxis { + public: + /* These are the methods we override from the base class */ + C804Axis(C804Controller *pController, int axisNo); + virtual ~C804Axis(); + asynStatus move(double position, int relative, double min_velocity, + double max_velocity, double acceleration); + asynStatus moveVelocity(double min_velocity, double max_velocity, + double acceleration); + asynStatus stop(double acceleration); + asynStatus home(double minVelocity, double maxVelocity, double acceleration, + int forwards); + asynStatus poll(bool *moving); + asynStatus pollNoUpdate(bool *moving); + asynStatus enable(int on); -protected: - C804Controller *pC_; + protected: + C804Controller *pC_; - void checkBounds(C804Controller *pController, int axisNo); - int last_position_steps_; - double motorRecResolution_; - time_t estimatedArrivalTime_; - time_t last_poll_; - int errorReported_; - bool enabled_; + void checkBounds(C804Controller *pController, int axisNo); + int last_position_steps_; + double motorRecResolution_; + time_t estimatedArrivalTime_; + time_t last_poll_; + int errorReported_; + bool enabled_; -private: - friend class C804Controller; + private: + friend class C804Controller; }; #endif diff --git a/sinqEPICSApp/src/newPmacV3Axis.cpp b/sinqEPICSApp/src/newPmacV3Axis.cpp index ccb1f01..80c60b2 100644 --- a/sinqEPICSApp/src/newPmacV3Axis.cpp +++ b/sinqEPICSApp/src/newPmacV3Axis.cpp @@ -1,4 +1,5 @@ #include "newPmacV3Axis.h" +#include "asynOctetSyncIO.h" #include "newPmacV3Controller.h" #include #include @@ -11,6 +12,7 @@ newPmacV3Axis::newPmacV3Axis(newPmacV3Controller *pC, int axisNo) : asynMotorAxis(pC, axisNo), pC_(pC) { static const char *functionName = "newPmacV3Axis::newPmacV3Axis"; + asynStatus status = asynSuccess; /* The superclass constructor SINQAxis calls in turn its superclass constructor @@ -36,8 +38,47 @@ newPmacV3Axis::newPmacV3Axis(newPmacV3Controller *pC, int axisNo) // Initialize all member variables initial_poll_ = true; + waitForHandshake_ = false; + + // Wait 10 seconds for the handshake until declaring a timeout + handshakeTimeout_ = 10; + + // Placeholder, is overwritten later time_at_init_poll_ = 0; - timeout_param_lib_init_ = 10; + + // After 3 idle polls, the parameter library definitely had enough time to + // be initialized + timeout_param_lib_init_ = 3 * pC->idlePollPeriod_; + + // Provide initial values for some parameter library entries + status = pC_->setIntegerParam(axisNo_, pC_->rereadEncoderPosition_, 0); + if (status != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: Setting an initial parameter library value " + "for rereadEncoderPosition_ in axis %d. Terminating IOC.", + functionName, axisNo_); + exit(-1); + } + + status = pC_->setDoubleParam(axisNo_, pC_->motorPosition_, 0.0); + if (status != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: Setting an initial parameter library value " + "for motorPosition_ in axis %d. Terminating IOC.", + functionName, axisNo_); + exit(-1); + } + + // This value is updated in the poll. Initially, we assume that the motor is + // not enabled. + status = pC_->setIntegerParam(axisNo_, pC_->motorEnabled_, 0); + if (status != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: Setting an initial parameter library value " + "for motorPosition_ in axis %d. Terminating IOC.", + functionName, axisNo_); + exit(-1); + } } newPmacV3Axis::~newPmacV3Axis(void) { @@ -45,16 +86,22 @@ newPmacV3Axis::~newPmacV3Axis(void) { // clean up the pointer pC here. } -// Read the configuration from the motor control unit and the parameter library +/* +Read the configuration from the motor control unit and the parameter library. +This operation is only allowed if the motor is not moving +*/ asynStatus newPmacV3Axis::readConfig() { + // Local variable declaration static const char *functionName = "newPmacV3Axis::readConfig"; asynStatus status = asynSuccess; char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; int nvals = 0; - double positive_limit = 0.0; - double negative_limit = 0.0; + double highLimit = 0.0; + double lowLimit = 0.0; double motorRecResolution = 0.0; + double position = 0.0; + int axStatus = 0; // ========================================================================= @@ -64,32 +111,75 @@ asynStatus newPmacV3Axis::readConfig() { if (status == asynParamUndefined) { return asynParamUndefined; } else if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorRecResolution_"); + return pC_->paramLibAccessFailed(status, functionName, + "motorRecResolution_"); } - // Software limits - snprintf(command, sizeof(command), "Q%2.2d08 Q%2.2d09", axisNo_, axisNo_); - status = pC_->writeRead(axisNo_, command, response); - nvals = sscanf(response, "%lf %lf", &positive_limit, &negative_limit); - if (pC_->checkNumExpectedReads(2, nvals, functionName, axisNo_) != - asynSuccess) { + // Software limits and current position + snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10 Q%2.2d13 Q%2.2d14", + axisNo_, axisNo_, axisNo_, axisNo_); + status = pC_->writeRead(axisNo_, command, response, true); + nvals = sscanf(response, "%d %lf %lf %lf", &axStatus, &position, &highLimit, + &lowLimit); + if (pC_->checkNumExpectedReads(4, nvals, functionName, command, response, + axisNo_) != asynSuccess) { + return asynError; + } + + // If the motor is not in idle status, do not read the configuration + if (axStatus != 0) { return asynError; } // Transform from motor to user coordinates - positive_limit = positive_limit * motorRecResolution; - negative_limit = negative_limit * motorRecResolution; + position = position * motorRecResolution; + highLimit = highLimit * motorRecResolution; + lowLimit = lowLimit * motorRecResolution; + + /* + The axis limits are set as: ({[]}) + where [] are the positive and negative limits set in EPICS/NICOS, {} are the + software limits set on the MCU and () are the hardware limit switches. In + other words, the EPICS/NICOS limits must be stricter than the software + limits on the MCU which in turn should be stricter than the hardware limit + switches. For example, if the hardware limit switches are at [-10, 10], the + software limits could be at [-9, 9] and the EPICS / NICOS limits could be at + [-8, 8]. Therefore, we cannot use the software limits read from the MCU + directly, but need to shrink them a bit. In this case, we're shrinking them + by 0.1 mm or 0.1 degree (depending on the axis type) on both sides. + */ + highLimit = highLimit - 0.1; + lowLimit = lowLimit + 0.1; // Store these values in the parameter library - status = pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, positive_limit); + status = pC_->setDoubleParam(axisNo_, pC_->motorPosition_, position); if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorHighLimit_"); + return pC_->paramLibAccessFailed(status, functionName, + "motorPosition_"); } - status = pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, negative_limit); + status = pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, lowLimit); if (status != asynSuccess) { - return pC_->paramLibAccessFailed(status, "motorLowLimit_"); + return pC_->paramLibAccessFailed(status, functionName, + "motorLowLimit_"); } - return asynSuccess; + status = pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, highLimit); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, functionName, + "motorHighLimit_"); + } + + // Update the parameter library immediately + status = callParamCallbacks(); + if (status != asynSuccess) { + // If we can't communicate with the parameter library, it doesn't make + // sense to try and upstream this to the user -> Just log the error + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Updating the parameter library failed for axis %d\n", + functionName, axisNo_); + return status; + } + + return this->readEncoderType(); } asynStatus newPmacV3Axis::poll(bool *moving) { @@ -138,22 +228,24 @@ asynStatus newPmacV3Axis::poll(bool *moving) { } } - // The poll function is just a wrapper around poll_no_param_lib_update and + // The poll function is just a wrapper around pollNoUpdate and // handles mainly the callParamCallbacks() function. This wrapper is used // to make sure callParamCallbacks() is called in case of a premature // return. - poll_status = newPmacV3Axis::poll_no_param_lib_update(moving); + poll_status = newPmacV3Axis::pollNoUpdate(moving); // If the poll status is ok, reset the error indicators in the parameter // library if (poll_status == asynSuccess) { pl_status = setIntegerParam(pC_->motorStatusProblem_, false); if (pl_status != asynSuccess) { - pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_"); + pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusProblem_"); } pl_status = setIntegerParam(pC_->motorStatusCommsError_, false); if (pl_status != asynSuccess) { - pC_->paramLibAccessFailed(pl_status, "motorStatusCommsError_"); + pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusCommsError_"); } } @@ -173,7 +265,7 @@ asynStatus newPmacV3Axis::poll(bool *moving) { } // Perform the actual poll -asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { +asynStatus newPmacV3Axis::pollNoUpdate(bool *moving) { // Return value for the poll asynStatus poll_status = asynSuccess; @@ -190,43 +282,88 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { int direction = 0; int error = 0; - int ax_status = 0; - double current_position = 0.0; - double previous_position = 0.0; - int low_limit_reached = 0; - int high_limit_reached = 0; + int axStatus = 0; + double currentPosition = 0.0; + double previousPosition = 0.0; double motorRecResolution = 0.0; + int handshakePerformed = 0; // ========================================================================= + // Are we currently waiting for a handshake? + if (waitForHandshake_) { + snprintf(command, sizeof(command), "P%2.2d23", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + + nvals = sscanf(response, "%d", &handshakePerformed); + if (pC_->checkNumExpectedReads(1, nvals, functionName, command, + response, axisNo_) != asynSuccess) { + return asynError; + } + + if (handshakePerformed == 1) { + // Handshake has been performed successfully -> Continue with the + // poll + waitForHandshake_ = false; + } else { + // Still waiting for the handshake. This is already part of the + // movement procedure! + if (time(NULL) < timeAtHandshake_ + handshakeTimeout_) { + *moving = true; + return asynSuccess; + } else { + // Timed out when waiting for the handshake + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Axis %d timed out when waiting for the " + "handshake with the MCU.\n", + functionName, axisNo_); + pl_status = setStringParam( + pC_->messageText_, "Handshake timed out. This is a bug."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + waitForHandshake_ = false; + return asynError; + } + } + } + // Motor resolution from parameter library pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, &motorRecResolution); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorRecResolution_"); } // Read the previous motor position pl_status = - pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &previous_position); + pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &previousPosition); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorPosition_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorPosition_"); } // Check the axis status (Pxx00) and the current motor position (Qxx10) - snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10", axisNo_, axisNo_); - rw_status = pC_->writeRead(axisNo_, command, response); - nvals = sscanf(response, "%d %lf", &ax_status, ¤t_position); - if (pC_->checkNumExpectedReads(2, nvals, functionName, axisNo_) != - asynSuccess) { + snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10 P%2.2d01", axisNo_, + axisNo_, axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + + nvals = sscanf(response, "%d %lf %d", &axStatus, ¤tPosition, &error); + if (pC_->checkNumExpectedReads(3, nvals, functionName, command, response, + axisNo_) != asynSuccess) { return asynError; } - // Change the position values from motor to user coordinates. - current_position = current_position * motorRecResolution; - // Intepret the status - switch (ax_status) { + switch (axStatus) { case -6: asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, "%s: Axis %d is stopping\n", functionName, axisNo_); @@ -237,6 +374,12 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { "%s: Axis %d is deactivated\n", functionName, axisNo_); *moving = false; + pl_status = setStringParam(pC_->messageText_, "Deactivated"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + // No further evaluation of the axis status is necessary return asynSuccess; case -4: @@ -246,6 +389,12 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { functionName); *moving = false; + pl_status = setStringParam(pC_->messageText_, "Emergency stop"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + // No further evaluation of the axis status is necessary return asynSuccess; case -3: @@ -253,6 +402,12 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { "%s: Axis %d is inhibited\n", functionName, axisNo_); *moving = false; + pl_status = setStringParam(pC_->messageText_, "Disabled"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + // No further evaluation of the axis status is necessary return asynSuccess; case 0: @@ -260,31 +415,58 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { "%s: Axis %d is ready for movement\n", functionName, axisNo_); *moving = false; break; + case 1: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Move order for %d acknowledged\n", functionName, + axisNo_); + *moving = true; + break; + case 2: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Move order for %d is possible\n", functionName, axisNo_); + *moving = true; + break; + case 3: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: %d in Air Cushion Outout status\n", functionName, + axisNo_); + *moving = true; + break; + case 4: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: %d in Air Cushion Input status\n", functionName, + axisNo_); + *moving = true; + break; case 5: asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, "%s: Axis %d is moving\n", functionName, axisNo_); *moving = true; break; + default: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Reached unreachable state P%2.2d00 = %d.\n", + functionName, axisNo_, axStatus); + pl_status = + setStringParam(pC_->messageText_, + "Unreachable state has been reached. This is a bug"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + *moving = false; } if (*moving) { // If the axis is moving, evaluate the movement direction - if ((current_position - previous_position) > 0) { + if ((currentPosition - previousPosition) > 0) { direction = 1; } else { direction = 0; } } - // Read the axis error - snprintf(command, sizeof(command), "P%2.2d01", axisNo_); - rw_status = pC_->writeRead(axisNo_, command, response); - nvals = sscanf(response, "%d", &error); - if (pC_->checkNumExpectedReads(1, nvals, functionName, axisNo_) != - asynSuccess) { - return asynError; - } - + // Error handling switch (error) { case 0: // No error @@ -292,19 +474,19 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { case 1: // EPICS should already prevent this issue in the first place, // since it contains the user limits - asynPrint( - pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Target position would exceed user limits in axis %d. EPICS " - "should prevent an out-of-bounds target position, this error " - "therefore indicates a bug.\n", - functionName, axisNo_); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Target position would exceed user limits in axis " + "%d. EPICS should prevent an out-of-bounds target " + "position, this is a bug.\n", + functionName, axisNo_); pl_status = setStringParam(pC_->messageText_, "Target position would exceed software limits. This " - "is a bug, please contact software support"); + "is a bug."); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; @@ -313,92 +495,42 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { // Command not possible asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Axis %d is still moving, but received another move " - "command. EPICS should prevent this, this error therefore " - "indicates a bug.\n", + "command. EPICS should prevent this, this is a bug.\n", functionName, axisNo_); - pl_status = setStringParam( - pC_->messageText_, - "Axis received move command while it is still moving. This is a " - "bug, please contact software support"); + pl_status = setStringParam(pC_->messageText_, + "Axis received move command while it is " + "still moving. This is a bug."); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; break; case 10: - // Limits reached - snprintf(command, sizeof(command), "M%2.2d21 M%2.2d22", axisNo_, - axisNo_); - rw_status = pC_->writeRead(axisNo_, command, response); - nvals = - sscanf(response, "%d %d", &high_limit_reached, &low_limit_reached); - if (pC_->checkNumExpectedReads(2, nvals, functionName, axisNo_) != - asynSuccess) { - return asynError; - } - - if (high_limit_reached && !low_limit_reached) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, - "%s: Axis %d reached its high limit.\n", functionName, - axisNo_); - - pl_status = setStringParam(pC_->messageText_, "High limit hit"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); - } - - } else if (!high_limit_reached && low_limit_reached) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, - "%s: Axis %d reached its low limit.\n", functionName, - axisNo_); - - pl_status = setStringParam(pC_->messageText_, "Low limit hit"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); - } - - } else if (high_limit_reached && low_limit_reached) { - // If the execution has hit this branch, both high and low - // limit are apparently hit at the same time. This most likely - // indicates an error in the configuration! - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Axis %d reached both its higher and lower limit.\n", - functionName, axisNo_); - - pl_status = setStringParam(pC_->messageText_, - "Both high and low limit hit"); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); - } - - poll_status = asynError; - } else { - // If the execution has hit this branch, neither upper nor lower - // limit has been reached, but the MCU states that limits have been - // reached. This is definitely a bug! - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Axis %d states that it has hit its limits, but at " - "the same time states that neither high or low limit " - "have been hit. This is a bug.\n", - functionName, axisNo_); - - snprintf(command, sizeof(command), - "Limits have been hit and not hit at the " - "same time " - "(P%2.2d01 = 10). This is a bug, please " - "contact Software " - "support.", - axisNo_); - pl_status = setStringParam(pC_->messageText_, command); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); - } - - poll_status = asynError; + /* + Software limits of the controller have been hit. Since the EPICS limits + are derived from the software limits and are a little bit smaller, this + error case can only happen if either the axis has an incremental encoder + which is not properly homed or if a bug occured. + */ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "%s: Axis %d hit the controller limits. Try homing the axis, " + "if that doesn't remove the error, this is a bug.\n", + functionName, axisNo_); + + snprintf(command, sizeof(command), + "Software limits hit (P%2.2d01 = %d). Try homing the motor. " + "If that doesn't work, contact the support", + axisNo_, error); + pl_status = setStringParam(pC_->messageText_, command); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } + poll_status = asynError; break; case 11: // Following error @@ -412,25 +544,26 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { axisNo_); pl_status = setStringParam(pC_->messageText_, command); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; break; case 13: // Watchdog of the controller final stage triggered - asynPrint( - pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Controller final stage watchdog triggered for axis %d.\n", - functionName, axisNo_); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Driver hardware error triggered for axis %d.\n", + functionName, axisNo_); snprintf(command, sizeof(command), - "Controller final stage watchdog triggered (P%2.2d01 = 13). " + "Driver hardware error (P%2.2d01 = 13). " "Please contact Electronics support.", axisNo_); pl_status = setStringParam(pC_->messageText_, command); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; @@ -440,16 +573,17 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { // since it contains the user limits asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Target position would exceed hardware limits in axis " - "%d. EPICS should prevent an out-of-bounds target position, " - "this error therefore indicates a bug.\n", + "%d. Homing might be necessary.\n", functionName, axisNo_); - pl_status = setStringParam( - pC_->messageText_, - "Controller final stage watchdog triggered (Pxx01 = 13). " - "Please contact Electronics support."); + snprintf(command, sizeof(command), + "Move command exceeds hardware limits (P%2.2d01 = %d). Try " + "homing the motor. If that doesn't work, contact the support", + axisNo_, error); + pl_status = setStringParam(pC_->messageText_, command); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; @@ -461,9 +595,10 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { pl_status = setStringParam(pC_->messageText_, "Axis reached an unreachable state. Please " - "contact software support"); + "contact electronics support"); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "messageText_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); } poll_status = asynError; @@ -474,44 +609,50 @@ asynStatus newPmacV3Axis::poll_no_param_lib_update(bool *moving) { if (error != 0) { pl_status = setIntegerParam(pC_->motorStatusProblem_, true); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusProblem_"); } } - pl_status = setIntegerParam(pC_->motorEnabled_, (ax_status != -3)); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorEnabled_"); + if (*moving == false) { + pl_status = setIntegerParam(pC_->motorMoveToHome_, 0); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorMoveToHome_"); + } } - pl_status = setIntegerParam(pC_->motorStatusHighLimit_, high_limit_reached); + pl_status = + setIntegerParam(pC_->motorEnabled_, (axStatus != -3 && axStatus != -5)); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusHighLimit_"); - } - - pl_status = setIntegerParam(pC_->motorStatusLowLimit_, low_limit_reached); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusLowLimit_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorEnabled_"); } pl_status = setIntegerParam(pC_->motorStatusMoving_, *moving); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusMoving_"); } pl_status = setIntegerParam(pC_->motorStatusDone_, !(*moving)); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusDone_"); } pl_status = setIntegerParam(pC_->motorStatusDirection_, direction); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusDirection_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusDirection_"); } - pl_status = setDoubleParam(pC_->motorPosition_, current_position); + pl_status = setDoubleParam(pC_->motorPosition_, currentPosition); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorPosition_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorPosition_"); } + return poll_status; } @@ -527,8 +668,7 @@ asynStatus newPmacV3Axis::move(double position, int relative, static const char *functionName = "newPmacV3Axis::move"; char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; - double motor_coordinates_position = 0.0; - double motor_position = 0.0; + double motorCoordinatesPosition = 0.0; int enabled = 0; double motorRecResolution = 0.0; @@ -536,13 +676,15 @@ asynStatus newPmacV3Axis::move(double position, int relative, pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnabled_, &enabled); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorEnabled_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorEnabled_"); } pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, &motorRecResolution); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorRecResolution_"); } if (enabled == 0) { @@ -551,52 +693,43 @@ asynStatus newPmacV3Axis::move(double position, int relative, return asynSuccess; } - // Use only absolute values - if (relative == 1) { - - pl_status = - pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &motor_position); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorPosition_"); - } - - motor_position = motor_position + position; - } - // Convert from user to motor units - motor_coordinates_position = motor_position / motorRecResolution; + motorCoordinatesPosition = position / motorRecResolution; - // Set the axis speed - snprintf(command, sizeof(command), "Q%d04=%lf", axisNo_, maxVelocity); - rw_status = pC_->writeRead(axisNo_, command, response); - if (rw_status != asynSuccess) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Setting the velocity failed for axis %d\n", functionName, - axisNo_); - pl_status = setIntegerParam(pC_->motorStatusProblem_, true); - if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_"); - } - - return rw_status; - } + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Start of axis %d to position %lf\n", functionName, axisNo_, + position); // Perform handshake, Set target position and start the move command - snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d01=%12.4f M%2.2d=1", - axisNo_, axisNo_, motor_coordinates_position, axisNo_); - rw_status = pC_->writeRead(axisNo_, command, response); + + if (relative) { + snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d02=%lf M%2.2d=2", + axisNo_, axisNo_, motorCoordinatesPosition, axisNo_); + } else { + snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d01=%lf M%2.2d=1", + axisNo_, axisNo_, motorCoordinatesPosition, axisNo_); + } + + // We don't expect an answer + rw_status = pC_->writeRead(axisNo_, command, response, false); if (rw_status != asynSuccess) { asynPrint( pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Starting movement to target position %lf failed for axis %d\n", - functionName, motor_position, axisNo_); + functionName, position, axisNo_); pl_status = setIntegerParam(pC_->motorStatusProblem_, true); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_"); + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorStatusProblem_"); } return rw_status; } + // In the next poll, we will check if the handshake has been performed in a + // reasonable time + waitForHandshake_ = true; + timeAtHandshake_ = time(NULL); + return rw_status; } @@ -614,7 +747,7 @@ asynStatus newPmacV3Axis::stop(double acceleration) { // ========================================================================= - pl_status = poll_no_param_lib_update(&moving); + pl_status = pollNoUpdate(&moving); if (pl_status != asynSuccess) { return pl_status; } @@ -622,7 +755,7 @@ asynStatus newPmacV3Axis::stop(double acceleration) { if (moving) { // only send a stop when actually moving snprintf(command, sizeof(command), "M%2.2d=8", axisNo_); - rw_status = pC_->writeRead(axisNo_, command, response); + rw_status = pC_->writeRead(axisNo_, command, response, false); if (rw_status != asynSuccess) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, @@ -631,10 +764,227 @@ asynStatus newPmacV3Axis::stop(double acceleration) { pl_status = setIntegerParam(pC_->motorStatusProblem_, true); if (pl_status != asynSuccess) { - return pC_->paramLibAccessFailed(pl_status, + return pC_->paramLibAccessFailed(pl_status, functionName, "motorStatusProblem_"); } } } return rw_status; -} \ No newline at end of file +} + +/* +Home the axis. On absolute encoder systems, this is a no-op +*/ +asynStatus newPmacV3Axis::home(double min_velocity, double max_velocity, + double acceleration, int forwards) { + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rw_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + static const char *functionName = "newPmacV3Axis::home"; + + // ========================================================================= + + pl_status = + pC_->getStringParam(axisNo_, pC_->encoderType_, pC_->MAXBUF_, response); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "encoderType_"); + } + + // Only send the home command if the axis has an incremental encoder + if (strcmp(response, IncrementalEncoder) == 0) { + snprintf(command, sizeof(command), "M%2.2d=9", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, false); + if (rw_status != asynSuccess) { + return rw_status; + } + + pl_status = setIntegerParam(pC_->motorMoveToHome_, 1); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "motorMoveToHome_"); + } + + pl_status = setStringParam(pC_->messageText_, "Homing"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + } + + return rw_status; +} + +/* +Read the encoder type and update the parameter library accordingly +*/ +asynStatus newPmacV3Axis::readEncoderType() { + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rw_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + static const char *functionName = "newPmacV3Axis::readEncoderType"; + int nvals = 0; + int encoder_id = 0; + char encoderType[pC_->MAXBUF_]; + + // ========================================================================= + + // Check if this is an absolute encoder + snprintf(command, sizeof(command), "I%2.2X04", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + + int reponse_length = strlen(response); + if (reponse_length < 3) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Unexpected reponse '%s' from axis %d on " + "controller %s while reading the encoder type. Aborting...\n", + functionName, response, axisNo_, pC_->portName); + return asynError; + } + + // We are only interested in the last two digits and the last value in + // the string before the terminator is \r + nvals = sscanf(response + (reponse_length - 3), "%2X", &encoder_id); + if (pC_->checkNumExpectedReads(1, nvals, functionName, command, response, + axisNo_) != asynSuccess) { + return asynError; + } + + snprintf(command, sizeof(command), "P46"); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + int number_of_axes = strtol(response, NULL, 10); + + // If true, the encoder is incremental + if (encoder_id <= number_of_axes) { + pl_status = setStringParam(pC_->encoderType_, IncrementalEncoder); + } else { + pl_status = setStringParam(pC_->encoderType_, AbsoluteEncoder); + } + + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "encoderType_"); + } + return asynSuccess; +} + +asynStatus newPmacV3Axis::enable(int on) { + + static const char *functionName = "newPmacV3Axis::enable"; + int ax_status = 0; + int timeout_enable_disable = 2; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + int nvals = 0; + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rw_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + // ========================================================================= + + // Check if the axis is currently enabled + snprintf(command, sizeof(command), "P%2.2d00", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + + nvals = sscanf(response, "%d", &ax_status); + if (pC_->checkNumExpectedReads(1, nvals, functionName, command, response, + axisNo_) != asynSuccess) { + return asynError; + } + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Axis status %d, new input %d \n", functionName, ax_status, + on); + + // Axis is already enabled / disabled and a new enable / disable command + // was sent => Do nothing + if ((ax_status != -3) == on) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "%s: axis %d on controller %s is already %s\n", functionName, + axisNo_, pC_->portName, on ? "enabled" : "disabled"); + return asynSuccess; + } + + // Enable / disable the axis + snprintf(command, sizeof(command), "M%2.2d14=%d", axisNo_, on); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: %s axis %d on controller %s\n", functionName, + on ? "Enable" : "Disable", axisNo_, pC_->portName); + if (on == 0) { + pl_status = setStringParam(pC_->messageText_, "Disabling ..."); + } else { + pl_status = setStringParam(pC_->messageText_, "Enabling ..."); + } + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + rw_status = pC_->writeRead(axisNo_, command, response, false); + if (rw_status != asynSuccess) { + return rw_status; + } + + // Query the axis status every few milliseconds until the axis has been + // enabled or until the timeout has been reached + snprintf(command, sizeof(command), "P%2.2d00", axisNo_); + int startTime = time(NULL); + while (time(NULL) < startTime + timeout_enable_disable) { + + // Read the axis status + usleep(100000); + rw_status = pC_->writeRead(axisNo_, command, response, true); + if (rw_status != asynSuccess) { + return rw_status; + } + nvals = sscanf(response, "%d", &ax_status); + if (pC_->checkNumExpectedReads(1, nvals, functionName, command, + response, axisNo_) != asynSuccess) { + return asynError; + } + + if ((ax_status != -3) == on) { + bool moving = false; + // Perform a poll to update the parameter library + poll(&moving); + return asynSuccess; + } + } + + // Failed to change axis status within timeout_enable_disable => Send a + // corresponding message + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Failed to %s axis %d on controller %s within %d seconds\n", + functionName, on ? "enable" : "disable", axisNo_, pC_->portName, + timeout_enable_disable); + + // Output message to user + snprintf(command, sizeof(command), "Failed to %s within %d seconds", + on ? "enable" : "disable", timeout_enable_disable); + pl_status = setStringParam(pC_->messageText_, "Enabling ..."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, functionName, + "messageText_"); + } + return asynError; +} diff --git a/sinqEPICSApp/src/newPmacV3Axis.h b/sinqEPICSApp/src/newPmacV3Axis.h index e3e2330..332f38c 100644 --- a/sinqEPICSApp/src/newPmacV3Axis.h +++ b/sinqEPICSApp/src/newPmacV3Axis.h @@ -14,15 +14,13 @@ class newPmacV3Axis : public asynMotorAxis { virtual ~newPmacV3Axis(); asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); - // asynStatus moveVelocity(double min_velocity, double max_velocity, - // double acceleration); asynStatus stop(double acceleration); - // asynStatus home(double minVelocity, double maxVelocity, double - // acceleration, - // int forwards); + asynStatus home(double minVelocity, double maxVelocity, double acceleration, + int forwards); asynStatus poll(bool *moving); - asynStatus poll_no_param_lib_update(bool *moving); - // asynStatus enable(int on); + asynStatus pollNoUpdate(bool *moving); + asynStatus enable(int on); + asynStatus readEncoderType(); protected: newPmacV3Controller *pC_; @@ -30,6 +28,9 @@ class newPmacV3Axis : public asynMotorAxis { void checkBounds(newPmacV3Controller *pController, int axisNo); asynStatus readConfig(); bool initial_poll_; + bool waitForHandshake_; + time_t timeAtHandshake_; + time_t handshakeTimeout_; time_t time_at_init_poll_; time_t timeout_param_lib_init_; diff --git a/sinqEPICSApp/src/newPmacV3Controller.cpp b/sinqEPICSApp/src/newPmacV3Controller.cpp index e389a24..868baed 100644 --- a/sinqEPICSApp/src/newPmacV3Controller.cpp +++ b/sinqEPICSApp/src/newPmacV3Controller.cpp @@ -85,7 +85,7 @@ newPmacV3Controller::newPmacV3Controller(const char *portName, // NICOS and in turn to the user status = createParam("MOTOR_MESSAGE_TEXT", asynParamOctet, &messageText_); if (status != asynSuccess) { - paramLibAccessFailed(status, "messageText_"); + paramLibAccessFailed(status, functionName, "messageText_"); asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%s: FATAL ERROR: unable to create parameter (%s). " "Terminating IOC.\n", @@ -111,6 +111,15 @@ newPmacV3Controller::newPmacV3Controller(const char *portName, exit(-1); } + status = createParam("ENCODER_TYPE", asynParamOctet, &encoderType_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: unable to create parameter (%s). " + "Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } + status = createParam("REREAD_ENCODER_POSITION", asynParamInt32, &rereadEncoderPosition_); if (status != asynSuccess) { @@ -140,6 +149,16 @@ newPmacV3Controller::newPmacV3Controller(const char *portName, exit(-1); } + status = + createParam("MOTOR_POSITION_RBV", asynParamFloat64, &motorPositionRBV_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: FATAL ERROR: unable to create parameter (%s). " + "Terminating IOC.\n", + functionName, stringifyAsynStatus(status)); + exit(-1); + } + /* Define the end-of-string of a message coming from the device to EPICS. It is not necessary to append a terminator to outgoing messages, since @@ -230,14 +249,14 @@ Sends the given command to the axis specified by axisNo and returns the response of the axis. */ asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command, - char *response) { + char *response, + bool expect_response) { // Definition of local variables. static const char *functionName = "newPmacV3Controller::writeRead"; asynStatus status = asynSuccess; asynStatus pl_status = asynSuccess; char full_command[MAXBUF_] = {0}; char user_message[MAXBUF_] = {0}; - int motorStatusCommsError = 0; int motorStatusProblem = 0; // Send the message and block the thread until either a response has been @@ -294,15 +313,48 @@ asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command, "%s: Sending command: %s\n", functionName, full_command); // Perform the actual writeRead - // status = - // pasynOctetSyncIO->write(lowLevelPortUser_, full_command, - // commandLength + offset, TIMEOUT_, - // &nbytesOut); - status = pasynOctetSyncIO->writeRead( lowLevelPortUser_, full_command, commandLength + offset, response, MAXBUF_, TIMEOUT_, &nbytesOut, &nbytesIn, &eomReason); + /* + If we expect a response, check if we got one. If no response was received, + flush the PMAC and try again. If that fails as well, return an error + */ + if (expect_response && strlen(response) == 0) { + // Flush message as defined in Turbo PMAC User Manual, p. 430: + // \x40\xB3000 + // VR_DOWNLOAD = \x40 + // VR_PMAC_FLUSH = \xB3 + char flush_msg[5] = {0}; + flush_msg[0] = '\x40'; + flush_msg[1] = '\xB3'; + size_t nbytesOut = 0; + status = pasynOctetSyncIO->write(lowLevelPortUser_, flush_msg, 5, + TIMEOUT_, &nbytesOut); + + // Wait after the flush so the MCU has time to prepare for the + // next command + usleep(100000); + + if (status == asynSuccess) { + // If flushing the MCU succeded, try to send the command again + status = pasynOctetSyncIO->writeRead( + lowLevelPortUser_, full_command, commandLength + offset, + response, MAXBUF_, TIMEOUT_, &nbytesOut, &nbytesIn, &eomReason); + + // If the command returned an empty string for the second time, give + // up and propagate the error. + if (strlen(response) == 0) { + status = asynError; + } + } else { + asynPrint(pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Unable to flush MCU (%s).\n", functionName, + stringifyAsynStatus(status)); + } + } + // Create custom error messages for different failure modes switch (status) { case asynSuccess: @@ -312,22 +364,19 @@ asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command, "connection timeout for axis %d", axisNo); break; case asynDisconnected: - snprintf(user_message, sizeof(user_message), "axis %d is not connected", - axisNo); + snprintf(user_message, sizeof(user_message), "axis is not connected"); break; case asynDisabled: - snprintf(user_message, sizeof(user_message), "axis %d is disabled", - axisNo); + snprintf(user_message, sizeof(user_message), "axis is disabled"); break; default: snprintf(user_message, sizeof(user_message), - "Communication with axis %d failed (%s)", axisNo, - stringifyAsynStatus(status)); + "Communication failed (%s)", stringifyAsynStatus(status)); break; } if (status != asynSuccess) { - // Check if the axis aleady is in an error communication mode. If it is + // Check if the axis already is in an error communication mode. If it is // not, upstream the error. This is done to avoid "flooding" the user // with different error messages if more than one error ocurred before // an error-free communication @@ -335,13 +384,15 @@ asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command, pl_status = getIntegerParam(axisNo, motorStatusProblem_, &motorStatusProblem); if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "motorStatusProblem_"); + return paramLibAccessFailed(pl_status, functionName, + "motorStatusProblem_"); } if (motorStatusProblem == 0) { pl_status = axis->setStringParam(this->messageText_, user_message); if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "messageText_"); + return paramLibAccessFailed(pl_status, functionName, + "messageText_"); } } } @@ -360,8 +411,10 @@ asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command, } if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "motorStatusCommsError_"); + return paramLibAccessFailed(pl_status, functionName, + "motorStatusCommsError_"); } + return asynSuccess; } @@ -370,10 +423,7 @@ asynStatus newPmacV3Controller::writeInt32(asynUser *pasynUser, int function = pasynUser->reason; asynStatus status = asynSuccess; - char command[MAXBUF_] = {0}; - char response[MAXBUF_] = {0}; static const char *functionName = "newPmacV3Controller::writeInt32"; - int nvals = 0; // ========================================================================= @@ -385,92 +435,12 @@ asynStatus newPmacV3Controller::writeInt32(asynUser *pasynUser, // Handle custom PVs if (function == enableMotor_) { - - int ax_status = 0; - asynStatus pl_status = asynSuccess; - int timeout_enable_disable = 2; - char message[MAXBUF_] = {0}; - - // ===================================================================== - - // Check if the axis is currently enabled - snprintf(command, sizeof(command), "P%2.2d00", axis->axisNo_); - status = writeRead(axis->axisNo_, command, response); - nvals = sscanf(response, "%d", &ax_status); - if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != - asynSuccess) { - return asynError; - } - - // Axis is already enabled / disabled and a new enable / disable command - // was sent => Do nothing - if ((ax_status != -3) == value) { - return asynSuccess; - } - - // Enable / disable the axis - snprintf(command, sizeof(command), "M%2.2d14=%d", axis->axisNo_, value); - asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, - "%s: %s axis %d on controller %s\n", functionName, - value ? "Enable" : "Disable", axis->axisNo_, portName); - if (value == 0) { - pl_status = setStringParam(messageText_, "Disabling ..."); - } else { - pl_status = setStringParam(messageText_, "Enabling ..."); - } - if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "messageText_"); - } - status = writeRead(axis->axisNo_, command, response); - if (status != asynSuccess) { - return status; - } - - // Query the axis status every few milliseconds until the axis has been - // enabled or until the timeout has been reached - snprintf(command, sizeof(command), "P%2.2d00", axis->axisNo_); - int startTime = time(NULL); - while (time(NULL) < startTime + timeout_enable_disable) { - - // Read the axis status - usleep(100); - status = writeRead(axis->axisNo_, command, response); - if (status != asynSuccess) { - return status; - } - nvals = sscanf(response, "%d", &ax_status); - if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != - asynSuccess) { - return asynError; - } - - if ((ax_status != -3) == value) { - bool moving = false; - // Perform a poll to update the parameter library - axis->poll(&moving); - return asynSuccess; - } - } - - // Failed to change axis status within timeout_enable_disable => Send a - // corresponding message - asynPrint( - this->pasynUserSelf, ASYN_TRACE_FLOW, - "%s: Failed to %s axis %d on controller %s within %d seconds\n", - functionName, value ? "enable" : "disable", axis->axisNo_, portName, - timeout_enable_disable); - - // Output message to user - snprintf(command, sizeof(command), "Failed to %s within %d seconds", - value ? "enable" : "disable", timeout_enable_disable); - pl_status = setStringParam(messageText_, "Enabling ..."); - if (pl_status != asynSuccess) { - return paramLibAccessFailed(pl_status, "messageText_"); - } - return asynError; + return axis->enable(value); } else if (function == rereadEncoderPosition_) { + char encoderType[MAXBUF_] = {0}; + /* This is not a command that can always be run when enabling a motor as it also causes relative encoders to reread a position @@ -488,68 +458,56 @@ asynStatus newPmacV3Controller::writeInt32(asynUser *pasynUser, // Poll the current status of the axis bool moving = false; - axis->poll(&moving); + status = axis->poll(&moving); + if (status != asynSuccess) { + return status; + } // Check if this is an absolute encoder - snprintf(command, sizeof(command), "I%2.2X04", axis->axisNo_); - status = writeRead(axis->axisNo_, command, response); + status = axis->readEncoderType(); if (status != asynSuccess) { return status; } - int reponse_length = strlen(response); - if (reponse_length < 3) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Unexpected reponse '%s' from axis %d on " - "controller %s while rereading encoder. Aborting...\n", - functionName, response, axis->axisNo_, portName); - return asynError; - } - - // We are only interested in the last two digits and the last value in - // the string before the terminator is \r - int encoder_id = 0; - nvals = sscanf(response + (reponse_length - 3), "%2X", &encoder_id); - if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != - asynSuccess) { - return asynError; - } - - snprintf(command, sizeof(command), "P46"); - status = writeRead(axis->axisNo_, command, response); + status = getStringParam(axis->axisNo_, encoderType_, encoderType); if (status != asynSuccess) { - return status; + return paramLibAccessFailed(status, functionName, "encoderType_"); } - int number_of_axes = strtol(response, NULL, 10); - if (encoder_id <= number_of_axes) { - asynPrint( - this->pasynUserSelf, ASYN_TRACE_ERROR, - "%s: Trying to reread absolute encoder of axis %d on " - "controller %s, but it is a relative encoder. Aborting...\n", - functionName, axis->axisNo_, portName); + // Abort if the axis is incremental + if (strcmp(encoderType, IncrementalEncoder) == 0) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_WARNING, + "%s: Trying to reread absolute encoder of axis %d on " + "controller %s, but it is a relative encoder.\n", + functionName, axis->axisNo_, portName); + status = setStringParam(messageText_, + "Cannot reread an incremental encoder."); + if (status != asynSuccess) { + return paramLibAccessFailed(status, functionName, + "messageText_"); + } return asynError; } - // Check if the axis is disabled. If not, inform the user that this is - // necessary + // Check if the axis is disabled. If not, inform the user that this + // is necessary int enabled = 0; status = getIntegerParam(axis->axisNo_, motorEnabled_, &enabled); if (status != asynSuccess) { - return paramLibAccessFailed(status, "motorEnabled_"); + return paramLibAccessFailed(status, functionName, "motorEnabled_"); } if (enabled == 1) { - asynPrint( - this->pasynUserSelf, ASYN_TRACE_WARNING, - "%s: Trying to reread absolute encoder of axis %d on " - "controller %s, but it is a relative encoder. Aborting...\n", - functionName, axis->axisNo_, portName); + asynPrint(this->pasynUserSelf, ASYN_TRACE_WARNING, + "%s: Axis %d on controller %s must be disabled before " + "rereading the encoder.\n", + functionName, axis->axisNo_, portName); status = setStringParam( messageText_, "Axis must be disabled before rereading the encoder."); if (status != asynSuccess) { - return paramLibAccessFailed(status, "messageText_"); + return paramLibAccessFailed(status, functionName, + "messageText_"); } return asynError; } @@ -564,7 +522,8 @@ asynStatus newPmacV3Controller::writeInt32(asynUser *pasynUser, // though status = axis->setIntegerParam(rereadEncoderPosition_, 0); if (status != asynSuccess) { - return paramLibAccessFailed(status, "rereadEncoderPosition_"); + return paramLibAccessFailed(status, functionName, + "rereadEncoderPosition_"); } return asynSuccess; @@ -581,11 +540,9 @@ and "rereading the encoder" must be covered. asynStatus newPmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) { - int function = pasynUser->reason, axStat; + int function = pasynUser->reason; asynStatus status = asynError; static const char *functionName = "newPmacV3Controller::readInt32"; - char command[MAXBUF_]; - char response[MAXBUF_]; // ===================================================================== @@ -595,42 +552,22 @@ asynStatus newPmacV3Controller::readInt32(asynUser *pasynUser, return asynError; } - if (function == motorEnabled_) { - // Readback value for motorEnabled - - snprintf(command, sizeof(command), "P%2.2d00", axis->axisNo_); - status = this->writeRead(axis->axisNo_, command, response); - if (status != asynSuccess) { - return status; - } - - int nvals = sscanf(response, "%d", value); - if (checkNumExpectedReads(1, nvals, functionName, axis->axisNo_) != - asynSuccess) { - return asynError; - } - status = setIntegerParam(motorEnabled_, (*value != -3)); - if (status != asynSuccess) { - return paramLibAccessFailed(status, "motorEnabled_"); - } - return callParamCallbacks(); // Update the PVs from the parameter - // library - - } else if (function == rereadEncoderPositionRBV_) { + if (function == rereadEncoderPositionRBV_) { // Readback value for rereadEncoderPosition - status = getIntegerParam(axis->axisNo_, rereadEncoderPosition_, value); if (status != asynSuccess) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "HERE\n"); - return paramLibAccessFailed(status, "rereadEncoderPosition_"); + return paramLibAccessFailed(status, functionName, + "rereadEncoderPosition_"); } status = setIntegerParam(axis->axisNo_, rereadEncoderPositionRBV_, *value); if (status != asynSuccess) { - return paramLibAccessFailed(status, "rereadEncoderPositionRBV_"); + return paramLibAccessFailed(status, functionName, + "rereadEncoderPositionRBV_"); } - return callParamCallbacks(); // Update the PVs from the parameter - // library + + // Update the PVs from the parameter library + return callParamCallbacks(); } else { return asynMotorController::readInt32(pasynUser, value); } @@ -676,6 +613,7 @@ const char *newPmacV3Controller::stringifyAsynStatus(asynStatus status) { } asynStatus newPmacV3Controller::paramLibAccessFailed(asynStatus status, + const char *functionName, const char *parameter) { char message[MAXBUF_] = {0}; snprintf(message, sizeof(message), @@ -684,23 +622,27 @@ asynStatus newPmacV3Controller::paramLibAccessFailed(asynStatus status, parameter, stringifyAsynStatus(status)); // Log the error message and try to propagate it - asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, message); + asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, "%s: %s", functionName, + message); setStringParam(messageText_, message); return status; } asynStatus newPmacV3Controller::checkNumExpectedReads(int expected, int read, const char *functionName, + const char *command, + const char *response, int axisNo_) { if (expected == read) { return asynSuccess; } else { char message[MAXBUF_] = {0}; snprintf(message, sizeof(message), - "Could not interpret controller response in function %s for " - "axis %d. This is a bug, please inform the software support.", - functionName, axisNo_); - asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, message); + "Could not interpret response %s for command %s (axis %d). " + "This is a bug.", + response, command, axisNo_); + asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, "%s: %s", functionName, + message); setStringParam(messageText_, message); setIntegerParam(motorStatusCommsError_, 1); return asynError; @@ -868,4 +810,4 @@ epicsExportRegistrar(newPmacV3ControllerRegister); #endif -} // extern "C" \ No newline at end of file +} // extern "C" diff --git a/sinqEPICSApp/src/newPmacV3Controller.h b/sinqEPICSApp/src/newPmacV3Controller.h index 4f2ffe3..0d6fc23 100644 --- a/sinqEPICSApp/src/newPmacV3Controller.h +++ b/sinqEPICSApp/src/newPmacV3Controller.h @@ -12,6 +12,9 @@ #include "asynMotorController.h" #include "newPmacV3Axis.h" +#define IncrementalEncoder "Incremental encoder" +#define AbsoluteEncoder "Absolute encoder" + class newPmacV3Controller : public asynMotorController { public: @@ -31,17 +34,25 @@ class newPmacV3Controller : public asynMotorController { // overloaded because we want to read the axis state asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value); + // // Overloaded to read configuration details from the motor records + // asynStatus drvUserCreate(asynUser *pasynUser, const char *drvInfo, + // const char **pptypeName, size_t *psize); + protected: asynUser *lowLevelPortUser_; - asynStatus writeRead(int axisNo, const char *command, char *response); + asynStatus writeRead(int axisNo, const char *command, char *response, + bool expect_response); // Create a descriptive string out of an asynStatus which can be used for // logging or communicating with the user const char *stringifyAsynStatus(asynStatus status); - asynStatus paramLibAccessFailed(asynStatus status, const char *parameter); + asynStatus paramLibAccessFailed(asynStatus status, const char *functionName, + const char *parameter); asynStatus checkNumExpectedReads(int expected, int read, - const char *function, int axisNo_); + const char *functionName, + const char *command, const char *response, + int axisNo_); newPmacV3Axis *castToAxis(asynMotorAxis *asynAxis); private: @@ -58,14 +69,24 @@ class newPmacV3Controller : public asynMotorController { */ static const double TIMEOUT_; + // Indices of additional PVs int messageText_; int enableMotor_; int motorEnabled_; int rereadEncoderPosition_; int rereadEncoderPositionRBV_; int readConfig_; + int encoderType_; + int motorPositionRBV_; + + /* + If the time between two sent messages is too short, the MCU communication + module might "lose" an answer. To prevent this, a small delay is introduced + in EPICS after each message exchange. Unit is microseconds. + */ + int afterMessageSleep_; friend class newPmacV3Axis; }; -#endif /* pmacV3Controller_H */ \ No newline at end of file +#endif /* pmacV3Controller_H */