Added support for (optional) variable speed drive mode and refactored

some records into sinqMotor

Makefile

@@ -11,7 +11,7 @@ REQUIRED+=asynMotor
 REQUIRED+=sinqMotor
 
 # Specify the version of sinqMotor we want to build against
-sinqMotor_VERSION=0.3.0
+sinqMotor_VERSION=mathis_s
 
 # These headers allow to depend on this library for derived drivers.
 HEADERS += src/pmacv3Axis.h

db/pmacv3.db

@@ -19,41 +19,4 @@ record(longout, "$(P)$(M):Read_Config") {
     field(DTYP, "asynInt32")
     field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) READ_CONFIG")
     field(PINI, "NO")
-}
-
-# ===================================================================
-# The following records read acceleration and velocity from the driver and 
-# copy those values into the corresponding fields of the main motor record.
-# This strategy is described here: https://epics.anl.gov/tech-talk/2022/msg00464.php
-
-# Helper record for the high limit which is filled in by the driver
-record(ai, "$(P)$(M):MOTOR_VELOCITY-RBV")
-{
-    field(DTYP, "asynFloat64")
-    field(INP,  "@asyn($(CONTROLLER),$(AXIS)) MOTOR_VELOCITY_FROM_DRIVER")
-    field(SCAN, "I/O Intr")
-    field(FLNK, "$(P)$(M):PUSH_VELO_TO_FIELD")
-}
-
-# Push the value into the field of the main motor record
-record(ao, "$(P)$(M):PUSH_VELO_TO_FIELD") {
-    field(DOL,  "$(P)$(M):MOTOR_VELOCITY-RBV CP")
-    field(OUT,  "$(P)$(M).VELO")
-    field(OMSL, "closed_loop") # This configuration keeps the PV and the field in sync
-}
-
-# Helper record for the low limit which is filled in by the driver
-record(ai, "$(P)$(M):MOTOR_ACCL-RBV")
-{
-    field(DTYP, "asynFloat64")
-    field(INP,  "@asyn($(CONTROLLER),$(AXIS)) MOTOR_ACCEL_FROM_DRIVER")
-    field(SCAN, "I/O Intr")
-    field(FLNK, "$(P)$(M):PUSH_ACCL_TO_FIELD")
-}
-
-# Push the value into the field of the main motor record
-record(ao, "$(P)$(M):PUSH_ACCL_TO_FIELD") {
-    field(DOL,  "$(P)$(M):MOTOR_ACCL-RBV CP")
-    field(OUT,  "$(P)$(M).ACCL")
-    field(OMSL, "closed_loop") # This configuration keeps the PV and the field in sync
 }

src/pmacv3Axis.cpp

@@ -87,6 +87,7 @@ asynStatus pmacv3Axis::readConfig() {
     double motorRecResolution = 0.0;
     double motorPosition = 0.0;
     double motorVelocity = 0.0;
+    double motorVmax = 0.0;
     double motorAccel = 0.0;
     int acoDelay = 0.0; // Offset time for the movement watchdog caused by
                         // the air cushions in milliseconds.
@@ -103,14 +104,14 @@ asynStatus pmacv3Axis::readConfig() {
 
     // Software limits and current position
     snprintf(command, sizeof(command),
-             "P%2.2d00 Q%2.2d10 Q%2.2d04 Q%2.2d06 P%2.2d22", axisNo_, axisNo_,
+             "P%2.2d00 Q%2.2d10 Q%2.2d03 Q%2.2d04 Q%2.2d06 P%2.2d22", axisNo_,
-             axisNo_, axisNo_, axisNo_);
+             axisNo_, axisNo_, axisNo_, axisNo_, axisNo_);
-    status = pC_->writeRead(axisNo_, command, response, 5);
+    status = pC_->writeRead(axisNo_, command, response, 6);
     if (status != asynSuccess) {
         return status;
     }
-    nvals = sscanf(response, "%d %lf %lf %lf %d", &axStatus, &motorPosition,
+    nvals = sscanf(response, "%d %lf %lf %lf %lf %d", &axStatus, &motorPosition,
-                   &motorVelocity, &motorAccel, &acoDelay);
+                   &motorVmax, &motorVelocity, &motorAccel, &acoDelay);
 
     // The acoDelay is given in milliseconds -> Convert to seconds, rounded up
     offsetMovTimeout_ = std::ceil(acoDelay / 1000.0);
@@ -120,7 +121,7 @@ asynStatus pmacv3Axis::readConfig() {
     // here to mm/s^2.
     motorAccel = motorAccel * 1000;
 
-    if (nvals != 5) {
+    if (nvals != 6) {
         return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
                                                 __PRETTY_FUNCTION__, __LINE__);
     }
@@ -135,25 +136,14 @@ asynStatus pmacv3Axis::readConfig() {
                                          __PRETTY_FUNCTION__, __LINE__);
     }
 
-    status =
+    // Write to the motor record fields
-        pC_->setDoubleParam(axisNo_, pC_->motorVelocityRBV_, motorVelocity);
+    status = setVeloFields(motorVelocity, 0.0, motorVmax);
     if (status != asynSuccess) {
-        return pC_->paramLibAccessFailed(status, "motorVelocityRBV_",
+        return status;
-                                         __PRETTY_FUNCTION__, __LINE__);
     }
-
+    status = setAcclField(motorAccel);
-    status = pC_->setDoubleParam(axisNo_, pC_->motorAccelRBV_, motorAccel);
     if (status != asynSuccess) {
-        return pC_->paramLibAccessFailed(status, "motorAccelRBV_",
+        return status;
-                                         __PRETTY_FUNCTION__, __LINE__);
-    }
-
-    // Set the initial enable based on the motor status value
-    status =
-        setIntegerParam(pC_->enableMotor_, (axStatus != -3 && axStatus != -5));
-    if (status != asynSuccess) {
-        return pC_->paramLibAccessFailed(status, "enableMotor_",
-                                         __PRETTY_FUNCTION__, __LINE__);
     }
 
     // Update the parameter library immediately
@@ -618,10 +608,10 @@ asynStatus pmacv3Axis::doPoll(bool *moving) {
         }
     }
 
-    pl_status = setIntegerParam(pC_->enableMotorRBV_,
+    pl_status = setIntegerParam(pC_->motorEnableRBV_,
                                 (axStatus != -3 && axStatus != -5));
     if (pl_status != asynSuccess) {
-        return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_",
+        return pC_->paramLibAccessFailed(pl_status, "motorEnableRBV_",
                                          __PRETTY_FUNCTION__, __LINE__);
     }
 
@@ -681,12 +671,15 @@ asynStatus pmacv3Axis::doMove(double position, int relative, double minVelocity,
 
     char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
     double motorCoordinatesPosition = 0.0;
-    int enabled = 0;
     double motorRecResolution = 0.0;
+    double motorVelocity = 0.0;
+    int enabled = 0;
+    int motorCanSetSpeed = 0;
+    int writeOffset = 0;
 
     // =========================================================================
 
-    pl_status = pC_->getIntegerParam(axisNo_, pC_->enableMotorRBV_, &enabled);
+    pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableRBV_, &enabled);
     if (pl_status != asynSuccess) {
         return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_",
                                          __PRETTY_FUNCTION__, __LINE__);
@@ -708,18 +701,43 @@ asynStatus pmacv3Axis::doMove(double position, int relative, double minVelocity,
 
     // Convert from EPICS to user / motor units
     motorCoordinatesPosition = position * motorRecResolution;
+    motorVelocity = maxVelocity * motorRecResolution;
 
     asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
               "%s => line %d:\nStart of axis %d to position %lf.\n",
               __PRETTY_FUNCTION__, __LINE__, axisNo_, position);
 
-    // Perform handshake, Set target position and start the move command
+    // Check if the speed is allowed to be changed
+    pl_status = pC_->getIntegerParam(axisNo_, pC_->motorCanSetSpeed_,
+                                     &motorCanSetSpeed);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorCanSetSpeed_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Prepend the new motor speed, if the user is allowed to set the speed.
+    // Mind the " " (space) before the closing "", as the command created here
+    // is prepended to the one down below.
+    if (motorCanSetSpeed != 0) {
+        snprintf(command, sizeof(command), "Q%2.2d04=%lf ", axisNo_,
+                 motorVelocity);
+        writeOffset = strlen(command);
+
+        asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
+                  "%s => line %d:\nSetting speed of axis %d to %lf.\n",
+                  __PRETTY_FUNCTION__, __LINE__, axisNo_, motorVelocity);
+    }
+
+    // Perform handshake, Set target position (and speed, if allowed) and start
+    // the move command
     if (relative) {
-        snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d02=%lf M%2.2d=2",
+        snprintf(&command[writeOffset], sizeof(command) - writeOffset,
-                 axisNo_, axisNo_, motorCoordinatesPosition, axisNo_);
+                 "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",
+        snprintf(&command[writeOffset], sizeof(command) - writeOffset,
-                 axisNo_, axisNo_, motorCoordinatesPosition, axisNo_);
+                 "P%2.2d23=0 Q%2.2d01=%lf M%2.2d=1", axisNo_, axisNo_,
+                 motorCoordinatesPosition, axisNo_);
     }
 
     // We don't expect an answer
@@ -950,7 +968,7 @@ asynStatus pmacv3Axis::rereadEncoder() {
     // Check if the axis is disabled. If not, inform the user that this
     // is necessary
     int enabled = 0;
-    pl_status = pC_->getIntegerParam(axisNo_, pC_->enableMotorRBV_, &enabled);
+    pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableRBV_, &enabled);
     if (pl_status != asynSuccess) {
         return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_",
                                          __PRETTY_FUNCTION__, __LINE__);
@@ -1031,13 +1049,6 @@ asynStatus pmacv3Axis::enable(bool on) {
                                              __PRETTY_FUNCTION__, __LINE__);
         }
 
-        // Reset the value in the param lib.
-        pl_status = setIntegerParam(pC_->enableMotor_, 1);
-        if (pl_status != asynSuccess) {
-            return pC_->paramLibAccessFailed(pl_status, "enableMotor_",
-                                             __PRETTY_FUNCTION__, __LINE__);
-        }
-
         return asynError;
     }
 
@@ -1120,4 +1131,4 @@ asynStatus pmacv3Axis::enable(bool on) {
 asynStatus pmacv3Axis::isEnabled(bool *on) {
     *on = (axisStatus_ != -3 && axisStatus_ != -5);
     return asynSuccess;
-}
+}

src/pmacv3Controller.cpp

@@ -54,10 +54,9 @@ pmacv3Controller::pmacv3Controller(const char *portName,
           - ENCODER_TYPE
           - REREAD_ENCODER_POSITION
           - READ_CONFIG
-          - MOTOR_VELOCITY_FROM_DRIVER
+          - ACCEL_FROM_DRIVER
-          - MOTOR_ACCEL_FROM_DRIVER
           */
-          5)
+          4)
 
 {
 
@@ -114,26 +113,6 @@ pmacv3Controller::pmacv3Controller(const char *portName,
         exit(-1);
     }
 
-    status = createParam("MOTOR_VELOCITY_FROM_DRIVER", asynParamFloat64,
-                         &motorVelocityRBV_);
-    if (status != asynSuccess) {
-        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
-                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
-                  "with %s).\nTerminating IOC",
-                  __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status));
-        exit(-1);
-    }
-
-    status = createParam("MOTOR_ACCEL_FROM_DRIVER", asynParamFloat64,
-                         &motorAccelRBV_);
-    if (status != asynSuccess) {
-        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
-                  "%s => line %d:\nFATAL ERROR (creating a parameter failed "
-                  "with %s).\nTerminating IOC",
-                  __PRETTY_FUNCTION__, __LINE__, 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
@@ -426,6 +405,16 @@ asynStatus pmacv3Controller::writeRead(int axisNo, const char *command,
     return asynSuccess;
 }
 
+asynStatus pmacv3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) {
+    if (pasynUser->reason == motorCanDisable_) {
+        // By default, motors cannot be disabled
+        *value = 1;
+        return asynSuccess;
+    } else {
+        return sinqController::readInt32(pasynUser, value);
+    }
+}
+
 asynStatus pmacv3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) {
     int function = pasynUser->reason;
 

src/pmacv3Controller.h

@@ -51,10 +51,20 @@ class pmacv3Controller : public sinqController {
     pmacv3Axis *getAxis(int axisNo);
 
     /**
-     * @brief Overloaded function of asynMotorController
+     * @brief Overloaded function of sinqController
      *
-     * The function is overloaded to allow enabling / disabling the motor and
+     * The function is overloaded to read the canDisable property.
-     * rereading the encoder.
+     *
+     * @param pasynUser           Specify the axis via the asynUser
+     * @param value               Read-out value
+     * @return asynStatus
+     */
+    asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
+
+    /**
+     * @brief Overloaded function of sinqController
+     *
+     * The function is overloaded to allow rereading the encoder and config.
      *
      * @param pasynUser           Specify the axis via the asynUser
      * @param value               New value
@@ -132,13 +142,6 @@ class pmacv3Controller : public sinqController {
     int readConfig_;
     int encoderType_;
 
-    /*
-    Same strategy as with the limits in sinqController -> Use additional PVs to
-    write speed and acceleration from the driver to the record.
-     */
-    int motorVelocityRBV_;
-    int motorAccelRBV_;
-
     friend class pmacv3Axis;
 };