Simplified handling of velocity mode using standard EPICS motor record fields

src/masterMacsAxis.cpp

@@ -11,6 +11,11 @@
 #include <string.h>
 #include <unistd.h>
 
+enum moveMode {
+    positionMode,
+    velocityMode,
+};
+
 struct masterMacsAxisImpl {
     /*
     The axis status and axis error of MasterMACS are given as an integer from
@@ -24,6 +29,9 @@ struct masterMacsAxisImpl {
     bool needInit = true;
     bool targetReachedUninitialized;
     bool dynamicLimits;
+    bool canPositionMove;
+    bool canVelocityMove;
+    moveMode lastMoveCommand;
 };
 
 /*
@@ -94,6 +102,9 @@ masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo)
           .timeAtHandshake = 0,
           .targetReachedUninitialized = true,
           .dynamicLimits = false,
+          .canPositionMove = true,
+          .canVelocityMove = false,
+          .lastMoveCommand = positionMode,
       })) {
 
     asynStatus status = asynSuccess;
@@ -190,9 +201,8 @@ asynStatus masterMacsAxis::init() {
     double motVelocity = 0.0;
     double motVmax = 0.0;
     double motAccel = 0.0;
-    int motMode = 0;
     int dynamicLimits = 0;
-    int motCanSetMode = 0;
+    int possibleModes = 0;
 
     // =========================================================================
 
@@ -306,36 +316,38 @@ asynStatus masterMacsAxis::init() {
     }
     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__);
-    }
-
-    // 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);
+    nvals = sscanf(response, "%d", &possibleModes);
     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);
+    switch (possibleModes) {
+    case 1:
+        pMasterMacsA_->canPositionMove = true;
+        pMasterMacsA_->canVelocityMove = false;
+        break;
+    case 2:
+        pMasterMacsA_->canPositionMove = false;
+        pMasterMacsA_->canVelocityMove = true;
+        break;
+    case 3:
+        pMasterMacsA_->canPositionMove = true;
+        pMasterMacsA_->canVelocityMove = true;
+        break;
+    default:
+        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
+                  "Controller \"%s\", axis %d => %s, line "
+                  "%d:\nunexpected answer %d for R31 (possible operation "
+                  "modes). Expected one of 1, 2 or 3.\n",
+                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
+                  possibleModes);
+    }
 
     status = readEncoderType();
     if (status != asynSuccess) {
@@ -432,10 +444,10 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
     asynStatus poll_status = asynSuccess;
 
     // Status of read-write-operations of ASCII commands to the controller
-    asynStatus rw_status = asynSuccess;
+    asynStatus rwStatus = asynSuccess;
 
     // Status of parameter library operations
-    asynStatus pl_status = asynSuccess;
+    asynStatus plStatus = asynSuccess;
 
     char response[pC_->MAXBUF_] = {0};
     int nvals = 0;
@@ -446,6 +458,7 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
     double previousPosition = 0.0;
     double motorRecResolution = 0.0;
     double handshakePerformed = 0;
+    double actualVelocity = 0.0;
 
     // =========================================================================
 
@@ -484,8 +497,8 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
         }
 
         pC_->read(axisNo_, 86, response);
-        if (rw_status != asynSuccess) {
-            return rw_status;
+        if (rwStatus != asynSuccess) {
+            return rwStatus;
         }
 
         nvals = sscanf(response, "%lf", &handshakePerformed);
@@ -511,15 +524,15 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
     getAxisParamChecked(this, motorRecResolution, &motorRecResolution);
 
     // Read the previous motor position
-    pl_status = motorPosition(&previousPosition);
-    if (pl_status != asynSuccess) {
-        return pl_status;
+    plStatus = motorPosition(&previousPosition);
+    if (plStatus != asynSuccess) {
+        return plStatus;
     }
 
     // Update the axis status
-    rw_status = readAxisStatus();
-    if (rw_status != asynSuccess) {
-        return rw_status;
+    rwStatus = readAxisStatus();
+    if (rwStatus != asynSuccess) {
+        return rwStatus;
     }
 
     // If we wait for a handshake, but the motor was moving in its last poll
@@ -539,23 +552,37 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
         pMasterMacsA_->targetReachedUninitialized = false;
     }
 
-    // Read the current position
-    rw_status = pC_->read(axisNo_, 12, response);
-    if (rw_status != asynSuccess) {
-        return rw_status;
-    }
-    nvals = sscanf(response, "%lf", &currentPosition);
-    if (nvals != 1) {
-        return pC_->couldNotParseResponse("R12", response, axisNo_,
-                                          __PRETTY_FUNCTION__, __LINE__);
+    // If moving in velocity mode, read out the current speed. Otherwise, read
+    // out the current position
+    if (pMasterMacsA_->lastMoveCommand == velocityMode && *moving) {
+        rwStatus = pC_->read(axisNo_, 14, response);
+        if (rwStatus != asynSuccess) {
+            return rwStatus;
+        }
+        nvals = sscanf(response, "%lf", &actualVelocity);
+        if (nvals != 1) {
+            return pC_->couldNotParseResponse("R14", response, axisNo_,
+                                              __PRETTY_FUNCTION__, __LINE__);
+        }
+
+    } else {
+        rwStatus = pC_->read(axisNo_, 12, response);
+        if (rwStatus != asynSuccess) {
+            return rwStatus;
+        }
+        nvals = sscanf(response, "%lf", &currentPosition);
+        if (nvals != 1) {
+            return pC_->couldNotParseResponse("R12", response, axisNo_,
+                                              __PRETTY_FUNCTION__, __LINE__);
+        }
     }
 
     /*
-    Read out the error if either a fault condition status flag has been set or
-    if a movement has just ended.
+    Read out the error if either a fault condition status flag has been set
+    or if a movement has just ended.
      */
     if (faultConditionSet() || !(*moving)) {
-        rw_status = readAxisError();
+        rwStatus = readAxisError();
     }
 
     msgPrintControlKey keyError = msgPrintControlKey(
@@ -735,9 +762,9 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
     // 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;
+        rwStatus = readLimits();
+        if (rwStatus != asynSuccess) {
+            return rwStatus;
         }
     }
 
@@ -762,16 +789,19 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
     setAxisParamChecked(this, motorStatusDone, !(*moving));
     setAxisParamChecked(this, motorStatusDirection, direction);
 
-    pl_status = setMotorPosition(currentPosition);
-    if (pl_status != asynSuccess) {
-        return pl_status;
+    plStatus = setMotorPosition(currentPosition);
+    if (plStatus != asynSuccess) {
+        return plStatus;
     }
+
+    // Write the actual velocity to the paramLib
+    setAxisParamChecked(this, motorVelocity, actualVelocity);
+
     return poll_status;
 }
 
-asynStatus masterMacsAxis::doMoveVelocity(double minVelocity,
-                                          double maxVelocity,
-                                          double acceleration) {
+asynStatus masterMacsAxis::moveVelocity(double minVelocity, double maxVelocity,
+                                        double acceleration) {
     // Suppress unused variable warning
     (void)minVelocity;
     (void)acceleration;
@@ -787,6 +817,18 @@ asynStatus masterMacsAxis::doMoveVelocity(double minVelocity,
 
     // =========================================================================
 
+    // Can the motor be operated in velocity mode?
+    if (!pMasterMacsA_->canVelocityMove) {
+        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
+                  "Controller \"%s\", axis %d => %s, line %d:\nAxis cannot "
+                  "operate in velocity mode.\n",
+                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
+        setAxisParamChecked(this, motorStatusProblem, true);
+        setAxisParamChecked(this, motorMessageText,
+                            "cannot operate in velocity mode");
+        return asynError;
+    }
+
     getAxisParamChecked(this, motorEnableRBV, &enabled);
     getAxisParamChecked(this, motorRecResolution, &motorRecResolution);
 
@@ -822,7 +864,14 @@ asynStatus masterMacsAxis::doMoveVelocity(double minVelocity,
 
     // 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);
+    status = pC_->write(axisNo_, 00, "3", timeout);
+    if (status != asynSuccess) {
+        return status;
+    }
+
+    // Cache the information that the current movement is in velocity mode
+    pMasterMacsA_->lastMoveCommand = velocityMode;
+    return status;
 }
 
 asynStatus masterMacsAxis::doMove(double position, int relative,
@@ -845,6 +894,18 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
 
     // =========================================================================
 
+    // Can the motor be operated in position mode?
+    if (!pMasterMacsA_->canPositionMove) {
+        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
+                  "Controller \"%s\", axis %d => %s, line %d:\nAxis cannot "
+                  "operate in position mode.\n",
+                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
+        setAxisParamChecked(this, motorStatusProblem, true);
+        setAxisParamChecked(this, motorMessageText,
+                            "cannot operate in position mode");
+        return asynError;
+    }
+
     getAxisParamChecked(this, motorEnableRBV, &enabled);
     getAxisParamChecked(this, motorRecResolution, &motorRecResolution);
 
@@ -930,6 +991,8 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
         return asynError;
     }
 
+    // Cache the information that the current movement is in position mode
+    pMasterMacsA_->lastMoveCommand = positionMode;
     return status;
 }
 
@@ -1230,8 +1293,8 @@ asynStatus masterMacsAxis::readAxisStatus() {
 
     // =========================================================================
 
-    asynStatus rw_status = pC_->read(axisNo_, 10, response);
-    if (rw_status == asynSuccess) {
+    asynStatus rwStatus = pC_->read(axisNo_, 10, response);
+    if (rwStatus == asynSuccess) {
 
         float axisStatus = 0;
         int nvals = sscanf(response, "%f", &axisStatus);
@@ -1243,7 +1306,7 @@ asynStatus masterMacsAxis::readAxisStatus() {
         pMasterMacsA_->axisStatus = toBitset(axisStatus);
     }
 
-    return rw_status;
+    return rwStatus;
 }
 
 asynStatus masterMacsAxis::readAxisError() {
@@ -1251,8 +1314,8 @@ asynStatus masterMacsAxis::readAxisError() {
 
     // =========================================================================
 
-    asynStatus rw_status = pC_->read(axisNo_, 11, response);
-    if (rw_status == asynSuccess) {
+    asynStatus rwStatus = pC_->read(axisNo_, 11, response);
+    if (rwStatus == asynSuccess) {
 
         float axisError = 0;
         int nvals = sscanf(response, "%f", &axisError);
@@ -1262,7 +1325,7 @@ asynStatus masterMacsAxis::readAxisError() {
         }
         pMasterMacsA_->axisError = toBitset(axisError);
     }
-    return rw_status;
+    return rwStatus;
 }
 
 bool masterMacsAxis::readyToBeSwitchedOn() {

src/masterMacsAxis.h

@@ -52,17 +52,15 @@ 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`.
+     * @brief TODO
      *
      * @param minVelocity
      * @param maxVelocity
      * @param acceleration
      * @return asynStatus
      */
-    asynStatus doMoveVelocity(double minVelocity, double maxVelocity,
-                              double acceleration);
+    asynStatus moveVelocity(double minVelocity, double maxVelocity,
+                            double acceleration);
 
     /**
      * @brief Implementation of the `doMove` function from sinqAxis. The