Initial commit, WIP

This commit is contained in:
2025-03-20 13:58:18 +01:00
parent e977855dd3
commit 0eaa75dc4b
12 changed files with 1132 additions and 81 deletions
View File
+16
View File
@@ -0,0 +1,16 @@
#include "turboPmacAxis.h"
class offsetAxis : public turboPmacAxis {
public:
/**
* @brief Implementation of the `doPoll` function from sinqAxis. The
* parameters are described in the documentation of `sinqAxis::doPoll`.
*
* @param moving
* @return asynStatus
*/
asynStatus doPoll(bool *moving);
private:
seleneLiftAxis liftAxis_;
}
+12
View File
@@ -0,0 +1,12 @@
#include "seleneAngleAxis.h"
#include "seleneLiftAxis.h"
seleneAngleAxis::seleneAngleAxis(turboPmacController *pController, int axisNo) {
// Initialize the associated lift axis as a nullptr. It is populated in the
// constructor of seleneLiftAxis.
liftAxis_ = nullptr;
}
asynStatus seleneAngleAxis::stop(double acceleration) {
return liftAxis_->stop(acceleration);
}
+117
View File
@@ -0,0 +1,117 @@
#ifndef seleneAngleAxis_H
#define seleneAngleAxis_H
#include "turboPmacAxis.h"
#include "turboPmacController.h"
// Forward declaration of the seleneLiftAxis class to resolve the cyclic
// dependency between the seleneLiftAxis and the seleneAngleAxis .h-file.
// See https://en.cppreference.com/w/cpp/language/class.
class seleneLiftAxis;
/**
* @brief Virtual axis for setting the angle of a Selene guide
*/
class seleneAngleAxis : public turboPmacAxis {
public:
/**
* @brief Destroy the turboPmacAxis
*
*/
virtual ~seleneAngleAxis();
/**
* @brief Implementation of the `stop` function from asynMotorAxis
*
* @param acceleration Acceleration ACCEL from the motor record. This
* value is currently not used.
* @return asynStatus
*/
asynStatus stop(double acceleration);
/**
* @brief This function does (almost) nothing, since the poll is done
* directly in seleneLiftAxis::doPoll(). It justs sets *moving.
*
* @param moving
* @return asynStatus
*/
asynStatus poll(bool *moving);
/**
* @brief Set the target value for the guide angle and trigger a position
* collection cycle
*
* @param position
* @param relative
* @param min_velocity
* @param maxOffset_velocity
* @param acceleration
* @return asynStatus
*/
asynStatus doMove(double position, int relative, double min_velocity,
double maxOffset_velocity, double acceleration);
/**
* @brief Readout of some values from the controller at IOC startup
*
* The following steps are performed:
* - Read out the motor status, motor position, velocity and acceleration
* from the MCU and store this information in the parameter library.
* - Set the enable PV according to the initial status of the axis.
*
* @return asynStatus
*/
asynStatus init();
/**
* @brief Reset the error(s) of both physical motors
*
* @param on
* @return asynStatus
*/
asynStatus reset();
/**
* @brief Call the stop() method of the associated liftAxis.
*
* @param on
* @return asynStatus
*/
asynStatus enable(bool on);
/**
* @brief The encoder of both physical motors is absolute, hence the encoder
* of the virtual axis is also absolute
*
* @return asynStatus
*/
asynStatus readEncoderType();
/**
* @brief Trigger a rereading of the encoder position of both physical
* motors
*
* @return asynStatus
*/
asynStatus rereadEncoder();
protected:
turboPmacController *pC_;
seleneLiftAxis *liftAxis_;
private:
/**
* @brief Construct a new seleneAngleAxis
*
* @param pController Pointer to the associated controller
* @param axisNo Index of the axis
*/
seleneAngleAxis(turboPmacController *pController, int axisNo,
seleneLiftAxis *liftAxis);
// The friend class declaration here is necessary so the
// seleneLiftAxis cann call the constructor of seleneAngleAxis
friend class seleneLiftAxis;
};
#endif
+5
View File
@@ -0,0 +1,5 @@
#---------------------------------------------
# SINQ specific DB definitions
#---------------------------------------------
registrar(turboPmacControllerRegister)
registrar(turboPmacAxisRegister)
+445
View File
@@ -0,0 +1,445 @@
#include "seleneLiftAxis.h"
#include "asynOctetSyncIO.h"
#include "epicsExport.h"
#include "iocsh.h"
#include "turboPmacController.h"
#include <cmath>
#include <errlog.h>
#include <initHooks.h>
#include <limits>
#include <math.h>
#include <string.h>
#include <unistd.h>
#include <vector>
/*
Contains all instances of turboPmacAxis which have been created and is used in
the initialization hook function.
*/
static std::vector<seleneLiftAxis *> axes;
/**
* @brief Hook function to perform certain actions during the IOC initialization
*
* @param iState
*/
static void epicsInithookFunction(initHookState iState) {
if (iState == initHookAfterDatabaseRunning) {
// Iterate through all axes of each and call the initialization method
// on each one of them.
for (std::vector<seleneLiftAxis *>::iterator itA = axes.begin();
itA != axes.end(); ++itA) {
turboPmacAxis *axis = *itA;
axis->init();
}
}
}
seleneLiftAxis::seleneLiftAxis(turboPmacController *pC, int liftAxisNo,
int angleAxisNo, int realAxis1No,
int realAxis2No)
: turboPmacAxis(pC, liftAxisNo, 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);
}
/*
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);
// Initialize the parameters
xLift_ = 0.0; // Overwritten in the init function
receivedTarget_ = false;
// Register the hook function during construction of the first axis
// object
if (axes.empty()) {
initHookRegister(&epicsInithookFunction);
}
// Collect all axes into this list which will be used in the hook
// function
axes.push_back(this);
}
seleneLiftAxis::~seleneLiftAxis() {}
asynStatus seleneLiftAxis::init() {
// Local variable declaration
asynStatus status = asynSuccess;
double motorRecResolution = 0.0;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
int nvals = 0;
// The parameter library takes some time to be initialized. Therefore we
// wait until the status is not asynParamUndefined anymore.
time_t now = time(NULL);
time_t maxInitTime = 60;
while (1) {
status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
&motorRecResolution);
if (status == asynParamUndefined) {
if (now + maxInitTime < time(NULL)) {
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line "
"%d\nInitializing the parameter library failed.\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__,
__LINE__);
return asynError;
}
} else if (status == asynSuccess) {
break;
} else if (status != asynSuccess) {
return pC_->paramLibAccessFailed(status, "motorRecResolution_",
axisNo_, __PRETTY_FUNCTION__,
__LINE__);
}
}
// Read out the horizontal distances of all other axes from axis 1
xOffset_[0] = 0.0; // By definition
snprintf(command, sizeof(command), "Q652 Q653 Q654 Q655 Q656");
status = pC_->writeRead(axisNo_, command, response, 5);
if (status != asynSuccess) {
return status;
}
nvals = sscanf(response, "%lf %lf %lf %lf %lf", &xOffset_[1], &xOffset_[2],
&xOffset_[3], &xOffset_[4], &xOffset_[5]);
if (nvals != 5) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Center between axis 2 and 3
xLift_ = 0.5 * (xOffset_[2] + xOffset_[3]);
// Read out the vertical distances of all other axes from axis 1
zOffset_[0] = 0.0; // By definition
zOffset_[1] = 0.0;
zOffset_[2] = 0.0;
zOffset_[3] = 0.0;
zOffset_[4] = 0.0;
zOffset_[5] = 0.0;
return normalizeOffsets();
}
asynStatus seleneLiftAxis::normalizeOffsets() {
asynStatus status = asynSuccess;
char response[pC_->MAXBUF_];
int nvals = 0;
// Read out the absolute positions of all axes
const char *command = "Q0110 Q0210 Q0310 Q0410 Q0510 Q0610";
status = pC_->writeRead(axisNo_, command, response, 6);
if (status != asynSuccess) {
return status;
}
double z[6] = {0.0};
nvals = sscanf(response, "%lf %lf %lf %lf %lf %lf", &z[0], &z[1], &z[2],
&z[3], &z[4], &z[5]);
if (nvals != 2) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Normalization
for (int i = 0; i < numAxes_; i++) {
z[i] = z[i] - zOffset_[i];
}
// Lift position is simply the mean of axis 2 and 3
status = setDoubleParam(pC_->motorPosition(), 0.5 * (z[2] + z[3]));
if (status != asynSuccess) {
return pC_->paramLibAccessFailed(status, "motorPosition_", axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// tan(angle) = (z3 - z2) / (x3 - x2)
double alpha = atan2(z[3] - z[2], xOffset_[3] - xOffset_[2]);
status = angleAxis_->setDoubleParam(pC_->motorPosition(), alpha);
if (status != asynSuccess) {
return pC_->paramLibAccessFailed(status, "motorPosition_", axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Adjust the position values of all physical axes accordingly. The values
// of axes 2 and 3 should result in zero, if not, something went wrong.
double tanAlpha = tan(alpha);
for (int i = 0; i < numAxes_; i++) {
// Calculate the new offset
double x = xOffset_[i] - xLift_;
double z = z[i] - x * tanAlpha;
status = axes_[i].setDoubleParam(pC_->motorPosition(), z);
if (status != asynSuccess) {
return pC_->paramLibAccessFailed(status, "motorPosition_", axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// TODO: Adjust limits
}
// Update all axes
callParamCallbacks();
angleAxis_->callParamCallbacks();
for (int i = 0; i < numAxes_; i++) {
axes_[i].callParamCallbacks();
}
}
asynStatus seleneLiftAxis::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 response[pC_->MAXBUF_], userMessage[pC_->MAXBUF_];
int isMoving = 0;
int nvals = 0;
int error = 0;
int axStatus = 0;
double posAx1 = 0.0;
double highLimitAx1 = 0.0;
double lowLimitAx1 = 0.0;
double posAx6 = 0.0;
double highLimitAx6 = 0.0;
double lowLimitAx6 = 0.0;
// =========================================================================
// Read the positions and the limits of axis 1 and 6 as well as the status
// and the error of the virtual axis
const char *command = "status error Q0100 Q0113 Q0114 Q0600 Q0613 Q0614";
rw_status = pC_->writeRead(axisNo_, command, response, 8);
if (rw_status != asynSuccess) {
return rw_status;
}
nvals = sscanf(response, "%d %d %lf %lf %lf %lf %lf %lf", &axStatus, &error,
&posAx1, &highLimitAx1, &lowLimitAx1, &posAx6, &highLimitAx6,
&lowLimitAx6);
if (nvals != 8) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
/*
The following code converts from the vertical height readback values of axis
1 and 6 to a virtual lift position and a corresponding angle. To do that, we
need first to correct for the different starting point of both axes (see
documentation of member variable yDistAx1ToAx6_)
*/
posAx6 = posAx6 - yDistAx1ToAx6_;
highLimitAx6 = highLimitAx6 - yDistAx1ToAx6_;
lowLimitAx6 = lowLimitAx6 - yDistAx1ToAx6_;
/*
Convert the z-axis position values of axis 1 and 6 into an angle and a
lift, using the center position between axis 1 and 6 as the rotation point.
The angle is the arcustangens(opposite/adjacent), where the adjacent is
the distance between axis 1 and 6 and the opposite is the difference
between the height of axis 6 and axis 1.
*/
double liftPos = 0.5 * (posAx1 + posAx6);
double liftHighLimit = 0.5 * (highLimitAx1 + highLimitAx6);
double liftLowLimit = 0.5 * (lowLimitAx1 + lowLimitAx6);
double anglePos = atan2(posAx6 - posAx1, xOffset_[5]);
/*
The angle limits are defined from the center position, hence the lift
position needs to be subtracted from the high and low limits of angle 6. See
the documentation in README.md for details.
*/
double angleHighLimit = atan2(highLimitAx6 - liftPos, xOffset_[5]);
double angleLowLimit = atan2(lowLimitAx6 - liftPos, xOffset_[5]);
// Set the RBV values for both axes
pl_status = setIntegerParam(pC_->motorStatusMoving(), isMoving);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_",
axisNo_, __PRETTY_FUNCTION__,
__LINE__);
}
pl_status = angleAxis_->setIntegerParam(pC_->motorStatusMoving(), isMoving);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_",
angleAxis_->axisNo(),
__PRETTY_FUNCTION__, __LINE__);
}
pl_status = setIntegerParam(pC_->motorStatusDone(), isMoving == 0);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_", axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
pl_status =
angleAxis_->setIntegerParam(pC_->motorStatusDone(), isMoving == 0);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_",
angleAxis_->axisNo(),
__PRETTY_FUNCTION__, __LINE__);
}
// If any of the physical axes has a status problem, the two virtual axes
// have a status problem
// Set the status of the two virtual axes
pl_status = setIntegerParam(pC_->motorStatusProblem(),
(realAxis1StatProb || realAxis2StatProb));
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
axisNo_, __PRETTY_FUNCTION__,
__LINE__);
}
pl_status = angleAxis_->setIntegerParam(
pC_->motorStatusProblem(), (realAxis1StatProb || realAxis2StatProb));
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
angleAxis_->axisNo(),
__PRETTY_FUNCTION__, __LINE__);
}
pl_status = setStringParam(pC_->motorMessageText(), userMessage);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
axisNo_, __PRETTY_FUNCTION__,
__LINE__);
}
pl_status =
angleAxis_->setStringParam(pC_->motorMessageText(), userMessage);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
angleAxis_->axisNo(),
__PRETTY_FUNCTION__, __LINE__);
}
// Calculate the position and the angle
double realAxis1Pos = 0;
double realAxis2Pos = 0;
pl_status = pC_->getDoubleParam(realAxis1_->axisNo(), pC_->motorPosition(),
&realAxis1Pos);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
realAxis1_->axisNo(),
__PRETTY_FUNCTION__, __LINE__);
}
pl_status = pC_->getDoubleParam(realAxis2_->axisNo(), pC_->motorPosition(),
&realAxis2Pos);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
realAxis2_->axisNo(),
__PRETTY_FUNCTION__, __LINE__);
}
// Update the parameter library for the seleneAngleAxis (the update for the
// lift axis itself is done in the body of the parent poll() method)
pl_status = angleAxis_->callParamCallbacks();
if (pC_->getMsgPrintControl().shouldBePrinted(
pC_->portName, angleAxis_->axisNo(), __PRETTY_FUNCTION__, __LINE__,
(pl_status != asynSuccess), pC_->asynUserSelf())) {
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line "
"%d:\ncallParamCallbacks failed with %s.%s\n",
pC_->portName, angleAxis_->axisNo(), __PRETTY_FUNCTION__,
__LINE__, pC_->stringifyAsynStatus(poll_status),
pC_->getMsgPrintControl().getSuffix());
}
}
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) {
return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
axisNo_, __PRETTY_FUNCTION__,
__LINE__);
}
// Signal to the deferredMovementCollectorLoop that a movement should be
// started to the defined target position.
targetPosition_ = position * motorRecResolution;
return asynSuccess;
}
asynStatus seleneLiftAxis::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 response[pC_->MAXBUF_];
// =========================================================================
// Stop all axes
const char *command = "P150=8";
rw_status = pC_->writeRead(axisNo_, command, response, 0);
if (rw_status != asynSuccess) {
asynPrint(
pC_->asynUserSelf(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d\nStopping the movement "
"failed.\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
pl_status = setIntegerParam(pC_->motorStatusProblem(), true);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
axisNo_, __PRETTY_FUNCTION__,
__LINE__);
}
}
return rw_status;
}
+147
View File
@@ -0,0 +1,147 @@
#ifndef seleneLiftAxis_H
#define seleneLiftAxis_H
#include "seleneAngleAxis.h"
#include "turboPmacAxis.h"
#include "turboPmacController.h"
/**
* @brief Virtual axis for setting the height of the center of a Selene guide
*
* This axis acts as the "master" for a partnering seleneAngleAxis (i.e. it
* controls the movement and polls the physical motors)
*/
class seleneLiftAxis : public turboPmacAxis {
public:
seleneLiftAxis(turboPmacController *pController, int liftAxisNo,
int angleAxisNo, int realAxis1No, int realAxis2No);
/**
* @brief Destroy the seleneLiftAxis
*
*/
virtual ~seleneLiftAxis();
/**
* @brief Implementation of the `stop` function from asynMotorAxis
*
* @param acceleration Acceleration ACCEL from the motor record. This
* value is currently not used.
* @return asynStatus
*/
asynStatus stop(double acceleration);
/**
* @brief Implementation of the `doPoll` function from sinqAxis. The
* parameters are described in the documentation of `sinqAxis::doPoll`.
*
* @param moving
* @return asynStatus
*/
asynStatus doPoll(bool *moving);
/**
* @brief Set the target value for the detector angle and trigger a position
* collection cycle
*
* @param position
* @param relative
* @param min_velocity
* @param maxOffset_velocity
* @param acceleration
* @return asynStatus
*/
asynStatus doMove(double position, int relative, double min_velocity,
double maxOffset_velocity, double acceleration);
/**
* @brief Start a movement to the target positions of this axis and the
* attached seleneAngleAxis.
*
* @return asynStatus
*/
asynStatus startCombinedMove();
/**
* @brief Readout of some values from the controller at IOC startup
*
* The following steps are performed:
* - Read out the motor status, motor position, velocity and acceleration
* from the MCU and store this information in the parameter library.
* - Set the enable PV according to the initial status of the axis.
*
* @return asynStatus
*/
asynStatus init();
/**
* @brief Reset the error(s) of both physical motors
*
* @param on
* @return asynStatus
*/
asynStatus reset();
/**
* @brief Enable / disable both physical motors
*
* @param on
* @return asynStatus
*/
asynStatus enable(bool on);
/**
* @brief Override of the home function of asynMotorAxis, which does nothing
*
* @param on
* @return asynStatus
*/
asynStatus home(double min_velocity, double maxOffset_velocity,
double acceleration, int forwards) {
return asynSuccess;
}
/**
* @brief The encoder of both physical motors is absolute, hence the encoder
* of the virtual axis is also absolute
*
* @return asynStatus
*/
asynStatus readEncoderType();
/**
* @brief Trigger a rereading of the encoder position of both physical
* motors
*
* @return asynStatus
*/
asynStatus rereadEncoder();
// TODO
asynStatus normalizeOffsets();
/**
* @brief Calculate the
*
* @param lift
* @param angle
* @return asynStatus
*/
double[6] calculateMotorPositions(double lift, double angle);
protected:
static const int numAxes_ = 6;
turboPmacController *pC_;
seleneAngleAxis *angleAxis_;
turboPmacAxis axes_[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)
double zOffset_[numAxes_];
double xLift_ = 0.0;
};
#endif