Deadband check in driver code

src/turboPmacAxis.cpp

@@ -27,6 +27,18 @@ struct turboPmacAxisImpl {
     // The axis status is used when enabling / disabling the motor
     int axisStatus;
     bool needInit;
+
+    /*
+    Deadband is defined in enconder units / 16 (a value of 16 is 1 actual
+    encoder count, see Turbo PMAC Manula, p.160)
+    */
+    double deadband;
+
+    /*
+    Scaling factor Ixx08, see Turbo PMAC User Manual, p. 268f:
+    [Ixx08] = counts / engineering units (eu)
+    */
+    double eu_to_counts;
 };
 
 /*
@@ -70,12 +82,15 @@ turboPmacAxis::turboPmacAxis(turboPmacController *pC, int axisNo,
         axes.push_back(this);
     }
 
-    pTurboPmacA_ = std::make_unique<turboPmacAxisImpl>(
-        (turboPmacAxisImpl){.waitForHandshake = false,
+    pTurboPmacA_ = std::make_unique<turboPmacAxisImpl>((turboPmacAxisImpl){
+        .waitForHandshake = false,
         .enableDisable = false,
         .timeAtHandshake = 0,
         .axisStatus = 0,
-                            .needInit = false});
+        .needInit = false,
+        .deadband = 0.0,
+        .eu_to_counts = 0.0,
+    });
 
     // Provide initial values for some parameter library entries
     status = pC_->setIntegerParam(axisNo_, pC_->rereadEncoderPosition(), 0);
@@ -163,12 +178,16 @@ asynStatus turboPmacAxis::init() {
 
     /*
     Read out the axis status, the current position, current and maximum speed,
-    acceleration and the air cushion delay.
+    acceleration, the air cushion delay, the scaling factor between counts and
+    engineering units ([Ixx08] = counts / engineering units) and the deadband
+    Ixx65.
     */
     snprintf(command, sizeof(command),
-             "P%2.2d00 Q%2.2d10 Q%2.2d03 Q%2.2d04 Q%2.2d06 P%2.2d22", axisNo_,
-             axisNo_, axisNo_, axisNo_, axisNo_, axisNo_);
-    status = pC_->writeRead(axisNo_, command, response, 6);
+             "P%2.2d00 Q%2.2d10 Q%2.2d03 Q%2.2d04 Q%2.2d06 P%2.2d22 I%2.2d08 "
+             "I%2.2d65",
+             axisNo_, axisNo_, axisNo_, axisNo_, axisNo_, axisNo_, axisNo_,
+             axisNo_);
+    status = pC_->writeRead(axisNo_, command, response, 8);
     if (status != asynSuccess) {
         asynPrint(
             pC_->pasynUser(), ASYN_TRACE_ERROR,
@@ -180,8 +199,10 @@ asynStatus turboPmacAxis::init() {
             pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__);
         pTurboPmacA_->needInit = true;
     }
-    nvals = sscanf(response, "%d %lf %lf %lf %lf %d", &axStatus, &motorPos,
-                   &motorVmax, &motorVelocity, &motorAccel, &acoDelay);
+    nvals =
+        sscanf(response, "%d %lf %lf %lf %lf %d %lf %lf", &axStatus, &motorPos,
+               &motorVmax, &motorVelocity, &motorAccel, &acoDelay,
+               &pTurboPmacA_->eu_to_counts, &pTurboPmacA_->deadband);
 
     // The acoDelay is given in milliseconds -> Convert to seconds, rounded up
     setOffsetMovTimeout(std::ceil(acoDelay / 1000.0));
@@ -191,7 +212,7 @@ asynStatus turboPmacAxis::init() {
     // here to mm/s^2.
     motorAccel = motorAccel * 1000;
 
-    if (nvals != 6) {
+    if (nvals != 8) {
         return pC_->couldNotParseResponse(command, response, axisNo_,
                                           __PRETTY_FUNCTION__, __LINE__);
     }
@@ -752,7 +773,7 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
 
     char command[pC_->MAXBUF_] = {0};
     char response[pC_->MAXBUF_] = {0};
-    double motorCoordinatesPosition = 0.0;
+    double motorTargetPosition = 0.0;
     double motorRecResolution = 0.0;
     double motorVelocity = 0.0;
     int enabled = 0;
@@ -773,13 +794,48 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
     }
 
     // Convert from EPICS to user / motor units
-    motorCoordinatesPosition = position * motorRecResolution;
+    motorTargetPosition = position * motorRecResolution;
     motorVelocity = maxVelocity * motorRecResolution;
 
-    asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW,
+    /*
+    Check if the new target position is within the range current position +/-
+    deadband. If that is the case, no movement command should be sent.
+
+    First, the deadband is read out (Ixx65, see Turbo PMAC User Manual, p.
+    160.). This value then needs to be converted into counts (by dividing it by
+    16). After words, the deadband in counts then need to be converted into
+    engineering units. For this, the scaling factor Ixx08 needs to be applied
+    (see Turbo PMAC User Manual, p. 268f):
+    [Ixx08] = counts / engineering units (eu)
+    deadband_eu = deadband_counts / Ixx08 = Ixx65 / (16 * Ixx08)
+
+    The values of Ixx08 and Ixx65 are read out during initialization and are
+    assumed to not change during operation.
+    */
+    double motorCurrentPosition = 0.0;
+    motorPosition(&motorCurrentPosition);
+    double deadband_eu = pTurboPmacA_->deadband / (pTurboPmacA_->eu_to_counts);
+    double deadband_lower = motorCurrentPosition - deadband_eu;
+    double deadband_upper = motorCurrentPosition + deadband_eu;
+
+    if (motorTargetPosition > deadband_lower &&
+        motorTargetPosition < deadband_upper) {
+
+        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
+                  "Controller \"%s\", axis %d  => %s, line %d\nNew target "
+                  "position %lf within deadband around current position %lf "
+                  "(deadband %lf to %lf). No move command sent.\n",
+                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
+                  motorTargetPosition, motorCurrentPosition, deadband_lower,
+                  deadband_upper);
+        return asynSuccess;
+    }
+
+    asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
               "Controller \"%s\", axis %d  => %s, line %d\nStart of axis to "
               "position %lf.\n",
-              pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, position);
+              pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
+              motorCurrentPosition);
 
     // Check if the speed is allowed to be changed
     getAxisParamChecked(this, motorCanSetSpeed, &motorCanSetSpeed);
@@ -804,11 +860,11 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
     if (relative) {
         snprintf(&command[writeOffset], sizeof(command) - writeOffset,
                  "P%2.2d23=0 Q%2.2d02=%lf M%2.2d=2", axisNo_, axisNo_,
-                 motorCoordinatesPosition, axisNo_);
+                 motorTargetPosition, axisNo_);
     } else {
         snprintf(&command[writeOffset], sizeof(command) - writeOffset,
                  "P%2.2d23=0 Q%2.2d01=%lf M%2.2d=1", axisNo_, axisNo_,
-                 motorCoordinatesPosition, axisNo_);
+                 motorTargetPosition, axisNo_);
     }
 
     // We don't expect an answer
@@ -820,7 +876,7 @@ asynStatus turboPmacAxis::doMove(double position, int relative,
             "Controller \"%s\", axis %d  => %s, line %d\nStarting movement to "
             "target position %lf failed.\n",
             pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
-            motorCoordinatesPosition);
+            motorTargetPosition);
         setAxisParamChecked(this, motorStatusProblem, true);
         return status;
     }