PositionDeadband, dynamic limit detection, velocity mode

Integrated a readout function for the position deadband. Also read from the controller if the axis has dynamic limits and only poll the limits repeatedly if that is the case. Lastly, added support for the velocity mode (untested!).
2026-01-20 15:09:51 +01:00
parent f3f0a77f10
commit fd41d4c9c0
4 changed files with 301 additions and 98 deletions
--- a/src/masterMacsAxis.cpp
+++ b/src/masterMacsAxis.cpp
@@ -23,6 +23,7 @@ struct masterMacsAxisImpl {
    time_t timeAtHandshake;
    bool needInit = true;
    bool targetReachedUninitialized;
+    bool dynamicLimits;
 };

 /*
@@ -92,6 +93,7 @@ masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo)
          .waitForHandshake = false,
          .timeAtHandshake = 0,
          .targetReachedUninitialized = true,
+          .dynamicLimits = false,
      })) {

    asynStatus status = asynSuccess;
@@ -179,14 +181,17 @@ Read out the following values:
 asynStatus masterMacsAxis::init() {

    // Local variable declaration
-    asynStatus pl_status = asynSuccess;
+    asynStatus status = asynSuccess;
    char response[pC_->MAXBUF_] = {0};
    int nvals = 0;
-    double motorRecResolution = 0.0;
-    double motorPosition = 0.0;
-    double motorVelocity = 0.0;
-    double motorVmax = 0.0;
-    double motorAccel = 0.0;
+    double motRecResolution = 0.0;
+    double motPosition = 0.0;
+    double motPositionDeadband = 0.0;
+    double motVelocity = 0.0;
+    double motVmax = 0.0;
+    double motAccel = 0.0;
+    double motMode = 0;
+    double motCanSetMode = 0;

    // =========================================================================

@@ -195,9 +200,9 @@ asynStatus masterMacsAxis::init() {
    time_t now = time(NULL);
    time_t maxInitTime = 60;
    while (1) {
-        pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
-                                        &motorRecResolution);
-        if (pl_status == asynParamUndefined) {
+        status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
+                                     &motRecResolution);
+        if (status == asynParamUndefined) {
            if (now + maxInitTime < time(NULL)) {
                asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                          "Controller \"%s\", axis %d  => %s, line "
@@ -206,10 +211,10 @@ asynStatus masterMacsAxis::init() {
                          __LINE__);
                return asynError;
            }
-        } else if (pl_status == asynSuccess) {
+        } else if (status == asynSuccess) {
            break;
-        } else if (pl_status != asynSuccess) {
-            return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
+        } else if (status != asynSuccess) {
+            return pC_->paramLibAccessFailed(status, "motorRecResolution_",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }
@@ -220,73 +225,130 @@ asynStatus masterMacsAxis::init() {
    setAxisParamChecked(this, motorConnected, false);

    // Read out the current position
-    pl_status = pC_->read(axisNo_, 12, response);
-    if (pl_status != asynSuccess) {
-        return pl_status;
+    status = pC_->read(axisNo_, 12, response);
+    if (status != asynSuccess) {
+        return status;
    }
-    nvals = sscanf(response, "%lf", &motorPosition);
+    nvals = sscanf(response, "%lf", &motPosition);
    if (nvals != 1) {
        return pC_->couldNotParseResponse("R12", response, axisNo_,
                                          __PRETTY_FUNCTION__, __LINE__);
    }
+    status = setMotorPosition(motPosition);
+    if (status != asynSuccess) {
+        return status;
+    }

    // Read out the current velocity
-    pl_status = pC_->read(axisNo_, 05, response);
-    if (pl_status != asynSuccess) {
-        return pl_status;
+    status = pC_->read(axisNo_, 05, response);
+    if (status != asynSuccess) {
+        return status;
    }
-    nvals = sscanf(response, "%lf", &motorVelocity);
+    nvals = sscanf(response, "%lf", &motVelocity);
    if (nvals != 1) {
        return pC_->couldNotParseResponse("R05", response, axisNo_,
                                          __PRETTY_FUNCTION__, __LINE__);
    }

    // Read out the maximum velocity
-    pl_status = pC_->read(axisNo_, 26, response);
-    if (pl_status != asynSuccess) {
-        return pl_status;
+    status = pC_->read(axisNo_, 26, response);
+    if (status != asynSuccess) {
+        return status;
    }
-    nvals = sscanf(response, "%lf", &motorVmax);
+    nvals = sscanf(response, "%lf", &motVmax);
    if (nvals != 1) {
        return pC_->couldNotParseResponse("R26", response, axisNo_,
                                          __PRETTY_FUNCTION__, __LINE__);
    }

-    // Read out the acceleration
-    pl_status = pC_->read(axisNo_, 06, response);
-    if (pl_status != asynSuccess) {
-        return pl_status;
+    status = setVeloFields(motVelocity, 0.0, motVmax);
+    if (status != asynSuccess) {
+        return status;
    }
-    nvals = sscanf(response, "%lf", &motorAccel);
+
+    // Read out the acceleration
+    status = pC_->read(axisNo_, 06, response);
+    if (status != asynSuccess) {
+        return status;
+    }
+    nvals = sscanf(response, "%lf", &motAccel);
    if (nvals != 1) {
        return pC_->couldNotParseResponse("R06", response, axisNo_,
                                          __PRETTY_FUNCTION__, __LINE__);
    }
-
-    // Store the motor position in the parameter library
-    pl_status = setMotorPosition(motorPosition);
-    if (pl_status != asynSuccess) {
-        return pl_status;
+    status = setAcclField(motAccel);
+    if (status != asynSuccess) {
+        return status;
    }

-    // Write to the motor record fields
-    pl_status = setVeloFields(motorVelocity, 0.0, motorVmax);
-    if (pl_status != asynSuccess) {
-        return pl_status;
+    // Read out the motor position deadband
+    status = pC_->read(axisNo_, 13, response);
+    if (status != asynSuccess) {
+        return status;
    }
-    pl_status = setAcclField(motorAccel);
-    if (pl_status != asynSuccess) {
-        return pl_status;
+    nvals = sscanf(response, "%lf", &motPositionDeadband);
+    if (nvals != 1) {
+        return pC_->couldNotParseResponse("R13", response, axisNo_,
+                                          __PRETTY_FUNCTION__, __LINE__);
+    }
+    setAxisParamChecked(this, motorPositionDeadband, motPositionDeadband);
+
+    // Check if the motor has dynamic limits
+    status = pC_->read(axisNo_, 32, response);
+    if (status != asynSuccess) {
+        return status;
+    }
+    nvals = sscanf(response, "%d", &pMasterMacsA_->dynamicLimits);
+    if (nvals != 1) {
+        return pC_->couldNotParseResponse("R32", response, axisNo_,
+                                          __PRETTY_FUNCTION__, __LINE__);
    }

-    pl_status = readEncoderType();
-    if (pl_status != asynSuccess) {
-        return pl_status;
+    // Check the current motor mode
+    status = pC_->read(axisNo_, 07, response);
+    if (status != asynSuccess) {
+        return status;
+    }
+    nvals = sscanf(response, "%d", &motMode);
+    if (nvals != 1) {
+        return pC_->couldNotParseResponse("R07", response, axisNo_,
+                                          __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // If the readback value from the controller is 3, it is in velocity mode,
+    // which sinqMotor encodes as a 1. Otherwise, it is in position mode.
+    setAxisParamChecked(this, motorMode, motMode == 3);
+
+    // Check if the motor can switch its mode
+    status = pC_->read(axisNo_, 31, response);
+    if (status != asynSuccess) {
+        return status;
+    }
+    nvals = sscanf(response, "%d", &motCanSetMode);
+    if (nvals != 1) {
+        return pC_->couldNotParseResponse("R31", response, axisNo_,
+                                          __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // If the readback value from the controller is 3, the motor supports both
+    // velocity and position mode, otherwise just one of them (the one read out
+    // with motMode).
+    setAxisParamChecked(this, motorCanSetMode, motCanSetMode == 3);
+
+    status = readEncoderType();
+    if (status != asynSuccess) {
+        return status;
+    }
+
+    // Read the axis limits
+    status = readLimits();
+    if (status != asynSuccess) {
+        return status;
    }

    // Update the parameter library immediately
-    pl_status = callParamCallbacks();
-    if (pl_status != asynSuccess) {
+    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
@@ -294,14 +356,71 @@ asynStatus masterMacsAxis::init() {
                  "Controller \"%s\", axis %d => %s, line "
                  "%d:\ncallParamCallbacks failed with %s.\n",
                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
-                  pC_->stringifyAsynStatus(pl_status));
-        return pl_status;
+                  pC_->stringifyAsynStatus(status));
+        return status;
    }

    // Axis is fully initialized
    setNeedInit(false);

-    return pl_status;
+    return status;
+}
+
+asynStatus masterMacsAxis::readLimits() {
+
+    asynStatus status = asynSuccess;
+
+    char response[pC_->MAXBUF_] = {0};
+    int nvals = 0;
+
+    double highLimit = 0.0;
+    double lowLimit = 0.0;
+    double limitsOffset = 0.0;
+
+    // =========================================================================
+
+    status = pC_->read(axisNo_, 34, response);
+    if (status != asynSuccess) {
+        return status;
+    }
+    nvals = sscanf(response, "%lf", &lowLimit);
+    if (nvals != 1) {
+        return pC_->couldNotParseResponse("R34", response, axisNo_,
+                                          __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    status = pC_->read(axisNo_, 33, response);
+    if (status != asynSuccess) {
+        return status;
+    }
+    nvals = sscanf(response, "%lf", &highLimit);
+    if (nvals != 1) {
+        return pC_->couldNotParseResponse("R33", response, axisNo_,
+                                          __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    /*
+    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.
+    */
+    getAxisParamChecked(this, motorLimitsOffset, &limitsOffset);
+
+    highLimit = highLimit - limitsOffset;
+    lowLimit = lowLimit + limitsOffset;
+
+    setAxisParamChecked(this, motorHighLimitFromDriver, highLimit);
+    setAxisParamChecked(this, motorLowLimitFromDriver, lowLimit);
+
+    return status;
 }

 // Perform the actual poll
@@ -324,9 +443,6 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
    double currentPosition = 0.0;
    double previousPosition = 0.0;
    double motorRecResolution = 0.0;
-    double highLimit = 0.0;
-    double lowLimit = 0.0;
-    double limitsOffset = 0.0;
    double handshakePerformed = 0;

    // =========================================================================
@@ -614,48 +730,13 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
        pC_->getMsgPrintControl().resetCount(keyError, pC_->pasynUser());
    }

-    // Read out the limits, if the motor is not moving
-    if (!(*moving)) {
-        rw_status = pC_->read(axisNo_, 34, response);
+    // Read out the limits, if the motor is not moving and if the limits are
+    // dynamic
+    if (pMasterMacsA_->dynamicLimits && !(*moving)) {
+        rw_status = readLimits();
        if (rw_status != asynSuccess) {
            return rw_status;
        }
-        nvals = sscanf(response, "%lf", &lowLimit);
-        if (nvals != 1) {
-            return pC_->couldNotParseResponse("R34", response, axisNo_,
-                                              __PRETTY_FUNCTION__, __LINE__);
-        }
-
-        rw_status = pC_->read(axisNo_, 33, response);
-        if (rw_status != asynSuccess) {
-            return rw_status;
-        }
-        nvals = sscanf(response, "%lf", &highLimit);
-        if (nvals != 1) {
-            return pC_->couldNotParseResponse("R33", response, axisNo_,
-                                              __PRETTY_FUNCTION__, __LINE__);
-        }
-
-        /*
-        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.
-        */
-        getAxisParamChecked(this, motorLimitsOffset, &limitsOffset);
-
-        highLimit = highLimit - limitsOffset;
-        lowLimit = lowLimit + limitsOffset;
-
-        setAxisParamChecked(this, motorHighLimitFromDriver, highLimit);
-        setAxisParamChecked(this, motorLowLimitFromDriver, lowLimit);
    }

    // Update the enable PV
@@ -686,6 +767,62 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
    return poll_status;
 }

+asynStatus masterMacsAxis::doMoveVelocity(double minVelocity,
+                                          double maxVelocity,
+                                          double acceleration) {
+    // Suppress unused variable warning
+    (void)minVelocity;
+    (void)acceleration;
+
+    // Status of read-write-operations of ASCII commands to the controller
+    asynStatus status = asynSuccess;
+
+    char command[pC_->MAXBUF_];
+
+    double motorRecResolution = 0.0;
+    double motorVelocity = 0.0;
+    int enabled = 0;
+
+    // =========================================================================
+
+    getAxisParamChecked(this, motorEnableRBV, &enabled);
+    getAxisParamChecked(this, motorRecResolution, &motorRecResolution);
+
+    if (enabled == 0) {
+        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
+                  "Controller \"%s\", axis %d => %s, line %d:\nAxis is "
+                  "disabled.\n",
+                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
+        return asynSuccess;
+    }
+
+    // Convert from EPICS to user / motor units
+    motorVelocity = maxVelocity * motorRecResolution;
+
+    snprintf(command, sizeof(command), "%lf", motorVelocity);
+    status = pC_->write(axisNo_, 05, command);
+    if (status != asynSuccess) {
+        setAxisParamChecked(this, motorStatusProblem, true);
+        return status;
+    }
+
+    asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW,
+              "Controller \"%s\", axis %d => %s, line %d:\nSetting speed "
+              "to %lf.\n",
+              pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
+              motorVelocity);
+
+    double timeout = pC_->comTimeout();
+    if (pMasterMacsA_->targetReachedUninitialized &&
+        timeout < PowerCycleTimeout) {
+        timeout = PowerCycleTimeout;
+    }
+
+    // Start the move. We do not use the MovTimeout watchdog here, because the
+    // motor can move for any time in velocity mode.
+    return pC_->write(axisNo_, 00, "3", timeout);
+}
+
 asynStatus masterMacsAxis::doMove(double position, int relative,
                                  double minVelocity, double maxVelocity,
                                  double acceleration) {
@@ -697,7 +834,7 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
    // Status of read-write-operations of ASCII commands to the controller
    asynStatus status = asynSuccess;

-    char value[pC_->MAXBUF_];
+    char command[pC_->MAXBUF_];
    double motorCoordinatesPosition = 0.0;
    double motorRecResolution = 0.0;
    double motorVelocity = 0.0;
@@ -740,8 +877,8 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
    // motor speed changed since the last move command.
    if (motorCanSetSpeed != 0) {

-        snprintf(value, sizeof(value), "%lf", motorVelocity);
-        status = pC_->write(axisNo_, 05, value);
+        snprintf(command, sizeof(command), "%lf", motorVelocity);
+        status = pC_->write(axisNo_, 05, command);
        if (status != asynSuccess) {
            setAxisParamChecked(this, motorStatusProblem, true);
            return status;
@@ -755,14 +892,14 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
    }

    // Set the target position
-    snprintf(value, sizeof(value), "%lf", motorCoordinatesPosition);
-    status = pC_->write(axisNo_, 02, value);
+    snprintf(command, sizeof(command), "%lf", motorCoordinatesPosition);
+    status = pC_->write(axisNo_, 02, command);
    if (status != asynSuccess) {
        setAxisParamChecked(this, motorStatusProblem, true);
        return status;
    }

-    // If the motor has just been enabled, use Enable
+    // If the motor has just been enabled, use a longer timeout for starting
    double timeout = pC_->comTimeout();
    if (pMasterMacsA_->targetReachedUninitialized &&
        timeout < PowerCycleTimeout) {
--- a/src/masterMacsAxis.h
+++ b/src/masterMacsAxis.h
@@ -51,6 +51,19 @@ class HIDDEN masterMacsAxis : public sinqAxis {
     */
    asynStatus doPoll(bool *moving);

+    /**
+     * @brief Implementation of the `doMoveVelocity` function from sinqAxis. The
+     * parameters are described in the documentation of
+     * `sinqAxis::doMoveVelocity`.
+     *
+     * @param minVelocity
+     * @param maxVelocity
+     * @param acceleration
+     * @return asynStatus
+     */
+    asynStatus doMoveVelocity(double minVelocity, double maxVelocity,
+                              double acceleration);
+
    /**
     * @brief Implementation of the `doMove` function from sinqAxis. The
     * parameters are described in the documentation of `sinqAxis::doMove`.
@@ -154,6 +167,12 @@ class HIDDEN masterMacsAxis : public sinqAxis {
     */
    asynStatus readAxisStatus();

+    /**
+     * @brief Read the upper and lower limits and store them in the parameter
+     * library.
+     */
+    asynStatus readLimits();
+
    /*
    The functions below read the specified status bit from the axisStatus (see
    masterMacsAxisImpl redefinition in masterMacsAxis.cpp) bitset. Since a bit
--- a/src/masterMacsController.cpp
+++ b/src/masterMacsController.cpp
@@ -570,7 +570,45 @@ asynStatus masterMacsController::readInt32(asynUser *pasynUser,
        *value = 1;
        return asynSuccess;
    } else {
-        return asynMotorController::readInt32(pasynUser, value);
+        return sinqController::readInt32(pasynUser, value);
+    }
+}
+
+asynStatus masterMacsController::writeInt32(asynUser *pasynUser,
+                                            epicsInt32 value) {
+    // masterMacs can be disabled
+    if (pasynUser->reason == motorSetMode()) {
+
+        // First call to the sinqController function. It checks whether it is
+        // possible to set the mode and whether the given value is valid.
+        asynStatus status = sinqController::writeInt32(pasynUser, value);
+        if (status == asynSuccess) {
+
+            // Now write to the hardware
+            char command[MAXBUF_];
+            int axisNo;
+            getAddress(pasynUser, &axisNo);
+
+            // Map the EPICS value to MasterMACS values (see
+            // MasterMACS_manual.pdf).
+            int adjustedValue = 0;
+            if (value == 0) {
+                adjustedValue = 1;
+            } else if (value == 1) {
+                adjustedValue = 3;
+            } else {
+                // This branch is unreachable, as it is is already checked
+                // within sinqController::writeInt32 that value is either 0
+                // or 1.
+                return asynError;
+            }
+
+            snprintf(command, sizeof(value), "%d", adjustedValue);
+            return write(axisNo, 07, command);
+        }
+        return status;
+    } else {
+        return sinqController::writeInt32(pasynUser, value);
    }
 }

--- a/src/masterMacsController.h
+++ b/src/masterMacsController.h
@@ -54,6 +54,15 @@ class HIDDEN masterMacsController : public sinqController {
     */
    asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);

+    /**
+     * @brief Overloaded version of the sinqController version
+     *
+     * @param pasynUser
+     * @param value
+     * @return asynStatus
+     */
+    asynStatus writeInt32(asynUser *pasynUser, epicsInt32 *value);
+
    /**
     * @brief Get the axis object
     *