Implemented velocity mode, but didn't test it yet

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!).

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,18 @@ 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 motRecResolution = 0.0;
-    double motorPosition = 0.0;
+    double motPosition = 0.0;
-    double motorVelocity = 0.0;
+    double motPositionDeadband = 0.0;
-    double motorVmax = 0.0;
+    double motVelocity = 0.0;
-    double motorAccel = 0.0;
+    double motVmax = 0.0;
+    double motAccel = 0.0;
+    int motMode = 0;
+    int dynamicLimits = 0;
+    int motCanSetMode = 0;
 
     // =========================================================================
 
@@ -195,9 +201,9 @@ asynStatus masterMacsAxis::init() {
     time_t now = time(NULL);
     time_t maxInitTime = 60;
     while (1) {
-        pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
+        status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
-                                        &motorRecResolution);
+                                     &motRecResolution);
-        if (pl_status == asynParamUndefined) {
+        if (status == asynParamUndefined) {
             if (now + maxInitTime < time(NULL)) {
                 asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                           "Controller \"%s\", axis %d  => %s, line "
@@ -206,10 +212,10 @@ asynStatus masterMacsAxis::init() {
                           __LINE__);
                 return asynError;
             }
-        } else if (pl_status == asynSuccess) {
+        } else if (status == asynSuccess) {
             break;
-        } else if (pl_status != asynSuccess) {
+        } else if (status != asynSuccess) {
-            return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
+            return pC_->paramLibAccessFailed(status, "motorRecResolution_",
                                              axisNo_, __PRETTY_FUNCTION__,
                                              __LINE__);
         }
@@ -220,73 +226,131 @@ asynStatus masterMacsAxis::init() {
     setAxisParamChecked(this, motorConnected, false);
 
     // Read out the current position
-    pl_status = pC_->read(axisNo_, 12, response);
+    status = pC_->read(axisNo_, 12, response);
-    if (pl_status != asynSuccess) {
+    if (status != asynSuccess) {
-        return pl_status;
+        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);
+    status = pC_->read(axisNo_, 05, response);
-    if (pl_status != asynSuccess) {
+    if (status != asynSuccess) {
-        return pl_status;
+        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);
+    status = pC_->read(axisNo_, 26, response);
-    if (pl_status != asynSuccess) {
+    if (status != asynSuccess) {
-        return pl_status;
+        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
+    status = setVeloFields(motVelocity, 0.0, motVmax);
-    pl_status = pC_->read(axisNo_, 06, response);
+    if (status != asynSuccess) {
-    if (pl_status != asynSuccess) {
+        return status;
-        return pl_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__);
     }
-
+    status = setAcclField(motAccel);
-    // Store the motor position in the parameter library
+    if (status != asynSuccess) {
-    pl_status = setMotorPosition(motorPosition);
+        return status;
-    if (pl_status != asynSuccess) {
-        return pl_status;
     }
 
-    // Write to the motor record fields
+    // Read out the motor position deadband
-    pl_status = setVeloFields(motorVelocity, 0.0, motorVmax);
+    status = pC_->read(axisNo_, 13, response);
-    if (pl_status != asynSuccess) {
+    if (status != asynSuccess) {
-        return pl_status;
+        return status;
     }
-    pl_status = setAcclField(motorAccel);
+    nvals = sscanf(response, "%lf", &motPositionDeadband);
-    if (pl_status != asynSuccess) {
+    if (nvals != 1) {
-        return pl_status;
+        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", &dynamicLimits);
+    if (nvals != 1) {
+        return pC_->couldNotParseResponse("R32", response, axisNo_,
+                                          __PRETTY_FUNCTION__, __LINE__);
+    }
+    pMasterMacsA_->dynamicLimits = bool(dynamicLimits);
+
+    // 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__);
     }
 
-    pl_status = readEncoderType();
+    // If the readback value from the controller is 3, it is in velocity mode,
-    if (pl_status != asynSuccess) {
+    // which sinqMotor encodes as a 1. Otherwise, it is in position mode.
-        return pl_status;
+    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();
+    status = callParamCallbacks();
-    if (pl_status != asynSuccess) {
+    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 +358,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));
+                  pC_->stringifyAsynStatus(status));
-        return pl_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 +445,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 +732,13 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
         pC_->getMsgPrintControl().resetCount(keyError, pC_->pasynUser());
     }
 
-    // Read out the limits, if the motor is not moving
+    // Read out the limits, if the motor is not moving and if the limits are
-    if (!(*moving)) {
+    // dynamic
-        rw_status = pC_->read(axisNo_, 34, response);
+    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 +769,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 +836,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 +879,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);
+        snprintf(command, sizeof(command), "%lf", motorVelocity);
-        status = pC_->write(axisNo_, 05, value);
+        status = pC_->write(axisNo_, 05, command);
         if (status != asynSuccess) {
             setAxisParamChecked(this, motorStatusProblem, true);
             return status;
@@ -755,14 +894,14 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
     }
 
     // Set the target position
-    snprintf(value, sizeof(value), "%lf", motorCoordinatesPosition);
+    snprintf(command, sizeof(command), "%lf", motorCoordinatesPosition);
-    status = pC_->write(axisNo_, 02, value);
+    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) {
@@ -1039,6 +1178,28 @@ asynStatus masterMacsAxis::enable(bool on) {
     return asynError;
 }
 
+asynStatus masterMacsAxis::setMode(int mode) {
+
+    char command[pC_->MAXBUF_] = {0};
+
+    // Map the EPICS value to MasterMACS values (see
+    // MasterMACS_manual.pdf).
+    int adjustedMode = 0;
+    if (mode == 0) {
+        adjustedMode = 1;
+    } else if (mode == 1) {
+        adjustedMode = 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(command), "%d", adjustedMode);
+    return pC_->write(axisNo(), 07, command);
+}
+
 bool masterMacsAxis::needInit() { return pMasterMacsA_->needInit; }
 
 /**

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`.
@@ -117,6 +130,15 @@ class HIDDEN masterMacsAxis : public sinqAxis {
      */
     asynStatus enable(bool on);
 
+    /**
+     * @brief Write the new operation mode (position or velocity) to the
+     * MasterMACS controller.
+     *
+     * @param mode
+     * @return asynStatus
+     */
+    asynStatus setMode(int mode);
+
     /**
      * @brief Read the encoder type (incremental or absolute) for this axis
      * from the MCU and store the information in the PV ENCODER_TYPE.
@@ -154,6 +176,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

src/masterMacsController.cpp

@@ -570,7 +570,7 @@ asynStatus masterMacsController::readInt32(asynUser *pasynUser,
         *value = 1;
         return asynSuccess;
     } else {
-        return asynMotorController::readInt32(pasynUser, value);
+        return sinqController::readInt32(pasynUser, value);
     }
 }
 

src/masterMacsController.h

@@ -75,7 +75,8 @@ class HIDDEN masterMacsController : public sinqController {
     /**
      * @brief Overloaded function of sinqController
      *
-     * The function is overloaded to allow resetting the node
+     * The function is overloaded to allow resetting the node and changing the
+     * operation mode.
      *
      * @param pasynUser           Specify the axis via the asynUser
      * @param value               New value