Initial commit, WIP
This commit is contained in:
445
src/seleneLiftAxis.cpp
Normal file
445
src/seleneLiftAxis.cpp
Normal 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;
|
||||
}
|
||||
Reference in New Issue
Block a user