diff --git a/src/offsetAxis.cpp b/src/offsetAxis.cpp index e69de29..bf6973e 100644 --- a/src/offsetAxis.cpp +++ b/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 +} \ No newline at end of file diff --git a/src/offsetAxis.h b/src/offsetAxis.h index cb4ca8c..aa2078d 100644 --- a/src/offsetAxis.h +++ b/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_; } \ No newline at end of file diff --git a/src/seleneLiftAxis.cpp b/src/seleneLiftAxis.cpp index f1786f3..830eea9 100644 --- a/src/seleneLiftAxis.cpp +++ b/src/seleneLiftAxis.cpp @@ -2,6 +2,7 @@ #include "asynOctetSyncIO.h" #include "epicsExport.h" #include "iocsh.h" +#include "offserAxis.h" #include "turboPmacController.h" #include #include @@ -35,50 +36,22 @@ static void epicsInithookFunction(initHookState iState) { } } -seleneLiftAxis::seleneLiftAxis(turboPmacController *pC, int liftAxisNo, - int angleAxisNo, int realAxis1No, - int realAxis2No) - : turboPmacAxis(pC, liftAxisNo, false), pC_(pC) { +seleneLiftAxis::seleneLiftAxis(turboPmacController *pC) + : turboPmacAxis(pC, numAxes_ + 1, false), pC_(pC) { - /* - The underlying physical / real axes need to be polled before the virtual - axes for lift and angle. To ensure this, the axis indices of the real axes - must be smaller than those of the virtual axes. If this is not the case, the - configuration is incorrect and therefore the IOC must be terminated. - */ - 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); + // Create the real axes and the virtual angle axis. + // They are deallocated in the destructor of sinqController + for (int i = 0; i < numAxes_; i++) { + offsetAxis axis = new offsetAxis(pC, i, this); + offsetAxes_[i] = axis; } - /* - The code below assumes that the first axis is the one with the lower x-value - 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); + // + angleAxis_ = new seleneAngleAxis(pC, numAxes_ + 2, 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 seleneLiftAxis::calculateMotorPositions(double lift, + double angle) { + // Initialize the array containing the positions + std::array 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; } \ No newline at end of file diff --git a/src/seleneLiftAxis.h b/src/seleneLiftAxis.h index a6158d2..8a14fd9 100644 --- a/src/seleneLiftAxis.h +++ b/src/seleneLiftAxis.h @@ -12,8 +12,7 @@ */ class seleneLiftAxis : public turboPmacAxis { public: - seleneLiftAxis(turboPmacController *pController, int liftAxisNo, - int angleAxisNo, int realAxis1No, int realAxis2No); + seleneLiftAxis(turboPmacController *pController); /** * @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 - * @param angle - * @return asynStatus + * In order to calculate the positions, this function convertes the given + * lift and angle into a linear equation. The x-values of the axes are read + * 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] calculateMotorPositions(double lift, double angle); + std::array 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