WIP

src/offsetAxis.cpp

@@ -0,0 +1,9 @@
+#include "offsetAxis.h"
+
+asynStatus offsetAxis::doPoll(bool *moving) {
+    // Perform the "normal" poll of a normal turboPmac
+    asynStatus status = turboPmac::doPoll(moving);
+
+    // Convert the motorPosition_ value in the parameter library from an
+    // absolute value to an offset
+}

src/offsetAxis.h

@@ -1,7 +1,11 @@
+#include "seleneLiftAxis.h"
 #include "turboPmacAxis.h"
 
 class offsetAxis : public turboPmacAxis {
   public:
+    offsetAxis(turboPmacController *pController, int axisNo,
+               seleneLiftAxis *liftAxis);
+
     /**
      * @brief Implementation of the `doPoll` function from sinqAxis. The
      * parameters are described in the documentation of `sinqAxis::doPoll`.
@@ -12,5 +16,6 @@ class offsetAxis : public turboPmacAxis {
     asynStatus doPoll(bool *moving);
 
   private:
+    double targetPosition_;
     seleneLiftAxis liftAxis_;
 }

src/seleneLiftAxis.cpp

@@ -2,6 +2,7 @@
 #include "asynOctetSyncIO.h"
 #include "epicsExport.h"
 #include "iocsh.h"
+#include "offserAxis.h"
 #include "turboPmacController.h"
 #include <cmath>
 #include <errlog.h>
@@ -35,50 +36,22 @@ static void epicsInithookFunction(initHookState iState) {
     }
 }
 
-seleneLiftAxis::seleneLiftAxis(turboPmacController *pC, int liftAxisNo,
+seleneLiftAxis::seleneLiftAxis(turboPmacController *pC)
-                               int angleAxisNo, int realAxis1No,
+    : turboPmacAxis(pC, numAxes_ + 1, false), pC_(pC) {
-                               int realAxis2No)
-    : turboPmacAxis(pC, liftAxisNo, false), pC_(pC) {
 
-    /*
+    // Create the real axes and the virtual angle axis.
-    The underlying physical / real axes need to be polled before the virtual
+    // They are deallocated in the destructor of sinqController
-    axes for lift and angle. To ensure this, the axis indices of the real axes
+    for (int i = 0; i < numAxes_; i++) {
-    must be smaller than those of the virtual axes. If this is not the case, the
+        offsetAxis axis = new offsetAxis(pC, i, this);
-    configuration is incorrect and therefore the IOC must be terminated.
+        offsetAxes_[i] = axis;
-    */
-    if (realAxis1No >= liftAxisNo || realAxis1No >= angleAxisNo ||
-        realAxis2No >= liftAxisNo || realAxis2No >= angleAxisNo) {
-        asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
-                  "Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
-                  "(Axes indices of the real axes (set to %d and %d) must be "
-                  "smaller than those of the virtual axes (set to %d and "
-                  "%d))\n. Terminating IOC",
-                  pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, axisNo,
-                  realAxis1No, realAxis2No, liftAxisNo, angleAxisNo);
-        exit(-1);
     }
 
-    /*
+    //
-    The code below assumes that the first axis is the one with the lower x-value
+    angleAxis_ = new seleneAngleAxis(pC, numAxes_ + 2, this);
-    on the fixed bearing and the second axis is the one on the loose bearing
-    with the higher x-value. This assumption is asserted here by the indices
-    */
-    if (realAxis1No > realAxis2No) {
-        asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
-                  "Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
-                  "(Index of axis 1 (%d) must be smaller that that of axis 2 "
-                  "(%d))\n. Terminating IOC",
-                  pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, axisNo,
-                  realAxis1No, realAxis2No);
-        exit(-1);
-    }
-
-    // The axes are deallocated in the destructor of sinqController
-    angleAxis_ = new seleneAngleAxis(pC, angleAxisNo, this);
 
     // Initialize the parameters
     xLift_ = 0.0; // Overwritten in the init function
-    receivedTarget_ = false;
+    targetPosition_ = 0.0;
 
     // Register the hook function during construction of the first axis
     // object
@@ -391,11 +364,6 @@ asynStatus seleneLiftAxis::doPoll(bool *moving) {
 asynStatus seleneLiftAxis::doMove(double position, int relative,
                                   double min_velocity, double max_velocity,
                                   double acceleration) {
-
-    // Calculate the position of all 6 underlying physical motors and send a
-    // combined move command to the MCU
-
-    double motorRecResolution = 0.0;
     asynStatus pl_status = pC_->getDoubleParam(
         axisNo_, pC_->motorRecResolution(), &motorRecResolution);
     if (pl_status != asynSuccess) {
@@ -403,12 +371,32 @@ asynStatus seleneLiftAxis::doMove(double position, int relative,
                                          axisNo_, __PRETTY_FUNCTION__,
                                          __LINE__);
     }
-
-    // Signal to the deferredMovementCollectorLoop that a movement should be
-    // started to the defined target position.
     targetPosition_ = position * motorRecResolution;
 
-    return asynSuccess;
+    return startCombinedMove();
+}
+
+asynStatus seleneLiftAxis::startCombinedMove() {
+
+    char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
+
+    auto targetPositions =
+        calculateMotorPositions(targetPosition_, angleAxis_->targetPosition_);
+
+    // Apply the offsets of the individual offset axes
+    for (int i = 0, i < numAxes_, i++) {
+        targetPositions[i] =
+            targetPositions[i] + offsetAxes_[i]->targetPosition_;
+    }
+
+    // Set all target positions and send the move command
+    snprintf(command, sizeof(command),
+             "Q151=%lf Q152=%lf Q153=%lf Q154=%lf Q155=%lf Q156=%lf P150=1",
+             targetPositions[0], targetPositions[1], targetPositions[2],
+             targetPositions[3], targetPositions[4], targetPositions[5]);
+
+    // No answer expected
+    return pC_->writeRead(axisNo_, command, response, 0);
 }
 
 asynStatus seleneLiftAxis::stop(double acceleration) {
@@ -442,4 +430,20 @@ asynStatus seleneLiftAxis::stop(double acceleration) {
     }
 
     return rw_status;
+}
+
+std::array<double, 6> seleneLiftAxis::calculateMotorPositions(double lift,
+                                                              double angle) {
+    // Initialize the array containing the positions
+    std::array<double, 6> positions;
+
+    // Calculate the individual positions by applying
+    // y = lift + tan(angle)*(x-xLift_). Apply the individual offset of the axis
+    // compared to that of the virtual axis (which is on the same level as axis
+    // 1)  as well.
+    double tanAngle = tan(angle);
+    for (int i = 0, i < numAxes_, i++) {
+        positions[i] = lift + tanAngle * (xOffset_[i] - xLift_) - zOffset_[i];
+    }
+    return positions;
 }

src/seleneLiftAxis.h

@@ -12,8 +12,7 @@
  */
 class seleneLiftAxis : public turboPmacAxis {
   public:
-    seleneLiftAxis(turboPmacController *pController, int liftAxisNo,
+    seleneLiftAxis(turboPmacController *pController);
-                   int angleAxisNo, int realAxis1No, int realAxis2No);
 
     /**
      * @brief Destroy the seleneLiftAxis
@@ -116,32 +115,47 @@ class seleneLiftAxis : public turboPmacAxis {
      */
     asynStatus rereadEncoder();
 
-    // TODO
+    // Calculate
     asynStatus normalizeOffsets();
 
     /**
-     * @brief Calculate the
+     * @brief Calculate the positions of all 6 real axes from the given lift and
+     * angle. This function does not change any values in the parameter library
+     * and does not communicate with the controller.
      *
-     * @param lift
+     * In order to calculate the positions, this function convertes the given
-     * @param angle
+     * lift and angle into a linear equation. The x-values of the axes are read
-     * @return asynStatus
+     * out from the controller in the constructor, which allows us to calculate
+     * the corresponding y-values. These are then corrected by the zero offset
+     * of the corresponding axis in order to calculate the actual positon.
+     *
+     * The corresponding equation is:
+     * y = lift + tan(angle)*(x-xLift_)
+     *
+     * @param lift    Position of the virtual lift axis in mm
+     * @param angle   Angle of the virtual angle axis in radian
+     * @return std::array<double, 6>
      */
-    double[6] calculateMotorPositions(double lift, double angle);
+    std::array<double, 6> calculateMotorPositions(double lift, double angle);
 
   protected:
     static const int numAxes_ = 6;
 
     turboPmacController *pC_;
     seleneAngleAxis *angleAxis_;
-    turboPmacAxis axes_[numAxes_];
+    turboPmacAxis offsetAxes_[numAxes_];
 
     // Horizontal distance from the first axis in mm (which is located at 0 mm)
     double xOffset_[numAxes_];
 
-    // Vertical distance from the first axis in mm (which is located at 0 mm)
+    // Vertical offset from the first axis in mm (which is located at 0 mm)
     double zOffset_[numAxes_];
 
-    double xLift_ = 0.0;
+    // Horizontal position of the virtual lift axis
+    double xLift_;
+
+    // Target position for the lift
+    double targetPosition_;
 };
 
 #endif