lin-epics-modules/seleneguide
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

WIP

Browse Source
This commit is contained in:
mathis_s
2025-03-21 10:17:54 +01:00
parent 0eaa75dc4b
commit a58671323d
4 changed files with 90 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/offsetAxis.cpp
View File

@ -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
}

5
src/offsetAxis.h
View File

@ -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_;
}

98
src/seleneLiftAxis.cpp
View File

@ -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,
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<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;
}

36
src/seleneLiftAxis.h
View File

@ -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>
*/
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