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) {
        pC_->paramLibAccessFailed(status, "motorCanDisable", axisNo_,
                                  __PRETTY_FUNCTION__, __LINE__);
    }

    // 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);
    }
}

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

asynStatus seleneAngleAxis::doMove(double position, int relative,
                                   double min_velocity, double max_velocity,
                                   double acceleration) {
    double motorRecResolution = 0.0;

    asynStatus pl_status = pC_->getDoubleParam(
        axisNo_, pC_->motorRecResolution(), &motorRecResolution);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
                                         axisNo_, __PRETTY_FUNCTION__,
                                         __LINE__);
    }
    targetPosition_ = position * motorRecResolution;
    targetSet_ = true;
    return liftAxis_->startCombinedMoveFromVirtualAxis();
    targetSet_ = false;
}

asynStatus seleneAngleAxis::targetPosition(double *targetPosition) {
    asynStatus status = asynSuccess;

    if (targetSet_) {
        *targetPosition = targetPosition_;
    } else {
        status = motorPosition(targetPosition);
    }
    return status;
}

asynStatus seleneAngleAxis::doPoll(bool *moving) {

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

    // In the doPoll method of `seleneLiftAxis`, the parameters
    // `motorStatusMoving` and `motorStatusDone` of this axis have already been
    // set accordingly.
    int isMoving = 0;

    asynStatus pl_status =
        pC_->getIntegerParam(axisNo(), pC_->motorStatusMoving(), &isMoving);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving",
                                         axisNo(), __PRETTY_FUNCTION__,
                                         __LINE__);
    }
    *moving = (isMoving != 0);

    double motPos = 0.0;
    pl_status = motorPosition(&motPos);
    if (pl_status != asynSuccess) {
        return pl_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;
    pl_status = liftAxis_->motorPosition(&liftPosition);
    if (pl_status != asynSuccess) {
        return pl_status;
    }

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

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

        double motPos = 0.0;
        pl_status = oAxis->motorPosition(&motPos);
        if (pl_status != asynSuccess) {
            return pl_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;
        }
    }

    pl_status = pC_->setDoubleParam(axisNo_, pC_->motorHighLimitFromDriver(),
                                    smallestHighLimit);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver",
                                         axisNo_, __PRETTY_FUNCTION__,
                                         __LINE__);
    }

    pl_status = pC_->setDoubleParam(axisNo_, pC_->motorLowLimitFromDriver(),
                                    smallestLowLimit);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorLowLimitFromDriver",
                                         axisNo_, __PRETTY_FUNCTION__,
                                         __LINE__);
    }

    // This axis should always be seen as enabled, since it is automatically
    // enabled before each movement and disabled afterwards
    pl_status = setIntegerParam(pC_->motorEnableRBV(), 1);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorEnableRBV_", axisNo_,
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    // If an error message is set in the liftAxis_, copy this message into this
    // axis.
    pl_status =
        pC_->getStringParam(liftAxis_->axisNo(), pC_->motorMessageText(),
                            sizeof(errorMessage), errorMessage);
    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "motorMessageText",
                                         liftAxis_->axisNo(),
                                         __PRETTY_FUNCTION__, __LINE__);
    }

    if (strlen(errorMessage) != 0) {
        pl_status = setStringParam(pC_->motorMessageText(), errorMessage);
        if (pl_status != asynSuccess) {
            return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
                                             axisNo_, __PRETTY_FUNCTION__,
                                             __LINE__);
        }
        return asynError;
    }

    return pl_status;
}

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

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

asynStatus seleneAngleAxis::readEncoderType() {
    asynStatus pl_status =
        setStringParam(pC_->encoderType(), IncrementalEncoder);

    if (pl_status != asynSuccess) {
        return pC_->paramLibAccessFailed(pl_status, "encoderType_", axisNo_,
                                         __PRETTY_FUNCTION__, __LINE__);
    }
    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();
}