Initial commit for masterMacs

2024-12-23 09:40:40 +01:00
commit 7d4c2ea1e4
8 changed files with 2065 additions and 0 deletions
--- a/src/masterMacsAxis.cpp
+++ b/src/masterMacsAxis.cpp
@@ -0,0 +1,794 @@
+#include "masterMacsAxis.h"
+#include "asynOctetSyncIO.h"
+#include "masterMacsController.h"
+#include <bitset>
+#include <cmath>
+#include <errlog.h>
+#include <limits>
+#include <math.h>
+#include <string.h>
+#include <unistd.h>
+
+masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo)
+    : sinqAxis(pC, axisNo), pC_(pC) {
+
+    asynStatus status = asynSuccess;
+
+    /*
+    The superclass constructor sinqAxis calls in turn its superclass constructor
+    asynMotorAxis. In the latter, a pointer to the constructed object this is
+    stored inside the array pAxes_:
+
+        pC->pAxes_[axisNo] = this;
+
+    Therefore, the axes are managed by the controller pC. If axisNo is out of
+    bounds, asynMotorAxis prints an error (see
+    https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorAxis.cpp,
+    line 40). However, we want the IOC creation to stop completely, since this
+    is a configuration error.
+    */
+    if (axisNo >= pC->numAxes_) {
+        asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+                  "%s => line %d:: FATAL ERROR: Axis index %d must be smaller "
+                  "than the total number of axes %d. Call the support.",
+                  __PRETTY_FUNCTION__, __LINE__, axisNo_, pC->numAxes_);
+        exit(-1);
+    }
+
+    // Initialize all member variables
+    initial_poll_ = true;
+    axisStatus_ = std::bitset<16>(0);
+    axisError_ = std::bitset<16>(0);
+
+    // Default values for the watchdog timeout mechanism
+    offsetMovTimeout_ = 30; // seconds
+    scaleMovTimeout_ = 2.0;
+
+    // masterMacs motors can always be disabled
+    status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable_, 1);
+    if (status != asynSuccess) {
+        asynPrint(
+            pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+            "%s => line %d:\nFATAL ERROR (setting a parameter value failed "
+            "with %s)\n. Terminating IOC",
+            __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status));
+        exit(-1);
+    }
+
+    // Assume that the motor is initially not moving
+    status = pC_->setIntegerParam(axisNo_, pC_->motorStatusMoving_, false);
+    if (status != asynSuccess) {
+        asynPrint(
+            pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+            "%s => line %d:\nFATAL ERROR (setting a parameter value failed "
+            "with %s)\n. Terminating IOC",
+            __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status));
+        exit(-1);
+    }
+}
+
+masterMacsAxis::~masterMacsAxis(void) {
+    // Since the controller memory is managed somewhere else, we don't need to
+    // clean up the pointer pC here.
+}
+
+/**
+Read out the following values:
+- current position
+- current velocity
+- maximum velocity
+- acceleration
+- Software limits
+ */
+asynStatus masterMacsAxis::atFirstPoll() {
+
+    // Local variable declaration
+    asynStatus pl_status = asynSuccess;
+    char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
+    int nvals = 0;
+    double motorRecResolution = 0.0;
+    double motorPosition = 0.0;
+    double motorVelocity = 0.0;
+    double motorVmax = 0.0;
+    double motorAccel = 0.0;
+
+    // =========================================================================
+
+    pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
+                                    &motorRecResolution);
+    if (pl_status == asynParamUndefined) {
+        return asynParamUndefined;
+    } else if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Read out the current position
+    snprintf(command, sizeof(command), "%dR12", axisNo_);
+    pl_status = pC_->writeRead(axisNo_, command, response, true);
+    if (pl_status != asynSuccess) {
+        return pl_status;
+    }
+    nvals = sscanf(response, "%lf", &motorPosition);
+    if (nvals != 1) {
+        return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
+                                                __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Read out the current velocity
+    snprintf(command, sizeof(command), "%dR05", axisNo_);
+    pl_status = pC_->writeRead(axisNo_, command, response, true);
+    if (pl_status != asynSuccess) {
+        return pl_status;
+    }
+    nvals = sscanf(response, "%lf", &motorVelocity);
+    if (nvals != 1) {
+        return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
+                                                __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Read out the maximum velocity
+    // snprintf(command, sizeof(command), "%dR26", axisNo_);
+    // pl_status = pC_->writeRead(axisNo_, command, response, true);
+    // if (pl_status != asynSuccess) {
+    //     return pl_status;
+    // }
+    // nvals = sscanf(response, "%lf", &motorVmax);
+    // if (nvals != 1) {
+    //     return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
+    //                                             __PRETTY_FUNCTION__,
+    //                                             __LINE__);
+    // }
+    // TODO: Temporary workaround until R26 is implemented
+    motorVmax = motorVelocity;
+
+    // Read out the acceleration
+    snprintf(command, sizeof(command), "%dR06", axisNo_);
+    pl_status = pC_->writeRead(axisNo_, command, response, true);
+    if (pl_status != asynSuccess) {
+        return pl_status;
+    }
+    nvals = sscanf(response, "%lf", &motorAccel);
+    if (nvals != 1) {
+        return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
+                                                __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Transform from motor to parameter library coordinates
+    motorPosition = motorPosition / motorRecResolution;
+
+    // Store these values in the parameter library
+    pl_status =
+        pC_->setDoubleParam(axisNo_, pC_->motorPosition_, motorPosition);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Write to the motor record fields
+    pl_status = setVeloFields(motorVelocity, 0.0, motorVmax);
+    if (pl_status != asynSuccess) {
+        return pl_status;
+    }
+    pl_status = setAcclField(motorAccel);
+    if (pl_status != asynSuccess) {
+        return pl_status;
+    }
+
+    pl_status = readEncoderType();
+    if (pl_status != asynSuccess) {
+        return pl_status;
+    }
+
+    // Update the parameter library immediately
+    pl_status = callParamCallbacks();
+    if (pl_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
+        asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+                  "%s => line %d:\ncallParamCallbacks failed with %s for "
+                  "axis %d.\n",
+                  __PRETTY_FUNCTION__, __LINE__,
+                  pC_->stringifyAsynStatus(pl_status), axisNo_);
+        return pl_status;
+    }
+    return pl_status;
+}
+
+// Perform the actual poll
+asynStatus masterMacsAxis::doPoll(bool *moving) {
+
+    // Return value for the poll
+    asynStatus poll_status = asynSuccess;
+
+    // Status of read-write-operations of ASCII commands to the controller
+    asynStatus rw_status = asynSuccess;
+
+    // Status of parameter library operations
+    asynStatus pl_status = asynSuccess;
+
+    char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
+    int nvals = 0;
+
+    int direction = 0;
+    bool hasError = false;
+    double currentPosition = 0.0;
+    double previousPosition = 0.0;
+    double motorRecResolution = 0.0;
+    double highLimit = 0.0;
+    double lowLimit = 0.0;
+    double limitsOffset = 0.0;
+    int wasMoving = 0;
+
+    // =========================================================================
+
+    // Motor resolution from parameter library
+    pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
+                                    &motorRecResolution);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Read the previous motor position
+    pl_status =
+        pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &previousPosition);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Transform from EPICS to motor coordinates (see comment in
+    // masterMacsAxis::readConfig())
+    previousPosition = previousPosition * motorRecResolution;
+
+    // Update the axis status
+    rw_status = readAxisStatus();
+    if (rw_status != asynSuccess) {
+        return rw_status;
+    }
+
+    // Update the movement status
+    *moving = isMoving();
+
+    // Read the current position
+    snprintf(command, sizeof(command), "%dR12", axisNo_);
+    rw_status = pC_->writeRead(axisNo_, command, response, true);
+    if (rw_status != asynSuccess) {
+        return rw_status;
+    }
+    nvals = sscanf(response, "%lf", &currentPosition);
+    if (nvals != 1) {
+        return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
+                                                __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Read the low limit
+    snprintf(command, sizeof(command), "%dR34", axisNo_);
+    rw_status = pC_->writeRead(axisNo_, command, response, true);
+    if (rw_status != asynSuccess) {
+        return rw_status;
+    }
+    nvals = sscanf(response, "%lf", &lowLimit);
+    if (nvals != 1) {
+        return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
+                                                __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Read the high limit
+    snprintf(command, sizeof(command), "%dR33", axisNo_);
+    rw_status = pC_->writeRead(axisNo_, command, response, true);
+    if (rw_status != asynSuccess) {
+        return rw_status;
+    }
+    nvals = sscanf(response, "%lf", &highLimit);
+    if (nvals != 1) {
+        return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
+                                                __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Did we just finish a movement? If yes, read out the error.
+    pl_status =
+        pC_->getIntegerParam(axisNo_, pC_->motorStatusMoving_, &wasMoving);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+    if (!isMoving() && wasMoving == 1) {
+        rw_status = readAxisError();
+        if (rw_status != asynSuccess) {
+            return rw_status;
+        }
+
+        // TODO> Error handling
+    }
+
+    /*
+    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.
+    */
+    pl_status =
+        pC_->getDoubleParam(axisNo_, pC_->motorLimitsOffset_, &limitsOffset);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorLimitsOffset_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+    highLimit = highLimit - limitsOffset;
+    lowLimit = lowLimit + limitsOffset;
+
+    //
+
+    // Update the enablement PV
+    pl_status = setIntegerParam(pC_->motorEnableRBV_, enabled());
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorEnableRBV_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    if (*moving) {
+        // If the axis is moving, evaluate the movement direction
+        if ((currentPosition - previousPosition) > 0) {
+            direction = 1;
+        } else {
+            direction = 0;
+        }
+    }
+
+    // Update the parameter library
+    if (hasError) {
+        pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+    }
+
+    if (!*moving) {
+        pl_status = setIntegerParam(pC_->motorMoveToHome_, 0);
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+    }
+
+    pl_status = setIntegerParam(pC_->motorStatusMoving_, *moving);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    pl_status = setIntegerParam(pC_->motorStatusDone_, !(*moving));
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    pl_status = setIntegerParam(pC_->motorStatusDirection_, direction);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorStatusDirection_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    pl_status =
+        pC_->setDoubleParam(axisNo_, pC_->motorHighLimitFromDriver_, highLimit);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    pl_status =
+        pC_->setDoubleParam(axisNo_, pC_->motorLowLimitFromDriver_, lowLimit);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Transform from motor to EPICS coordinates (see comment in
+    // masterMacsAxis::atFirstPoll())
+    currentPosition = currentPosition / motorRecResolution;
+
+    pl_status = setDoubleParam(pC_->motorPosition_, currentPosition);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    return poll_status;
+}
+
+asynStatus masterMacsAxis::doMove(double position, int relative,
+                                  double minVelocity, double maxVelocity,
+                                  double acceleration) {
+
+    // Status of read-write-operations of ASCII commands to the controller
+    asynStatus rw_status = asynSuccess;
+
+    // Status of parameter library operations
+    asynStatus pl_status = asynSuccess;
+
+    char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
+    double motorCoordinatesPosition = 0.0;
+    double motorRecResolution = 0.0;
+    double motorVelocity = 0.0;
+    int enabled = 0;
+    int motorCanSetSpeed = 0;
+
+    // =========================================================================
+
+    pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableRBV_, &enabled);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
+                                    &motorRecResolution);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    if (enabled == 0) {
+        asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+                  "%s => line %d:\nAxis %d is disabled.\n", __PRETTY_FUNCTION__,
+                  __LINE__, axisNo_);
+        return asynSuccess;
+    }
+
+    // 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);
+
+    // 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__);
+    }
+
+    // Set the new motor speed, if the user is allowed to do so.
+    if (motorCanSetSpeed != 0) {
+        snprintf(command, sizeof(command), "%dS05=%lf", axisNo_, motorVelocity);
+        rw_status = pC_->writeRead(axisNo_, command, response, false);
+        if (rw_status != asynSuccess) {
+            pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
+            if (pl_status != asynSuccess) {
+                return pC_->paramLibAccessFailed(pl_status,
+                                                 "motorStatusProblem_",
+                                                 __PRETTY_FUNCTION__, __LINE__);
+            }
+            return rw_status;
+        }
+
+        asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
+                  "%s => line %d:\nSetting speed of axis %d to %lf.\n",
+                  __PRETTY_FUNCTION__, __LINE__, axisNo_, motorVelocity);
+    }
+
+    // Set the target position
+    snprintf(command, sizeof(command), "%dS02=%lf", axisNo_,
+             motorCoordinatesPosition);
+    rw_status = pC_->writeRead(axisNo_, command, response, false);
+    if (rw_status != asynSuccess) {
+        pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+        return rw_status;
+    }
+
+    // Start the move
+    if (relative) {
+        snprintf(command, sizeof(command), "%dS00=2", axisNo_);
+    } else {
+        snprintf(command, sizeof(command), "%dS00=1", axisNo_);
+    }
+    rw_status = pC_->writeRead(axisNo_, command, response, false);
+    if (rw_status != asynSuccess) {
+        pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+        return rw_status;
+    }
+
+    // Waiting for a handshake is already part of the movement procedure =>
+    // Start the watchdog
+    if (startMovTimeoutWatchdog() != asynSuccess) {
+        return asynError;
+    }
+
+    return rw_status;
+}
+
+asynStatus masterMacsAxis::stop(double acceleration) {
+
+    // Status of read-write-operations of ASCII commands to the controller
+    asynStatus rw_status = asynSuccess;
+
+    // Status of parameter library operations
+    asynStatus pl_status = asynSuccess;
+
+    char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
+
+    // =========================================================================
+
+    snprintf(command, sizeof(command), "%dS00=8", axisNo_);
+    rw_status = pC_->writeRead(axisNo_, command, response, false);
+    if (rw_status != asynSuccess) {
+        pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+    }
+
+    return rw_status;
+}
+
+/*
+Home the axis. On absolute encoder systems, this is a no-op
+*/
+asynStatus masterMacsAxis::doHome(double min_velocity, double max_velocity,
+                                  double acceleration, int forwards) {
+
+    // Status of read-write-operations of ASCII commands to the controller
+    asynStatus rw_status = asynSuccess;
+
+    // Status of parameter library operations
+    asynStatus pl_status = asynSuccess;
+
+    char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
+
+    // =========================================================================
+
+    pl_status = pC_->getStringParam(axisNo_, pC_->encoderType_,
+                                    sizeof(response), response);
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "encoderType_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    // Only send the home command if the axis has an incremental encoder
+    if (strcmp(response, IncrementalEncoder) == 0) {
+
+        snprintf(command, sizeof(command), "%dS00=9", axisNo_);
+        rw_status = pC_->writeRead(axisNo_, command, response, false);
+        if (rw_status != asynSuccess) {
+            return rw_status;
+        }
+
+        pl_status = setIntegerParam(pC_->motorMoveToHome_, 1);
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+
+        pl_status = setStringParam(pC_->motorMessageText_, "Homing");
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+    } else {
+        pl_status = setStringParam(pC_->motorMessageText_,
+                                   "Can't home a motor with absolute encoder");
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+    }
+
+    return rw_status;
+}
+
+/*
+Read the encoder type and update the parameter library accordingly
+*/
+asynStatus masterMacsAxis::readEncoderType() {
+
+    // Status of read-write-operations of ASCII commands to the controller
+    asynStatus rw_status = asynSuccess;
+
+    // Status of parameter library operations
+    asynStatus pl_status = asynSuccess;
+
+    char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
+    int nvals = 0;
+    int encoder_id = 0;
+
+    // =========================================================================
+
+    // Check if this is an absolute encoder
+    snprintf(command, sizeof(command), "%dR60", axisNo_);
+    rw_status = pC_->writeRead(axisNo_, command, response, true);
+    if (rw_status != asynSuccess) {
+        return rw_status;
+    }
+
+    nvals = sscanf(response, "%d", &encoder_id);
+    if (nvals != 1) {
+        return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
+                                                __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    /*
+    Defined encoder IDs:
+    0=INC (Incremental)
+    1=SSI (Absolute encoder with SSI interface)
+    2=SSI (Absolute encoder with BiSS interface)
+     */
+    if (encoder_id == 0) {
+        pl_status = setStringParam(pC_->encoderType_, IncrementalEncoder);
+    } else {
+        pl_status = setStringParam(pC_->encoderType_, AbsoluteEncoder);
+    }
+
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "encoderType_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+    return asynSuccess;
+}
+
+asynStatus masterMacsAxis::enable(bool on) {
+
+    int timeout_enable_disable = 2;
+    char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
+
+    // Status of read-write-operations of ASCII commands to the controller
+    asynStatus rw_status = asynSuccess;
+
+    // Status of parameter library operations
+    asynStatus pl_status = asynSuccess;
+
+    bool moving = false;
+    doPoll(&moving);
+
+    // =========================================================================
+
+    // If the axis is currently moving, it cannot be disabled. Ignore the
+    // command and inform the user. We check the last known status of the
+    // axis instead of "moving", since status -6 is also moving, but the
+    // motor can actually be disabled in this state!
+    if (moving) {
+        asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+                  "%s => line %d:\nAxis %d is not idle and can therefore not "
+                  "be enabled / disabled.\n",
+                  __PRETTY_FUNCTION__, __LINE__, axisNo_);
+
+        pl_status =
+            setStringParam(pC_->motorMessageText_,
+                           "Axis cannot be disabled while it is moving.");
+        if (pl_status != asynSuccess) {
+            return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
+                                             __PRETTY_FUNCTION__, __LINE__);
+        }
+
+        return asynError;
+    }
+
+    // Axis is already enabled / disabled and a new enable / disable command
+    // was sent => Do nothing
+    if ((axisStatus_ != -3) == on) {
+        asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING,
+                  "%s => line %d:\nAxis %d on controller %s is already %s.\n",
+                  __PRETTY_FUNCTION__, __LINE__, axisNo_, pC_->portName,
+                  on ? "enabled" : "disabled");
+        return asynSuccess;
+    }
+
+    // Enable / disable the axis if it is not moving
+    snprintf(command, sizeof(command), "%dS04=%d", axisNo_, on);
+    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
+              "%s => line %d:\n%s axis %d on controller %s\n",
+              __PRETTY_FUNCTION__, __LINE__, on ? "Enable" : "Disable", axisNo_,
+              pC_->portName);
+    if (on == 0) {
+        pl_status = setStringParam(pC_->motorMessageText_, "Disabling ...");
+    } else {
+        pl_status = setStringParam(pC_->motorMessageText_, "Enabling ...");
+    }
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+    rw_status = pC_->writeRead(axisNo_, command, response, false);
+    if (rw_status != asynSuccess) {
+        return rw_status;
+    }
+
+    // Query the axis status every few milliseconds until the axis has been
+    // enabled or until the timeout has been reached
+    int startTime = time(NULL);
+    while (time(NULL) < startTime + timeout_enable_disable) {
+
+        // Read the axis status
+        usleep(100000);
+        rw_status = readAxisStatus();
+        if (rw_status != asynSuccess) {
+            return rw_status;
+        }
+
+        if (switchedOn() == on) {
+            bool moving = false;
+            // Perform a poll to update the parameter library
+            poll(&moving);
+            return asynSuccess;
+        }
+    }
+
+    // Failed to change axis status within timeout_enable_disable => Send a
+    // corresponding message
+    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
+              "%s => line %d:\nFailed to %s axis %d on controller %s within %d "
+              "seconds\n",
+              __PRETTY_FUNCTION__, __LINE__, on ? "enable" : "disable", axisNo_,
+              pC_->portName, timeout_enable_disable);
+
+    // Output message to user
+    snprintf(command, sizeof(command), "Failed to %s within %d seconds",
+             on ? "enable" : "disable", timeout_enable_disable);
+    pl_status = setStringParam(pC_->motorMessageText_, "Enabling ...");
+    if (pl_status != asynSuccess) {
+        return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
+                                         __PRETTY_FUNCTION__, __LINE__);
+    }
+    return asynError;
+}
+
+asynStatus masterMacsAxis::readAxisStatus() {
+    char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
+
+    // =========================================================================
+
+    snprintf(command, sizeof(command), "%dR10", axisNo_);
+    asynStatus rw_status = pC_->writeRead(axisNo_, command, response, true);
+    if (rw_status == asynSuccess) {
+
+        int axisStatus = 0;
+        int nvals = sscanf(response, "%d", &axisStatus);
+        if (nvals != 1) {
+            return pC_->errMsgCouldNotParseResponse(
+                command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
+        }
+        axisStatus_ = std::bitset<16>(axisStatus);
+    }
+    return rw_status;
+}
+
+asynStatus masterMacsAxis::readAxisError() {
+    char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
+
+    // =========================================================================
+
+    snprintf(command, sizeof(command), "%dR11", axisNo_);
+    asynStatus rw_status = pC_->writeRead(axisNo_, command, response, true);
+    if (rw_status == asynSuccess) {
+
+        int axisError = 0;
+        int nvals = sscanf(response, "%d", &axisError);
+        if (nvals != 1) {
+            return pC_->errMsgCouldNotParseResponse(
+                command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
+        }
+        axisError_ = std::bitset<16>(axisError);
+    }
+    return rw_status;
+}