seleneguide/src/seleneAngleAxis.cpp

#include "seleneAngleAxis.h"
#include "seleneGuideController.h"
#include "seleneLiftAxis.h"
#include "turboPmacController.h"
#include <limits>
#include <math.h>

seleneAngleAxis::seleneAngleAxis(seleneGuideController *pController, int axisNo,
                                 seleneLiftAxis *axis)
    : turboPmacAxis(pController, axisNo, false), pC_(pController) {

    asynStatus status = asynSuccess;

    // Initialize the associated lift axis as a nullptr. It is populated in the
    // constructor of seleneLiftAxis.
    liftAxis_ = axis;
    targetSet_ = false;

    // Selene guide motors cannot be disabled
    status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable(), 0);
    if (status != asynSuccess) {
        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
                  "(setting a parameter value failed "
                  "with %s)\n. Terminating IOC",
                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
                  pC_->stringifyAsynStatus(status));
        exit(-1);
    }

    // Even though this happens already in sinqAxis, a default value for
    // motorMessageText is set here again, because apparently the sinqAxis
    // constructor is not run before the string is accessed?
    status = setStringParam(pC_->motorMessageText(), "");
    if (status != asynSuccess) {
        asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
                  "Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
                  "(setting a parameter value failed "
                  "with %s)\n. Terminating IOC",
                  pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
                  pC_->stringifyAsynStatus(status));
        exit(-1);
    }
}

seleneAngleAxis::~seleneAngleAxis() {}

asynStatus seleneAngleAxis::stop(double acceleration) {
    return liftAxis_->stop(acceleration);
}

asynStatus seleneAngleAxis::doMove(double position, int relative,
                                   double minVelocity, double maxVelocity,
                                   double acceleration) {

    // Suppress unused variable warnings
    (void)relative;
    (void)minVelocity;
    (void)maxVelocity;
    (void)acceleration;

    double motorRecResolution = 0.0;

    getAxisParamChecked(this, motorRecResolution, &motorRecResolution);
    setTargetPosition(position * motorRecResolution);
    targetSet_ = true;
    return liftAxis_->startCombinedMoveFromVirtualAxis();
    targetSet_ = false;
}

double seleneAngleAxis::targetPosition() {
    if (targetSet_) {
        return turboPmacAxis::targetPosition();
    } else {
        double targetPos = 0;
        motorPosition(&targetPos);
        return targetPos;
    }
}

asynStatus seleneAngleAxis::doPoll(bool *moving) {

    char errorMessage[pC_->MAXBUF_] = {0};
    asynStatus status = asynSuccess;

    // In the doPoll method of `seleneLiftAxis`, the parameters
    // `motorStatusMoving` and `motorStatusDone` of this axis have already been
    // set accordingly.
    getAxisParamChecked(this, motorStatusMoving, moving);

    double motPos = 0.0;
    status = motorPosition(&motPos);
    if (status != asynSuccess) {
        return status;
    }

    /*
    Calculating the limits for the angle is rather involved. Consider the
    following example:

    | *
    | |
    * |
    | |
    1 2

    where "*" is the position of the motor and "|" shows the available drive
    train. The center of rotation is in between axis "1" and axis "2" at
    position 0.5 (with axis 1 being at 0.0 and axis 2 being at 1.0). The guide
    between the two "*" has an elevation of 3 and a negative angle (by
    definition).

    Since axis 2 is at its limit, the angle clearly can't get smaller. When we
    want to increase the angle (moving axis 1 up and 2 down), axis 1 will hit
    its limits before axis 2. At the selene guide, we have 6 axes in total,
    meaning that we have to take into consideration each one of them.

    In order to do that, we loop through each axis and calculate the minimum
    (low limit) and maximum (high limit) absolute rotation angle allowed by this
    particular axis at its current position. We then select the smallest value
    for both low and high limit as the current axis limits. See README.md for
    the kinematic equations.
    */
    double smallestHighLimit = std::numeric_limits<double>::infinity();
    double smallestLowLimit = std::numeric_limits<double>::infinity();
    double liftPosition = 0.0;
    status = liftAxis_->motorPosition(&liftPosition);
    if (status != asynSuccess) {
        return status;
    }

    for (int i = 0; i < liftAxis_->numAxes_; i++) {
        seleneOffsetAxis *oAxis = liftAxis_->offsetAxis(i);

        double leverArm = oAxis->xOffset() - liftAxis_->xPos();

        double motPos = 0.0;
        status = oAxis->motorPosition(&motPos);
        if (status != asynSuccess) {
            return status;
        }

        /*
        If we are before the rotation center, the high limit of the offset axis
        must be used to calculate the high angle limit. If we are behind the
        rotation center (positive lever arm), the low limit of the offset axis
        must be used to calcate the high angle limit.
        */
        double calcHighLimit = 0.0;
        double calcLowLimit = 0.0;
        if (leverArm > 0) {
            calcHighLimit = oAxis->absoluteLowLimitLiftCS();
            calcLowLimit = oAxis->absoluteHighLimitLiftCS();
        } else {
            calcHighLimit = oAxis->absoluteHighLimitLiftCS();
            calcLowLimit = oAxis->absoluteLowLimitLiftCS();
        }

        // Since the angle coordinate system is inverted, we need to invert the
        // limits as well (minus sign).
        double angleHighLimit =
            -atan((calcHighLimit - liftPosition - motPos) / leverArm) * 180.0 /
            std::numbers::pi;
        double angleLowLimit =
            -atan((calcLowLimit - liftPosition - motPos) / leverArm) * 180.0 /
            std::numbers::pi;

        // We are interested in the absolute smallest limits
        if (std::abs(smallestHighLimit) > std::abs(angleHighLimit)) {
            smallestHighLimit = angleHighLimit;
        }
        if (std::abs(smallestLowLimit) > std::abs(angleLowLimit)) {
            smallestLowLimit = angleLowLimit;
        }
    }

    setAxisParamChecked(this, motorHighLimitFromDriver, smallestHighLimit);
    setAxisParamChecked(this, motorLowLimitFromDriver, smallestLowLimit);

    // This axis should always be seen as enabled, since it is automatically
    // enabled before each movement and disabled afterwards
    setAxisParamChecked(this, motorEnableRBV, true);

    // If an error message is set in the liftAxis_, copy this message into this
    // axis.
    getAxisParamChecked(liftAxis_, motorMessageText, &errorMessage);

    if (strlen(errorMessage) != 0) {
        setAxisParamChecked(this, motorMessageText, errorMessage);
        return asynError;
    }

    return status;
}

asynStatus seleneAngleAxis::doReset() { return liftAxis_->reset(); }

asynStatus seleneAngleAxis::enable(bool on) { return liftAxis_->enable(on); }

asynStatus seleneAngleAxis::readEncoderType() {
    setAxisParamChecked(this, encoderType, IncrementalEncoder);
    return asynSuccess;
}

asynStatus seleneAngleAxis::doHome(double minVelocity, double maxVelocity,
                                   double acceleration, int forwards) {
    return liftAxis_->doHome(minVelocity, maxVelocity, acceleration, forwards);
}

asynStatus seleneAngleAxis::normalizePositions() {
    return liftAxis_->normalizePositions();
}