Compare commits
16 Commits
pmacV3-rew
...
master
Author | SHA1 | Date | |
---|---|---|---|
354e9d90fb | |||
deea821e3f | |||
7a46788fd5 | |||
9e77eb585c | |||
d0c009ea38 | |||
7e1fc78f76 | |||
9e0d8a4322 | |||
3cccfe930c | |||
8860d0c59f | |||
b6c38be113 | |||
b14b50c25a | |||
477ffdbc0b | |||
0a23ec8f22 | |||
eb1bb58c36 | |||
80205727c7 | |||
39098fd0d1 |
@ -9,28 +9,24 @@ AlignConsecutiveAssignments:
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCompound: false
|
||||
AlignFunctionPointers: false
|
||||
PadOperators: true
|
||||
AlignConsecutiveBitFields:
|
||||
Enabled: false
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCompound: false
|
||||
AlignFunctionPointers: false
|
||||
PadOperators: false
|
||||
AlignConsecutiveDeclarations:
|
||||
Enabled: false
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCompound: false
|
||||
AlignFunctionPointers: false
|
||||
PadOperators: false
|
||||
AlignConsecutiveMacros:
|
||||
Enabled: false
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCompound: false
|
||||
AlignFunctionPointers: false
|
||||
PadOperators: false
|
||||
AlignConsecutiveShortCaseStatements:
|
||||
Enabled: false
|
||||
@ -44,10 +40,8 @@ AlignTrailingComments:
|
||||
OverEmptyLines: 0
|
||||
AllowAllArgumentsOnNextLine: true
|
||||
AllowAllParametersOfDeclarationOnNextLine: true
|
||||
AllowBreakBeforeNoexceptSpecifier: Never
|
||||
AllowShortBlocksOnASingleLine: Never
|
||||
AllowShortCaseLabelsOnASingleLine: false
|
||||
AllowShortCompoundRequirementOnASingleLine: true
|
||||
AllowShortEnumsOnASingleLine: true
|
||||
AllowShortFunctionsOnASingleLine: All
|
||||
AllowShortIfStatementsOnASingleLine: Never
|
||||
@ -81,8 +75,7 @@ BraceWrapping:
|
||||
SplitEmptyFunction: true
|
||||
SplitEmptyRecord: true
|
||||
SplitEmptyNamespace: true
|
||||
BreakAdjacentStringLiterals: true
|
||||
BreakAfterAttributes: Leave
|
||||
BreakAfterAttributes: Never
|
||||
BreakAfterJavaFieldAnnotations: false
|
||||
BreakArrays: true
|
||||
BreakBeforeBinaryOperators: None
|
||||
@ -167,7 +160,6 @@ PenaltyBreakBeforeFirstCallParameter: 19
|
||||
PenaltyBreakComment: 300
|
||||
PenaltyBreakFirstLessLess: 120
|
||||
PenaltyBreakOpenParenthesis: 0
|
||||
PenaltyBreakScopeResolution: 500
|
||||
PenaltyBreakString: 1000
|
||||
PenaltyBreakTemplateDeclaration: 10
|
||||
PenaltyExcessCharacter: 1000000
|
||||
@ -185,7 +177,6 @@ RequiresClausePosition: OwnLine
|
||||
RequiresExpressionIndentation: OuterScope
|
||||
SeparateDefinitionBlocks: Leave
|
||||
ShortNamespaceLines: 1
|
||||
SkipMacroDefinitionBody: false
|
||||
SortIncludes: CaseSensitive
|
||||
SortJavaStaticImport: Before
|
||||
SortUsingDeclarations: LexicographicNumeric
|
||||
@ -207,7 +198,6 @@ SpaceBeforeParensOptions:
|
||||
AfterFunctionDeclarationName: false
|
||||
AfterIfMacros: true
|
||||
AfterOverloadedOperator: false
|
||||
AfterPlacementOperator: true
|
||||
AfterRequiresInClause: false
|
||||
AfterRequiresInExpression: false
|
||||
BeforeNonEmptyParentheses: false
|
||||
@ -233,7 +223,7 @@ StatementAttributeLikeMacros:
|
||||
StatementMacros:
|
||||
- Q_UNUSED
|
||||
- QT_REQUIRE_VERSION
|
||||
TabWidth: 8
|
||||
TabWidth: 4
|
||||
UseTab: Never
|
||||
VerilogBreakBetweenInstancePorts: true
|
||||
WhitespaceSensitiveMacros:
|
51
.gitlab-ci.yml
Normal file
51
.gitlab-ci.yml
Normal file
@ -0,0 +1,51 @@
|
||||
default:
|
||||
image: docker.psi.ch:5000/wall_e/sinqepics:latest
|
||||
|
||||
stages:
|
||||
- test
|
||||
- build
|
||||
|
||||
cppcheck:
|
||||
stage: test
|
||||
script:
|
||||
- cppcheck --std=c++17 --addon=cert --addon=misc --error-exitcode=1 sinqEPICSApp/
|
||||
allow_failure: true # Long term this needs to be removed
|
||||
artifacts:
|
||||
expire_in: 1 week
|
||||
tags:
|
||||
- docker
|
||||
|
||||
formatting:
|
||||
stage: test
|
||||
script:
|
||||
- clang-format --style=file --Werror --dry-run sinqEPICSApp/src/*.cpp sinqEPICSApp/src/*.c sinqEPICSApp/src/*.h
|
||||
allow_failure: true # Long term this needs to be removed
|
||||
artifacts:
|
||||
expire_in: 1 week
|
||||
tags:
|
||||
- docker
|
||||
|
||||
# clangtidy:
|
||||
# stage: test
|
||||
# script:
|
||||
# - curl https://docker.psi.ch:5000/v2/_catalog
|
||||
# # - dnf update -y
|
||||
# # - dnf install -y clang-tools-extra
|
||||
# # - clang-tidy sinqEPICSApp/src/*.cpp sinqEPICSApp/src/*.c sinqEPICSApp/src/*.h -checks=cppcoreguidelines-*,cert-*
|
||||
# # tags:
|
||||
# # - docker
|
||||
|
||||
build_module:
|
||||
stage: build
|
||||
script:
|
||||
- sed -i 's/ARCH_FILTER=.*/ARCH_FILTER=linux%/' Makefile.RHEL8
|
||||
- make -f Makefile.RHEL8 install
|
||||
- cp -rT "/ioc/modules/sinq/$(ls -U /ioc/modules/sinq/ | head -1)" "./sinq-${CI_COMMIT_SHORT_SHA}"
|
||||
artifacts:
|
||||
name: "sinq-${CI_COMMIT_SHORT_SHA}"
|
||||
paths:
|
||||
- "sinq-${CI_COMMIT_SHORT_SHA}/*"
|
||||
expire_in: 1 week
|
||||
when: always
|
||||
tags:
|
||||
- docker
|
@ -4,21 +4,19 @@ include /ioc/tools/driver.makefile
|
||||
MODULE=sinq
|
||||
BUILDCLASSES=Linux
|
||||
EPICS_VERSIONS=7.0.7
|
||||
ARCH_FILTER=RHEL%
|
||||
ARCH_FILTER=RHEL8%
|
||||
|
||||
# additional module dependencies
|
||||
REQUIRED+=SynApps
|
||||
REQUIRED+=stream
|
||||
REQUIRED+=scaler
|
||||
REQUIRED+=asynMotor
|
||||
|
||||
# Release version
|
||||
LIBVERSION=2024-newPmacV3
|
||||
REQUIRED+=motorBase
|
||||
|
||||
# DB files to include in the release
|
||||
TEMPLATES += sinqEPICSApp/Db/dimetix.db
|
||||
TEMPLATES += sinqEPICSApp/Db/slsvme.db
|
||||
TEMPLATES += sinqEPICSApp/Db/spsamor.db
|
||||
TEMPLATES += sinqEPICSApp/Db/el734.db
|
||||
|
||||
# DBD files to include in the release
|
||||
DBDS += sinqEPICSApp/src/sinq.dbd
|
||||
@ -32,15 +30,12 @@ SOURCES += sinqEPICSApp/src/NanotecDriver.cpp
|
||||
SOURCES += sinqEPICSApp/src/stptok.cpp
|
||||
SOURCES += sinqEPICSApp/src/PhytronDriver.cpp
|
||||
SOURCES += sinqEPICSApp/src/EuroMoveDriver.cpp
|
||||
SOURCES += sinqEPICSApp/src/pmacAsynIPPort.c
|
||||
SOURCES += sinqEPICSApp/src/pmacAxis.cpp
|
||||
SOURCES += sinqEPICSApp/src/pmacController.cpp
|
||||
# SOURCES += sinqEPICSApp/src/pmacAsynIPPort.c
|
||||
# SOURCES += sinqEPICSApp/src/pmacAxis.cpp
|
||||
# SOURCES += sinqEPICSApp/src/pmacController.cpp
|
||||
SOURCES += sinqEPICSApp/src/MasterMACSDriver.cpp
|
||||
SOURCES += sinqEPICSApp/src/C804Axis.cpp
|
||||
SOURCES += sinqEPICSApp/src/C804Controller.cpp
|
||||
SOURCES += sinqEPICSApp/src/newPmacV3Axis.cpp
|
||||
SOURCES += sinqEPICSApp/src/newPmacV3Controller.cpp
|
||||
SOURCES += sinqEPICSApp/src/pmacController.cpp
|
||||
|
||||
USR_CFLAGS += -Wall -Wextra # -Werror
|
||||
|
||||
|
@ -38,3 +38,12 @@ Those political problems require a special development model:
|
||||
Take care of the sinqEPICsApp/src/sinq.dbd file. This is the one which differs mostly between
|
||||
amorsim and master branches.
|
||||
|
||||
# Formatting
|
||||
|
||||
Formatting is done via the [`.clang-format`](./.clang-format) file checked into
|
||||
the repository. One option to apply the formatting to a given file is via the
|
||||
command below.
|
||||
|
||||
```
|
||||
clang-format -i -style=file <file>
|
||||
```
|
||||
|
34
sinqEPICSApp/Db/el734.db
Normal file
34
sinqEPICSApp/Db/el734.db
Normal file
@ -0,0 +1,34 @@
|
||||
record(motor,"$(P)$(M)")
|
||||
{
|
||||
field(DESC,"$(DESC)")
|
||||
field(DTYP,"$(DTYP)")
|
||||
field(DIR,"$(DIR)")
|
||||
field(VELO,"$(VELO)")
|
||||
field(HVEL,"$(VELO)")
|
||||
field(VBAS,"$(VELO)")
|
||||
field(VMAX, "${VMAX}")
|
||||
field(ACCL,"$(ACCL)")
|
||||
field(BDST,"$(BDST)")
|
||||
field(BVEL,"$(BVEL)")
|
||||
field(BACC,"$(BACC)")
|
||||
field(OUT,"@asyn($(PORT),$(ADDR))")
|
||||
field(MRES,"$(MRES)")
|
||||
field(PREC,"$(PREC)")
|
||||
field(EGU,"$(EGU)")
|
||||
field(DHLM,"$(DHLM)")
|
||||
field(DLLM,"$(DLLM)")
|
||||
field(INIT,"$(INIT)")
|
||||
field(PINI, "NO")
|
||||
field(TWV,"1")
|
||||
field(RTRY,"0")
|
||||
}
|
||||
|
||||
# The message text
|
||||
record(waveform, "$(P)$(M)-MsgTxt") {
|
||||
field(DTYP, "asynOctetRead")
|
||||
field(INP, "@asyn($(PORT),$(N),1) MOTOR_MESSAGE_TEXT")
|
||||
field(FTVL, "CHAR")
|
||||
field(NELM, "80")
|
||||
field(SCAN, "I/O Intr")
|
||||
}
|
||||
|
@ -46,9 +46,9 @@ asynStatus C804Axis::poll(bool *moving) {
|
||||
// Local variable declaration
|
||||
static const char *functionName = "C804Axis::poll";
|
||||
|
||||
// The poll function is just a wrapper around pollNoUpdate and
|
||||
// The poll function is just a wrapper around poll_no_param_lib_update and
|
||||
// handles mainly the callParamCallbacks() function
|
||||
asynStatus status_poll = C804Axis::pollNoUpdate(moving);
|
||||
asynStatus status_poll = C804Axis::poll_no_param_lib_update(moving);
|
||||
|
||||
// According to the function documentation of asynMotorAxis::poll, this
|
||||
// function should be called at the end of a poll implementation.
|
||||
@ -64,7 +64,7 @@ asynStatus C804Axis::poll(bool *moving) {
|
||||
}
|
||||
|
||||
// Perform the actual poll
|
||||
asynStatus C804Axis::pollNoUpdate(bool *moving) {
|
||||
asynStatus C804Axis::poll_no_param_lib_update(bool *moving) {
|
||||
// Local variable declaration
|
||||
static const char *functionName = "C804Axis::poll";
|
||||
asynStatus status;
|
||||
@ -134,8 +134,7 @@ asynStatus C804Axis::pollNoUpdate(bool *moving) {
|
||||
pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
|
||||
&motorRecResolution_);
|
||||
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, "Polling axis %d\n",
|
||||
axisNo_);
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, "Poll axis %d\n", axisNo_);
|
||||
|
||||
/*
|
||||
We know that the motor resolution must not be zero. During the startup of
|
||||
|
@ -1,43 +1,40 @@
|
||||
#ifndef C804Axis_H
|
||||
#define C804Axis_H
|
||||
|
||||
#include "SINQAxis.h"
|
||||
#include "SINQController.h"
|
||||
#include "SINQAxis.h"
|
||||
|
||||
// Forward declaration of the controller class to resolve the cyclic dependency
|
||||
// between C804Controller.h and C804Axis.h. See
|
||||
// https://en.cppreference.com/w/cpp/language/class.
|
||||
// between C804Controller.h and C804Axis.h. See https://en.cppreference.com/w/cpp/language/class.
|
||||
class C804Controller;
|
||||
|
||||
class C804Axis : public SINQAxis {
|
||||
public:
|
||||
/* These are the methods we override from the base class */
|
||||
C804Axis(C804Controller *pController, int axisNo);
|
||||
virtual ~C804Axis();
|
||||
asynStatus move(double position, int relative, double min_velocity,
|
||||
double max_velocity, double acceleration);
|
||||
asynStatus moveVelocity(double min_velocity, double max_velocity,
|
||||
double acceleration);
|
||||
asynStatus stop(double acceleration);
|
||||
asynStatus home(double minVelocity, double maxVelocity, double acceleration,
|
||||
int forwards);
|
||||
asynStatus poll(bool *moving);
|
||||
asynStatus pollNoUpdate(bool *moving);
|
||||
asynStatus enable(int on);
|
||||
class C804Axis : public SINQAxis
|
||||
{
|
||||
public:
|
||||
/* These are the methods we override from the base class */
|
||||
C804Axis(C804Controller *pController, int axisNo);
|
||||
virtual ~C804Axis();
|
||||
asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration);
|
||||
asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration);
|
||||
asynStatus stop(double acceleration);
|
||||
asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards);
|
||||
asynStatus poll(bool *moving);
|
||||
asynStatus poll_no_param_lib_update(bool *moving);
|
||||
asynStatus enable(int on);
|
||||
|
||||
protected:
|
||||
C804Controller *pC_;
|
||||
protected:
|
||||
C804Controller *pC_;
|
||||
|
||||
void checkBounds(C804Controller *pController, int axisNo);
|
||||
int last_position_steps_;
|
||||
double motorRecResolution_;
|
||||
time_t estimatedArrivalTime_;
|
||||
time_t last_poll_;
|
||||
int errorReported_;
|
||||
bool enabled_;
|
||||
void checkBounds(C804Controller *pController, int axisNo);
|
||||
int last_position_steps_;
|
||||
double motorRecResolution_;
|
||||
time_t estimatedArrivalTime_;
|
||||
time_t last_poll_;
|
||||
int errorReported_;
|
||||
bool enabled_;
|
||||
|
||||
private:
|
||||
friend class C804Controller;
|
||||
private:
|
||||
friend class C804Controller;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
@ -176,7 +176,9 @@ C804Controller::C804Controller(const char *portName, const char *lowLevelPortNam
|
||||
C804Controller::~C804Controller(void)
|
||||
{
|
||||
/*
|
||||
Cleanup of the memory allocated in the asynMotorController constructor
|
||||
Cleanup of the memory allocated in this->pAxes_. As discussed in the constructor,
|
||||
this is not strictly necessary due to the way EPICS works, but it is good
|
||||
practice anyway to properly clean up resources.
|
||||
*/
|
||||
free(this->pAxes_);
|
||||
}
|
||||
|
@ -16,6 +16,7 @@ public:
|
||||
/* These are the methods that we override */
|
||||
C804Axis *getAxis(asynUser *pasynUser);
|
||||
C804Axis *getAxis(int axisNo);
|
||||
C804Axis *castToC804Axis(asynMotorAxis *asynAxis);
|
||||
|
||||
protected:
|
||||
asynUser *lowLevelPortUser_;
|
||||
@ -25,7 +26,6 @@ protected:
|
||||
time_t idlePollPeriod_;
|
||||
|
||||
void log(const char *message);
|
||||
C804Axis *castToC804Axis(asynMotorAxis *asynAxis);
|
||||
asynStatus lowLevelWriteRead(int axisNo, const char *command, char *response, bool expect_response);
|
||||
|
||||
private:
|
||||
|
@ -143,6 +143,11 @@ asynStatus EL734Controller::transactController(int axisNo, char command[COMLEN],
|
||||
|
||||
pasynOctetSyncIO->flush(pasynUserController_);
|
||||
|
||||
if (axis != NULL)
|
||||
{
|
||||
axis->updateMsgTxtFromDriver("");
|
||||
}
|
||||
|
||||
status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
|
||||
reply, COMLEN, 2., &out, &in, &reason);
|
||||
if (status != asynSuccess)
|
||||
|
@ -250,6 +250,8 @@ PhytronAxis::PhytronAxis(PhytronController *pC, int axisNo, int enc)
|
||||
haveBrake = 0;
|
||||
brakeIO = -1;
|
||||
next_poll = -1;
|
||||
homing = 0;
|
||||
homing_direction = 0;
|
||||
}
|
||||
|
||||
int PhytronAxis::setBrake(int brakeNO)
|
||||
|
@ -114,7 +114,7 @@ typedef struct {
|
||||
unsigned int dbInit;
|
||||
}EL737priv;
|
||||
|
||||
static void dummyAsynCallback([[maybe_unused]] asynUser *pasynUser)
|
||||
static void dummyAsynCallback(asynUser *pasynUser)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -1,990 +0,0 @@
|
||||
#include "newPmacV3Axis.h"
|
||||
#include "asynOctetSyncIO.h"
|
||||
#include "newPmacV3Controller.h"
|
||||
#include <cmath>
|
||||
#include <errlog.h>
|
||||
#include <limits>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
newPmacV3Axis::newPmacV3Axis(newPmacV3Controller *pC, int axisNo)
|
||||
: asynMotorAxis(pC, axisNo), pC_(pC) {
|
||||
|
||||
static const char *functionName = "newPmacV3Axis::newPmacV3Axis";
|
||||
asynStatus status = asynSuccess;
|
||||
|
||||
/*
|
||||
The superclass constructor SINQAxis calls in turn its superclass constructor
|
||||
asynMotorAxis. In the latter, a pointer to the constructed object this is
|
||||
stored inside the array pAxes_:
|
||||
|
||||
pC->pAxes_[axisNo] = this;
|
||||
|
||||
Therefore, the axes are managed by the controller pC. See C804Controller.cpp
|
||||
for further explanation. If axisNo is out of bounds, asynMotorAxis prints an
|
||||
error (see
|
||||
https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorAxis.cpp,
|
||||
line 40). However, we want the IOC creation to stop completely, since this
|
||||
is a configuration error.
|
||||
*/
|
||||
if (axisNo >= pC->numAxes_) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: Axis index %d must be smaller than the "
|
||||
"total number of axes %d. Terminating IOC.",
|
||||
functionName, axisNo_, pC->numAxes_);
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
// Initialize all member variables
|
||||
initial_poll_ = true;
|
||||
waitForHandshake_ = false;
|
||||
|
||||
// Wait 10 seconds for the handshake until declaring a timeout
|
||||
handshakeTimeout_ = 10;
|
||||
|
||||
// Placeholder, is overwritten later
|
||||
time_at_init_poll_ = 0;
|
||||
|
||||
// After 3 idle polls, the parameter library definitely had enough time to
|
||||
// be initialized
|
||||
timeout_param_lib_init_ = 3 * pC->idlePollPeriod_;
|
||||
|
||||
// Provide initial values for some parameter library entries
|
||||
status = pC_->setIntegerParam(axisNo_, pC_->rereadEncoderPosition_, 0);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: Setting an initial parameter library value "
|
||||
"for rereadEncoderPosition_ in axis %d. Terminating IOC.",
|
||||
functionName, axisNo_);
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
status = pC_->setDoubleParam(axisNo_, pC_->motorPosition_, 0.0);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: Setting an initial parameter library value "
|
||||
"for motorPosition_ in axis %d. Terminating IOC.",
|
||||
functionName, axisNo_);
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
// This value is updated in the poll. Initially, we assume that the motor is
|
||||
// not enabled.
|
||||
status = pC_->setIntegerParam(axisNo_, pC_->motorEnabled_, 0);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: Setting an initial parameter library value "
|
||||
"for motorPosition_ in axis %d. Terminating IOC.",
|
||||
functionName, axisNo_);
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
|
||||
newPmacV3Axis::~newPmacV3Axis(void) {
|
||||
// Since the controller memory is managed somewhere else, we don't need to
|
||||
// clean up the pointer pC here.
|
||||
}
|
||||
|
||||
/*
|
||||
Read the configuration from the motor control unit and the parameter library.
|
||||
This operation is only allowed if the motor is not moving
|
||||
*/
|
||||
asynStatus newPmacV3Axis::readConfig() {
|
||||
|
||||
// Local variable declaration
|
||||
static const char *functionName = "newPmacV3Axis::readConfig";
|
||||
asynStatus status = asynSuccess;
|
||||
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
int nvals = 0;
|
||||
double highLimit = 0.0;
|
||||
double lowLimit = 0.0;
|
||||
double motorRecResolution = 0.0;
|
||||
double position = 0.0;
|
||||
int axStatus = 0;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
// Motor resolution from parameter library
|
||||
status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
|
||||
&motorRecResolution);
|
||||
if (status == asynParamUndefined) {
|
||||
return asynParamUndefined;
|
||||
} else if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, functionName,
|
||||
"motorRecResolution_");
|
||||
}
|
||||
|
||||
// Software limits and current position
|
||||
snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10 Q%2.2d13 Q%2.2d14",
|
||||
axisNo_, axisNo_, axisNo_, axisNo_);
|
||||
status = pC_->writeRead(axisNo_, command, response, true);
|
||||
nvals = sscanf(response, "%d %lf %lf %lf", &axStatus, &position, &highLimit,
|
||||
&lowLimit);
|
||||
if (pC_->checkNumExpectedReads(4, nvals, functionName, command, response,
|
||||
axisNo_) != asynSuccess) {
|
||||
return asynError;
|
||||
}
|
||||
|
||||
// If the motor is not in idle status, do not read the configuration
|
||||
if (axStatus != 0) {
|
||||
return asynError;
|
||||
}
|
||||
|
||||
// Transform from motor to user coordinates
|
||||
position = position * motorRecResolution;
|
||||
highLimit = highLimit * motorRecResolution;
|
||||
lowLimit = lowLimit * motorRecResolution;
|
||||
|
||||
/*
|
||||
The axis limits are set as: ({[]})
|
||||
where [] are the positive and negative limits set in EPICS/NICOS, {} are the
|
||||
software limits set on the MCU and () are the hardware limit switches. In
|
||||
other words, the EPICS/NICOS limits must be stricter than the software
|
||||
limits on the MCU which in turn should be stricter than the hardware limit
|
||||
switches. For example, if the hardware limit switches are at [-10, 10], the
|
||||
software limits could be at [-9, 9] and the EPICS / NICOS limits could be at
|
||||
[-8, 8]. Therefore, we cannot use the software limits read from the MCU
|
||||
directly, but need to shrink them a bit. In this case, we're shrinking them
|
||||
by 0.1 mm or 0.1 degree (depending on the axis type) on both sides.
|
||||
*/
|
||||
highLimit = highLimit - 0.1;
|
||||
lowLimit = lowLimit + 0.1;
|
||||
|
||||
// Store these values in the parameter library
|
||||
status = pC_->setDoubleParam(axisNo_, pC_->motorPosition_, position);
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, functionName,
|
||||
"motorPosition_");
|
||||
}
|
||||
status = pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, lowLimit);
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, functionName,
|
||||
"motorLowLimit_");
|
||||
}
|
||||
status = pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, highLimit);
|
||||
if (status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(status, functionName,
|
||||
"motorHighLimit_");
|
||||
}
|
||||
|
||||
// Update the parameter library immediately
|
||||
status = callParamCallbacks();
|
||||
if (status != asynSuccess) {
|
||||
// If we can't communicate with the parameter library, it doesn't make
|
||||
// sense to try and upstream this to the user -> Just log the error
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Updating the parameter library failed for axis %d\n",
|
||||
functionName, axisNo_);
|
||||
return status;
|
||||
}
|
||||
|
||||
return this->readEncoderType();
|
||||
}
|
||||
|
||||
asynStatus newPmacV3Axis::poll(bool *moving) {
|
||||
// Local variable declaration
|
||||
static const char *functionName = "newPmacV3Axis::poll";
|
||||
asynStatus pl_status = asynSuccess;
|
||||
asynStatus poll_status = asynSuccess;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
// If this poll is the initial poll, check if the parameter library has
|
||||
// already been initialized. If not, force EPCIS to repeat the poll until
|
||||
// the initialization is complete (or until a timeout is reached). Once the
|
||||
// parameter library has been initialized, read configuration data from the
|
||||
// motor controller into it.
|
||||
if (initial_poll_) {
|
||||
|
||||
if (time_at_init_poll_ == 0) {
|
||||
time_at_init_poll_ = time(NULL);
|
||||
}
|
||||
|
||||
if (time(NULL) > (time_at_init_poll_ + timeout_param_lib_init_)) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: Could not initialize the parameter "
|
||||
"library until the timeout of %ld seconds after IOC "
|
||||
"startup. Terminating IOC.",
|
||||
functionName, timeout_param_lib_init_);
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
poll_status = readConfig();
|
||||
if (poll_status == asynSuccess) {
|
||||
initial_poll_ = false;
|
||||
} else if (poll_status == asynParamUndefined) {
|
||||
// Wait for 100 ms until trying the entire poll again
|
||||
usleep(100000);
|
||||
return poll_status;
|
||||
} else {
|
||||
// Something else went completly wrong => Abort the program
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: Reading a value from the parameter "
|
||||
"library failed for axis %d (%s). Terminating",
|
||||
functionName, axisNo_,
|
||||
pC_->stringifyAsynStatus(poll_status));
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
|
||||
// The poll function is just a wrapper around pollNoUpdate and
|
||||
// handles mainly the callParamCallbacks() function. This wrapper is used
|
||||
// to make sure callParamCallbacks() is called in case of a premature
|
||||
// return.
|
||||
poll_status = newPmacV3Axis::pollNoUpdate(moving);
|
||||
|
||||
// If the poll status is ok, reset the error indicators in the parameter
|
||||
// library
|
||||
if (poll_status == asynSuccess) {
|
||||
pl_status = setIntegerParam(pC_->motorStatusProblem_, false);
|
||||
if (pl_status != asynSuccess) {
|
||||
pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorStatusProblem_");
|
||||
}
|
||||
pl_status = setIntegerParam(pC_->motorStatusCommsError_, false);
|
||||
if (pl_status != asynSuccess) {
|
||||
pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorStatusCommsError_");
|
||||
}
|
||||
}
|
||||
|
||||
// According to the function documentation of asynMotorAxis::poll, this
|
||||
// function should be called at the end of a poll implementation.
|
||||
pl_status = callParamCallbacks();
|
||||
if (pl_status != asynSuccess) {
|
||||
// If we can't communicate with the parameter library, it doesn't make
|
||||
// sense to try and upstream this to the user -> Just log the error
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Updating the parameter library failed for axis %d\n",
|
||||
functionName, axisNo_);
|
||||
poll_status = pl_status;
|
||||
}
|
||||
|
||||
return poll_status;
|
||||
}
|
||||
|
||||
// Perform the actual poll
|
||||
asynStatus newPmacV3Axis::pollNoUpdate(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;
|
||||
|
||||
static const char *functionName = "newPmacV3Axis::poll";
|
||||
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
int nvals = 0;
|
||||
|
||||
int direction = 0;
|
||||
int error = 0;
|
||||
int axStatus = 0;
|
||||
double currentPosition = 0.0;
|
||||
double previousPosition = 0.0;
|
||||
double motorRecResolution = 0.0;
|
||||
int handshakePerformed = 0;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
// Are we currently waiting for a handshake?
|
||||
if (waitForHandshake_) {
|
||||
snprintf(command, sizeof(command), "P%2.2d23", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
nvals = sscanf(response, "%d", &handshakePerformed);
|
||||
if (pC_->checkNumExpectedReads(1, nvals, functionName, command,
|
||||
response, axisNo_) != asynSuccess) {
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if (handshakePerformed == 1) {
|
||||
// Handshake has been performed successfully -> Continue with the
|
||||
// poll
|
||||
waitForHandshake_ = false;
|
||||
} else {
|
||||
// Still waiting for the handshake. This is already part of the
|
||||
// movement procedure!
|
||||
if (time(NULL) < timeAtHandshake_ + handshakeTimeout_) {
|
||||
*moving = true;
|
||||
return asynSuccess;
|
||||
} else {
|
||||
// Timed out when waiting for the handshake
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Axis %d timed out when waiting for the "
|
||||
"handshake with the MCU.\n",
|
||||
functionName, axisNo_);
|
||||
pl_status = setStringParam(
|
||||
pC_->messageText_, "Handshake timed out. This is a bug.");
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
waitForHandshake_ = false;
|
||||
return asynError;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Motor resolution from parameter library
|
||||
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
|
||||
&motorRecResolution);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorRecResolution_");
|
||||
}
|
||||
|
||||
// Read the previous motor position
|
||||
pl_status =
|
||||
pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &previousPosition);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorPosition_");
|
||||
}
|
||||
|
||||
// Check the axis status (Pxx00) and the current motor position (Qxx10)
|
||||
snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10 P%2.2d01", axisNo_,
|
||||
axisNo_, axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
nvals = sscanf(response, "%d %lf %d", &axStatus, ¤tPosition, &error);
|
||||
if (pC_->checkNumExpectedReads(3, nvals, functionName, command, response,
|
||||
axisNo_) != asynSuccess) {
|
||||
return asynError;
|
||||
}
|
||||
|
||||
// Intepret the status
|
||||
switch (axStatus) {
|
||||
case -6:
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: Axis %d is stopping\n", functionName, axisNo_);
|
||||
*moving = true;
|
||||
break;
|
||||
case -5:
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: Axis %d is deactivated\n", functionName, axisNo_);
|
||||
*moving = false;
|
||||
|
||||
pl_status = setStringParam(pC_->messageText_, "Deactivated");
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
|
||||
// No further evaluation of the axis status is necessary
|
||||
return asynSuccess;
|
||||
case -4:
|
||||
asynPrint(
|
||||
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Emergency stop has been activated. All axes are stopped.\n",
|
||||
functionName);
|
||||
*moving = false;
|
||||
|
||||
pl_status = setStringParam(pC_->messageText_, "Emergency stop");
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
|
||||
// No further evaluation of the axis status is necessary
|
||||
return asynSuccess;
|
||||
case -3:
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: Axis %d is inhibited\n", functionName, axisNo_);
|
||||
*moving = false;
|
||||
|
||||
pl_status = setStringParam(pC_->messageText_, "Disabled");
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
|
||||
// No further evaluation of the axis status is necessary
|
||||
return asynSuccess;
|
||||
case 0:
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: Axis %d is ready for movement\n", functionName, axisNo_);
|
||||
*moving = false;
|
||||
break;
|
||||
case 1:
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: Move order for %d acknowledged\n", functionName,
|
||||
axisNo_);
|
||||
*moving = true;
|
||||
break;
|
||||
case 2:
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: Move order for %d is possible\n", functionName, axisNo_);
|
||||
*moving = true;
|
||||
break;
|
||||
case 3:
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: %d in Air Cushion Outout status\n", functionName,
|
||||
axisNo_);
|
||||
*moving = true;
|
||||
break;
|
||||
case 4:
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: %d in Air Cushion Input status\n", functionName,
|
||||
axisNo_);
|
||||
*moving = true;
|
||||
break;
|
||||
case 5:
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: Axis %d is moving\n", functionName, axisNo_);
|
||||
*moving = true;
|
||||
break;
|
||||
default:
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Reached unreachable state P%2.2d00 = %d.\n",
|
||||
functionName, axisNo_, axStatus);
|
||||
pl_status =
|
||||
setStringParam(pC_->messageText_,
|
||||
"Unreachable state has been reached. This is a bug");
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
*moving = false;
|
||||
}
|
||||
|
||||
if (*moving) {
|
||||
// If the axis is moving, evaluate the movement direction
|
||||
if ((currentPosition - previousPosition) > 0) {
|
||||
direction = 1;
|
||||
} else {
|
||||
direction = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Error handling
|
||||
switch (error) {
|
||||
case 0:
|
||||
// No error
|
||||
break;
|
||||
case 1:
|
||||
// EPICS should already prevent this issue in the first place,
|
||||
// since it contains the user limits
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Target position would exceed user limits in axis "
|
||||
"%d. EPICS should prevent an out-of-bounds target "
|
||||
"position, this is a bug.\n",
|
||||
functionName, axisNo_);
|
||||
|
||||
pl_status =
|
||||
setStringParam(pC_->messageText_,
|
||||
"Target position would exceed software limits. This "
|
||||
"is a bug.");
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
|
||||
poll_status = asynError;
|
||||
break;
|
||||
case 5:
|
||||
// Command not possible
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Axis %d is still moving, but received another move "
|
||||
"command. EPICS should prevent this, this is a bug.\n",
|
||||
functionName, axisNo_);
|
||||
|
||||
pl_status = setStringParam(pC_->messageText_,
|
||||
"Axis received move command while it is "
|
||||
"still moving. This is a bug.");
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
|
||||
poll_status = asynError;
|
||||
break;
|
||||
case 10:
|
||||
/*
|
||||
Software limits of the controller have been hit. Since the EPICS limits
|
||||
are derived from the software limits and are a little bit smaller, this
|
||||
error case can only happen if either the axis has an incremental encoder
|
||||
which is not properly homed or if a bug occured.
|
||||
*/
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING,
|
||||
"%s: Axis %d hit the controller limits. Try homing the axis, "
|
||||
"if that doesn't remove the error, this is a bug.\n",
|
||||
functionName, axisNo_);
|
||||
|
||||
snprintf(command, sizeof(command),
|
||||
"Software limits hit (P%2.2d01 = %d). Try homing the motor. "
|
||||
"If that doesn't work, contact the support",
|
||||
axisNo_, error);
|
||||
pl_status = setStringParam(pC_->messageText_, command);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
|
||||
poll_status = asynError;
|
||||
break;
|
||||
case 11:
|
||||
// Following error
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Maximum allowed following error exceeded for axis %d.\n",
|
||||
functionName, axisNo_);
|
||||
|
||||
snprintf(command, sizeof(command),
|
||||
"Maximum allowed following error exceeded (P%2.2d01 = 11). "
|
||||
"Please contact Electronics support.",
|
||||
axisNo_);
|
||||
pl_status = setStringParam(pC_->messageText_, command);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
|
||||
poll_status = asynError;
|
||||
break;
|
||||
case 13:
|
||||
// Watchdog of the controller final stage triggered
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Driver hardware error triggered for axis %d.\n",
|
||||
functionName, axisNo_);
|
||||
|
||||
snprintf(command, sizeof(command),
|
||||
"Driver hardware error (P%2.2d01 = 13). "
|
||||
"Please contact Electronics support.",
|
||||
axisNo_);
|
||||
pl_status = setStringParam(pC_->messageText_, command);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
|
||||
poll_status = asynError;
|
||||
break;
|
||||
case 14:
|
||||
// EPICS should already prevent this issue in the first place,
|
||||
// since it contains the user limits
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Target position would exceed hardware limits in axis "
|
||||
"%d. Homing might be necessary.\n",
|
||||
functionName, axisNo_);
|
||||
|
||||
snprintf(command, sizeof(command),
|
||||
"Move command exceeds hardware limits (P%2.2d01 = %d). Try "
|
||||
"homing the motor. If that doesn't work, contact the support",
|
||||
axisNo_, error);
|
||||
pl_status = setStringParam(pC_->messageText_, command);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
|
||||
poll_status = asynError;
|
||||
break;
|
||||
default:
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Axis %d reached an unreachable state (P%2.2d01 = %d).\n",
|
||||
functionName, axisNo_, axisNo_, error);
|
||||
|
||||
pl_status = setStringParam(pC_->messageText_,
|
||||
"Axis reached an unreachable state. Please "
|
||||
"contact electronics support");
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
|
||||
poll_status = asynError;
|
||||
break;
|
||||
}
|
||||
|
||||
// Update the parameter library
|
||||
if (error != 0) {
|
||||
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorStatusProblem_");
|
||||
}
|
||||
}
|
||||
|
||||
if (*moving == false) {
|
||||
pl_status = setIntegerParam(pC_->motorMoveToHome_, 0);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorMoveToHome_");
|
||||
}
|
||||
}
|
||||
|
||||
pl_status =
|
||||
setIntegerParam(pC_->motorEnabled_, (axStatus != -3 && axStatus != -5));
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorEnabled_");
|
||||
}
|
||||
|
||||
pl_status = setIntegerParam(pC_->motorStatusMoving_, *moving);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorStatusMoving_");
|
||||
}
|
||||
|
||||
pl_status = setIntegerParam(pC_->motorStatusDone_, !(*moving));
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorStatusDone_");
|
||||
}
|
||||
|
||||
pl_status = setIntegerParam(pC_->motorStatusDirection_, direction);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorStatusDirection_");
|
||||
}
|
||||
|
||||
pl_status = setDoubleParam(pC_->motorPosition_, currentPosition);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorPosition_");
|
||||
}
|
||||
|
||||
return poll_status;
|
||||
}
|
||||
|
||||
asynStatus newPmacV3Axis::move(double position, int relative,
|
||||
double minVelocity, double maxVelocity,
|
||||
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;
|
||||
|
||||
static const char *functionName = "newPmacV3Axis::move";
|
||||
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
double motorCoordinatesPosition = 0.0;
|
||||
int enabled = 0;
|
||||
double motorRecResolution = 0.0;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnabled_, &enabled);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorEnabled_");
|
||||
}
|
||||
|
||||
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
|
||||
&motorRecResolution);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorRecResolution_");
|
||||
}
|
||||
|
||||
if (enabled == 0) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Axis %d is disabled\n", functionName, axisNo_);
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
// Convert from user to motor units
|
||||
motorCoordinatesPosition = position / motorRecResolution;
|
||||
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: Start of axis %d to position %lf\n", functionName, axisNo_,
|
||||
position);
|
||||
|
||||
// Perform handshake, Set target position and start the move command
|
||||
|
||||
if (relative) {
|
||||
snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d02=%lf M%2.2d=2",
|
||||
axisNo_, axisNo_, motorCoordinatesPosition, axisNo_);
|
||||
} else {
|
||||
snprintf(command, sizeof(command), "P%2.2d23=0 Q%2.2d01=%lf M%2.2d=1",
|
||||
axisNo_, axisNo_, motorCoordinatesPosition, axisNo_);
|
||||
}
|
||||
|
||||
// We don't expect an answer
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, false);
|
||||
if (rw_status != asynSuccess) {
|
||||
asynPrint(
|
||||
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Starting movement to target position %lf failed for axis %d\n",
|
||||
functionName, position, axisNo_);
|
||||
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorStatusProblem_");
|
||||
}
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
// In the next poll, we will check if the handshake has been performed in a
|
||||
// reasonable time
|
||||
waitForHandshake_ = true;
|
||||
timeAtHandshake_ = time(NULL);
|
||||
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
asynStatus newPmacV3Axis::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;
|
||||
|
||||
static const char *functionName = "newPmacV3Axis::stopAxis";
|
||||
bool moving = false;
|
||||
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
|
||||
// =========================================================================
|
||||
|
||||
pl_status = pollNoUpdate(&moving);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pl_status;
|
||||
}
|
||||
|
||||
if (moving) {
|
||||
// only send a stop when actually moving
|
||||
snprintf(command, sizeof(command), "M%2.2d=8", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, false);
|
||||
|
||||
if (rw_status != asynSuccess) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Stopping the movement failed for axis %d\n",
|
||||
functionName, axisNo_);
|
||||
|
||||
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorStatusProblem_");
|
||||
}
|
||||
}
|
||||
}
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
/*
|
||||
Home the axis. On absolute encoder systems, this is a no-op
|
||||
*/
|
||||
asynStatus newPmacV3Axis::home(double min_velocity, double max_velocity,
|
||||
double acceleration, int forwards) {
|
||||
|
||||
// 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 command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
static const char *functionName = "newPmacV3Axis::home";
|
||||
|
||||
// =========================================================================
|
||||
|
||||
pl_status =
|
||||
pC_->getStringParam(axisNo_, pC_->encoderType_, pC_->MAXBUF_, response);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"encoderType_");
|
||||
}
|
||||
|
||||
// Only send the home command if the axis has an incremental encoder
|
||||
if (strcmp(response, IncrementalEncoder) == 0) {
|
||||
snprintf(command, sizeof(command), "M%2.2d=9", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, false);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
pl_status = setIntegerParam(pC_->motorMoveToHome_, 1);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"motorMoveToHome_");
|
||||
}
|
||||
|
||||
pl_status = setStringParam(pC_->messageText_, "Homing");
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
}
|
||||
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
/*
|
||||
Read the encoder type and update the parameter library accordingly
|
||||
*/
|
||||
asynStatus newPmacV3Axis::readEncoderType() {
|
||||
|
||||
// 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 command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
static const char *functionName = "newPmacV3Axis::readEncoderType";
|
||||
int nvals = 0;
|
||||
int encoder_id = 0;
|
||||
char encoderType[pC_->MAXBUF_];
|
||||
|
||||
// =========================================================================
|
||||
|
||||
// Check if this is an absolute encoder
|
||||
snprintf(command, sizeof(command), "I%2.2X04", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
int reponse_length = strlen(response);
|
||||
if (reponse_length < 3) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Unexpected reponse '%s' from axis %d on "
|
||||
"controller %s while reading the encoder type. Aborting...\n",
|
||||
functionName, response, axisNo_, pC_->portName);
|
||||
return asynError;
|
||||
}
|
||||
|
||||
// We are only interested in the last two digits and the last value in
|
||||
// the string before the terminator is \r
|
||||
nvals = sscanf(response + (reponse_length - 3), "%2X", &encoder_id);
|
||||
if (pC_->checkNumExpectedReads(1, nvals, functionName, command, response,
|
||||
axisNo_) != asynSuccess) {
|
||||
return asynError;
|
||||
}
|
||||
|
||||
snprintf(command, sizeof(command), "P46");
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
int number_of_axes = strtol(response, NULL, 10);
|
||||
|
||||
// If true, the encoder is incremental
|
||||
if (encoder_id <= number_of_axes) {
|
||||
pl_status = setStringParam(pC_->encoderType_, IncrementalEncoder);
|
||||
} else {
|
||||
pl_status = setStringParam(pC_->encoderType_, AbsoluteEncoder);
|
||||
}
|
||||
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"encoderType_");
|
||||
}
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus newPmacV3Axis::enable(int on) {
|
||||
|
||||
static const char *functionName = "newPmacV3Axis::enable";
|
||||
int ax_status = 0;
|
||||
int timeout_enable_disable = 2;
|
||||
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
int nvals = 0;
|
||||
|
||||
// Status of read-write-operations of ASCII commands to the controller
|
||||
asynStatus rw_status = asynSuccess;
|
||||
|
||||
// Status of parameter library operations
|
||||
asynStatus pl_status = asynSuccess;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
// Check if the axis is currently enabled
|
||||
snprintf(command, sizeof(command), "P%2.2d00", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
nvals = sscanf(response, "%d", &ax_status);
|
||||
if (pC_->checkNumExpectedReads(1, nvals, functionName, command, response,
|
||||
axisNo_) != asynSuccess) {
|
||||
return asynError;
|
||||
}
|
||||
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: Axis status %d, new input %d \n", functionName, ax_status,
|
||||
on);
|
||||
|
||||
// Axis is already enabled / disabled and a new enable / disable command
|
||||
// was sent => Do nothing
|
||||
if ((ax_status != -3) == on) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING,
|
||||
"%s: axis %d on controller %s is already %s\n", functionName,
|
||||
axisNo_, pC_->portName, on ? "enabled" : "disabled");
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
// Enable / disable the axis
|
||||
snprintf(command, sizeof(command), "M%2.2d14=%d", axisNo_, on);
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: %s axis %d on controller %s\n", functionName,
|
||||
on ? "Enable" : "Disable", axisNo_, pC_->portName);
|
||||
if (on == 0) {
|
||||
pl_status = setStringParam(pC_->messageText_, "Disabling ...");
|
||||
} else {
|
||||
pl_status = setStringParam(pC_->messageText_, "Enabling ...");
|
||||
}
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, false);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
// Query the axis status every few milliseconds until the axis has been
|
||||
// enabled or until the timeout has been reached
|
||||
snprintf(command, sizeof(command), "P%2.2d00", axisNo_);
|
||||
int startTime = time(NULL);
|
||||
while (time(NULL) < startTime + timeout_enable_disable) {
|
||||
|
||||
// Read the axis status
|
||||
usleep(100000);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
nvals = sscanf(response, "%d", &ax_status);
|
||||
if (pC_->checkNumExpectedReads(1, nvals, functionName, command,
|
||||
response, axisNo_) != asynSuccess) {
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if ((ax_status != -3) == on) {
|
||||
bool moving = false;
|
||||
// Perform a poll to update the parameter library
|
||||
poll(&moving);
|
||||
return asynSuccess;
|
||||
}
|
||||
}
|
||||
|
||||
// Failed to change axis status within timeout_enable_disable => Send a
|
||||
// corresponding message
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s: Failed to %s axis %d on controller %s within %d seconds\n",
|
||||
functionName, on ? "enable" : "disable", axisNo_, pC_->portName,
|
||||
timeout_enable_disable);
|
||||
|
||||
// Output message to user
|
||||
snprintf(command, sizeof(command), "Failed to %s within %d seconds",
|
||||
on ? "enable" : "disable", timeout_enable_disable);
|
||||
pl_status = setStringParam(pC_->messageText_, "Enabling ...");
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
return asynError;
|
||||
}
|
@ -1,41 +0,0 @@
|
||||
#ifndef pmacV3AXIS_H
|
||||
#define pmacV3AXIS_H
|
||||
#include "asynMotorAxis.h"
|
||||
|
||||
// Forward declaration of the controller class to resolve the cyclic dependency
|
||||
// between C804Controller.h and C804Axis.h. See
|
||||
// https://en.cppreference.com/w/cpp/language/class.
|
||||
class newPmacV3Controller;
|
||||
|
||||
class newPmacV3Axis : public asynMotorAxis {
|
||||
public:
|
||||
/* These are the methods we override from the base class */
|
||||
newPmacV3Axis(newPmacV3Controller *pController, int axisNo);
|
||||
virtual ~newPmacV3Axis();
|
||||
asynStatus move(double position, int relative, double min_velocity,
|
||||
double max_velocity, double acceleration);
|
||||
asynStatus stop(double acceleration);
|
||||
asynStatus home(double minVelocity, double maxVelocity, double acceleration,
|
||||
int forwards);
|
||||
asynStatus poll(bool *moving);
|
||||
asynStatus pollNoUpdate(bool *moving);
|
||||
asynStatus enable(int on);
|
||||
asynStatus readEncoderType();
|
||||
|
||||
protected:
|
||||
newPmacV3Controller *pC_;
|
||||
|
||||
void checkBounds(newPmacV3Controller *pController, int axisNo);
|
||||
asynStatus readConfig();
|
||||
bool initial_poll_;
|
||||
bool waitForHandshake_;
|
||||
time_t timeAtHandshake_;
|
||||
time_t handshakeTimeout_;
|
||||
time_t time_at_init_poll_;
|
||||
time_t timeout_param_lib_init_;
|
||||
|
||||
private:
|
||||
friend class newPmacV3Controller;
|
||||
};
|
||||
|
||||
#endif
|
@ -1,813 +0,0 @@
|
||||
|
||||
#include "newPmacV3Controller.h"
|
||||
#include "asynMotorController.h"
|
||||
#include "asynOctetSyncIO.h"
|
||||
#include "newPmacV3Axis.h"
|
||||
#include <epicsExport.h>
|
||||
#include <errlog.h>
|
||||
#include <iocsh.h>
|
||||
#include <netinet/in.h>
|
||||
#include <registryFunction.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
static const char *driverName = "newPmacV3Controller";
|
||||
|
||||
// Static pointers (valid for the entire lifetime of the IOC). The number behind
|
||||
// the strings gives the integer number of each variant (see also method
|
||||
// stringifyAsynStatus)
|
||||
static const char *asynSuccessStringified = "success"; // 0
|
||||
static const char *asynTimeoutStringified = "timeout"; // 1
|
||||
static const char *asynOverflowStringified = "overflow"; // 2
|
||||
static const char *asynErrorStringified = "error"; // 3
|
||||
static const char *asynDisconnectedStringified = "disconnected"; // 4
|
||||
static const char *asynDisabledStringified = "disabled"; // 5
|
||||
static const char *asynParamAlreadyExistsStringified =
|
||||
"parameter already exists"; // 6
|
||||
static const char *asynParamNotFoundStringified = "parameter not found"; // 7
|
||||
static const char *asynParamWrongTypeStringified = "wrong type"; // 8
|
||||
static const char *asynParamBadIndexStringified = "bad index"; // 9
|
||||
static const char *asynParamUndefinedStringified = "parameter undefined"; // 10
|
||||
static const char *asynParamInvalidListStringified = "invalid list"; // 11
|
||||
|
||||
const double newPmacV3Controller::TIMEOUT_ = 5.0; // seconds
|
||||
|
||||
/*
|
||||
Constructor arguments
|
||||
- portName:
|
||||
- lowLevelPortName
|
||||
- numAxes
|
||||
- movingPollPeriod: Time between polls when moving (in seconds)
|
||||
- idlePollPeriod: Time between polls when not moving (in seconds)
|
||||
*/
|
||||
newPmacV3Controller::newPmacV3Controller(const char *portName,
|
||||
const char *lowLevelPortName,
|
||||
int numAxes, double movingPollPeriod,
|
||||
double idlePollPeriod,
|
||||
const int &extraParams)
|
||||
: asynMotorController(
|
||||
portName, numAxes, NUM_MOTOR_DRIVER_PARAMS + extraParams,
|
||||
0, // No additional interfaces beyond those in base class
|
||||
0, // No additional callback interfaces beyond those in base class
|
||||
ASYN_CANBLOCK | ASYN_MULTIDEVICE,
|
||||
1, // autoconnect
|
||||
0, 0) // Default priority and stack size
|
||||
|
||||
{
|
||||
|
||||
// Initialization of local variables
|
||||
static const char *functionName =
|
||||
"newPmacV3Controller::newPmacV3Controller";
|
||||
asynStatus status = asynSuccess;
|
||||
|
||||
// Initialization of all member variables
|
||||
lowLevelPortUser_ = nullptr;
|
||||
|
||||
// =========================================================================;
|
||||
|
||||
/*
|
||||
We try to connect to the port via the port name provided by the constructor.
|
||||
If this fails, the function is terminated via exit
|
||||
*/
|
||||
pasynOctetSyncIO->connect(lowLevelPortName, 0, &lowLevelPortUser_, NULL);
|
||||
if (status != asynSuccess || lowLevelPortUser_ == nullptr) {
|
||||
errlogPrintf("FATAL ERROR: in %s: cannot connect to MCU controller\n. "
|
||||
"Terminating",
|
||||
functionName);
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Create additional PVs
|
||||
|
||||
// MOTOR_MESSAGE_TEXT corresponds to the PV definition inside
|
||||
// sinqn_asyn_motor.db. This text is used to forward status messages to
|
||||
// NICOS and in turn to the user
|
||||
status = createParam("MOTOR_MESSAGE_TEXT", asynParamOctet, &messageText_);
|
||||
if (status != asynSuccess) {
|
||||
paramLibAccessFailed(status, functionName, "messageText_");
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: unable to create parameter (%s). "
|
||||
"Terminating IOC.\n",
|
||||
functionName, stringifyAsynStatus(status));
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
status = createParam("ENABLE_AXIS", asynParamInt32, &enableMotor_);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: unable to create parameter (%s). "
|
||||
"Terminating IOC.\n",
|
||||
functionName, stringifyAsynStatus(status));
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
status = createParam("AXIS_ENABLED", asynParamInt32, &motorEnabled_);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: unable to create parameter (%s). "
|
||||
"Terminating IOC.\n",
|
||||
functionName, stringifyAsynStatus(status));
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
status = createParam("ENCODER_TYPE", asynParamOctet, &encoderType_);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: unable to create parameter (%s). "
|
||||
"Terminating IOC.\n",
|
||||
functionName, stringifyAsynStatus(status));
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
status = createParam("REREAD_ENCODER_POSITION", asynParamInt32,
|
||||
&rereadEncoderPosition_);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: unable to create parameter (%s). "
|
||||
"Terminating IOC.\n",
|
||||
functionName, stringifyAsynStatus(status));
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
status = createParam("REREAD_ENCODER_POSITION_RBV", asynParamInt32,
|
||||
&rereadEncoderPositionRBV_);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: unable to create parameter (%s). "
|
||||
"Terminating IOC.\n",
|
||||
functionName, stringifyAsynStatus(status));
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
status = createParam("READ_CONFIG", asynParamInt32, &readConfig_);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: unable to create parameter (%s). "
|
||||
"Terminating IOC.\n",
|
||||
functionName, stringifyAsynStatus(status));
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
status =
|
||||
createParam("MOTOR_POSITION_RBV", asynParamFloat64, &motorPositionRBV_);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: unable to create parameter (%s). "
|
||||
"Terminating IOC.\n",
|
||||
functionName, stringifyAsynStatus(status));
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
/*
|
||||
Define the end-of-string of a message coming from the device to EPICS.
|
||||
It is not necessary to append a terminator to outgoing messages, since
|
||||
the message length is encoded in the message header in the getSetResponse
|
||||
method.
|
||||
*/
|
||||
const char *message_from_device =
|
||||
"\006"; // Hex-code for ACK (acknowledge) -> Each message from the MCU
|
||||
// is terminated by this value
|
||||
status = pasynOctetSyncIO->setInputEos(
|
||||
lowLevelPortUser_, message_from_device, strlen(message_from_device));
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(
|
||||
this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: Unable to set input EOS (%s). Terminating IOC.\n",
|
||||
functionName, stringifyAsynStatus(status));
|
||||
pasynOctetSyncIO->disconnect(lowLevelPortUser_);
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
status = startPoller(movingPollPeriod, idlePollPeriod, 1);
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(
|
||||
lowLevelPortUser_, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: Could not start poller (%s). Terminating IOC.\n",
|
||||
functionName, stringifyAsynStatus(status));
|
||||
exit(-1);
|
||||
}
|
||||
status = callParamCallbacks();
|
||||
if (status != asynSuccess) {
|
||||
asynPrint(
|
||||
lowLevelPortUser_, ASYN_TRACE_ERROR,
|
||||
"%s: FATAL ERROR: Could not start poller (%s). Terminating IOC.\n",
|
||||
functionName, stringifyAsynStatus(status));
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
|
||||
newPmacV3Controller::~newPmacV3Controller(void) {
|
||||
/*
|
||||
Cleanup of the memory allocated in the asynMotorController constructor
|
||||
*/
|
||||
free(this->pAxes_);
|
||||
}
|
||||
|
||||
/*
|
||||
Access one of the axes of the controller via the axis adress stored in asynUser.
|
||||
If the axis does not exist or is not a Axis, a nullptr is returned and an
|
||||
error is emitted.
|
||||
*/
|
||||
newPmacV3Axis *newPmacV3Controller::getAxis(asynUser *pasynUser) {
|
||||
asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser);
|
||||
return newPmacV3Controller::castToAxis(asynAxis);
|
||||
}
|
||||
|
||||
/*
|
||||
Access one of the axes of the controller via the axis index.
|
||||
If the axis does not exist or is not a Axis, the function must return Null
|
||||
*/
|
||||
newPmacV3Axis *newPmacV3Controller::getAxis(int axisNo) {
|
||||
asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo);
|
||||
return newPmacV3Controller::castToAxis(asynAxis);
|
||||
}
|
||||
|
||||
newPmacV3Axis *newPmacV3Controller::castToAxis(asynMotorAxis *asynAxis) {
|
||||
static const char *functionName = "newPmacV3Controller::getAxis";
|
||||
|
||||
// =========================================================================
|
||||
|
||||
// If the axis slot of the pAxes_ array is empty, a nullptr must be returned
|
||||
if (asynAxis == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// Here, an error is emitted since asyn_axis is not a nullptr but also not
|
||||
// an instance of Axis
|
||||
newPmacV3Axis *axis = dynamic_cast<newPmacV3Axis *>(asynAxis);
|
||||
if (axis == nullptr) {
|
||||
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
|
||||
"%s: Axis %d is not an instance of newPmacV3Axis",
|
||||
functionName, axis->axisNo_);
|
||||
}
|
||||
return axis;
|
||||
}
|
||||
|
||||
/*
|
||||
Sends the given command to the axis specified by axisNo and returns the response
|
||||
of the axis.
|
||||
*/
|
||||
asynStatus newPmacV3Controller::writeRead(int axisNo, const char *command,
|
||||
char *response,
|
||||
bool expect_response) {
|
||||
// Definition of local variables.
|
||||
static const char *functionName = "newPmacV3Controller::writeRead";
|
||||
asynStatus status = asynSuccess;
|
||||
asynStatus pl_status = asynSuccess;
|
||||
char full_command[MAXBUF_] = {0};
|
||||
char user_message[MAXBUF_] = {0};
|
||||
int motorStatusProblem = 0;
|
||||
|
||||
// Send the message and block the thread until either a response has been
|
||||
// received or the timeout is triggered
|
||||
int eomReason = 0; // Flag indicating why the message has ended
|
||||
// Number of bytes of the outgoing message (which is command + the
|
||||
// end-of-string terminator defined in the constructor)
|
||||
size_t nbytesOut = 0;
|
||||
// Number of bytes of the incoming message (which is response + the
|
||||
// end-of-string terminator defined in the constructor)
|
||||
size_t nbytesIn = 0;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
newPmacV3Axis *axis = getAxis(axisNo);
|
||||
if (axis == nullptr) {
|
||||
// We already did the error logging directly in getAxis
|
||||
return asynError;
|
||||
}
|
||||
|
||||
/*
|
||||
The message protocol of the pmacV3 used at PSI looks as follows (all
|
||||
characters immediately following each other without a newline):
|
||||
0x40 (ASCII value of @) -> Request for download
|
||||
0xBF (ASCII value of ¿) -> Select mode "get_response"
|
||||
0x00 (ASCII value of 0)
|
||||
0x00 (ASCII value of 0)
|
||||
0x00 (ASCII value of 0)
|
||||
0x00 (ASCII value of 0)
|
||||
0x00 (ASCII value of 0)
|
||||
[message length in network byte order] -> Use the htons function for this
|
||||
value [Actual message] It is not necessary to append a terminator, since
|
||||
this protocol encodes the message length at the beginning. See Turbo PMAC
|
||||
User Manual, page 418 in VR_PMAC_GETRESPONSE
|
||||
|
||||
The message has to be build manually into the buffer full_command, since it
|
||||
contains NULL terminators in its middle, therefore the string manipulation
|
||||
methods of C don't work.
|
||||
*/
|
||||
|
||||
// The entire message is equal to the command length
|
||||
const size_t commandLength =
|
||||
strlen(command) + 1; // +1 because of the appended /r
|
||||
const int offset = 8;
|
||||
|
||||
// Positions 2 to 6 must have the value 0. Since full_command is initialized
|
||||
// as an array of zeros, we don't need to set these bits manually.
|
||||
full_command[0] = '\x40';
|
||||
full_command[1] = '\xBF';
|
||||
full_command[7] = commandLength;
|
||||
|
||||
snprintf((char *)full_command + offset, MAXBUF_ - offset, "%s\r", command);
|
||||
asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER,
|
||||
"%s: Sending command: %s\n", functionName, full_command);
|
||||
|
||||
// Perform the actual writeRead
|
||||
status = pasynOctetSyncIO->writeRead(
|
||||
lowLevelPortUser_, full_command, commandLength + offset, response,
|
||||
MAXBUF_, TIMEOUT_, &nbytesOut, &nbytesIn, &eomReason);
|
||||
|
||||
/*
|
||||
If we expect a response, check if we got one. If no response was received,
|
||||
flush the PMAC and try again. If that fails as well, return an error
|
||||
*/
|
||||
if (expect_response && strlen(response) == 0) {
|
||||
// Flush message as defined in Turbo PMAC User Manual, p. 430:
|
||||
// \x40\xB3000
|
||||
// VR_DOWNLOAD = \x40
|
||||
// VR_PMAC_FLUSH = \xB3
|
||||
char flush_msg[5] = {0};
|
||||
flush_msg[0] = '\x40';
|
||||
flush_msg[1] = '\xB3';
|
||||
size_t nbytesOut = 0;
|
||||
status = pasynOctetSyncIO->write(lowLevelPortUser_, flush_msg, 5,
|
||||
TIMEOUT_, &nbytesOut);
|
||||
|
||||
// Wait after the flush so the MCU has time to prepare for the
|
||||
// next command
|
||||
usleep(100000);
|
||||
|
||||
if (status == asynSuccess) {
|
||||
// If flushing the MCU succeded, try to send the command again
|
||||
status = pasynOctetSyncIO->writeRead(
|
||||
lowLevelPortUser_, full_command, commandLength + offset,
|
||||
response, MAXBUF_, TIMEOUT_, &nbytesOut, &nbytesIn, &eomReason);
|
||||
|
||||
// If the command returned an empty string for the second time, give
|
||||
// up and propagate the error.
|
||||
if (strlen(response) == 0) {
|
||||
status = asynError;
|
||||
}
|
||||
} else {
|
||||
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: Unable to flush MCU (%s).\n", functionName,
|
||||
stringifyAsynStatus(status));
|
||||
}
|
||||
}
|
||||
|
||||
// Create custom error messages for different failure modes
|
||||
switch (status) {
|
||||
case asynSuccess:
|
||||
break; // Communicate nothing
|
||||
case asynTimeout:
|
||||
snprintf(user_message, sizeof(user_message),
|
||||
"connection timeout for axis %d", axisNo);
|
||||
break;
|
||||
case asynDisconnected:
|
||||
snprintf(user_message, sizeof(user_message), "axis is not connected");
|
||||
break;
|
||||
case asynDisabled:
|
||||
snprintf(user_message, sizeof(user_message), "axis is disabled");
|
||||
break;
|
||||
default:
|
||||
snprintf(user_message, sizeof(user_message),
|
||||
"Communication failed (%s)", stringifyAsynStatus(status));
|
||||
break;
|
||||
}
|
||||
|
||||
if (status != asynSuccess) {
|
||||
// Check if the axis already is in an error communication mode. If it is
|
||||
// not, upstream the error. This is done to avoid "flooding" the user
|
||||
// with different error messages if more than one error ocurred before
|
||||
// an error-free communication
|
||||
|
||||
pl_status =
|
||||
getIntegerParam(axisNo, motorStatusProblem_, &motorStatusProblem);
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, functionName,
|
||||
"motorStatusProblem_");
|
||||
}
|
||||
|
||||
if (motorStatusProblem == 0) {
|
||||
pl_status = axis->setStringParam(this->messageText_, user_message);
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Log the overall status (communication successfull or not)
|
||||
if (status == asynSuccess) {
|
||||
asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER,
|
||||
"%s: device response: %s\n", functionName, response);
|
||||
pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 0);
|
||||
} else {
|
||||
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
|
||||
"%s: asynOctetSyncIO->writeRead failed for command %s on "
|
||||
"axis %d (%s)\n",
|
||||
functionName, command, axisNo, stringifyAsynStatus(status));
|
||||
pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 1);
|
||||
}
|
||||
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, functionName,
|
||||
"motorStatusCommsError_");
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus newPmacV3Controller::writeInt32(asynUser *pasynUser,
|
||||
epicsInt32 value) {
|
||||
|
||||
int function = pasynUser->reason;
|
||||
asynStatus status = asynSuccess;
|
||||
static const char *functionName = "newPmacV3Controller::writeInt32";
|
||||
|
||||
// =========================================================================
|
||||
|
||||
newPmacV3Axis *axis = getAxis(pasynUser);
|
||||
if (axis == nullptr) {
|
||||
// We already did the error logging directly in getAxis
|
||||
return asynError;
|
||||
}
|
||||
|
||||
// Handle custom PVs
|
||||
if (function == enableMotor_) {
|
||||
return axis->enable(value);
|
||||
|
||||
} else if (function == rereadEncoderPosition_) {
|
||||
|
||||
char encoderType[MAXBUF_] = {0};
|
||||
|
||||
/*
|
||||
This is not a command that can always be run when enabling a
|
||||
motor as it also causes relative encoders to reread a position
|
||||
necessitating recalibration. We only want it to run on absolute
|
||||
encoders. We also want it to be clear to instrument scientists, that
|
||||
power has to be cut to the motor, in order to reread the encoder as not
|
||||
all motors have breaks and they may start to move when disabled. For
|
||||
that reason, we don't automatically disable the motors to run the
|
||||
command and instead require that the scientists first disable the motor.
|
||||
*/
|
||||
|
||||
if (!value) {
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
// Poll the current status of the axis
|
||||
bool moving = false;
|
||||
status = axis->poll(&moving);
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
|
||||
// Check if this is an absolute encoder
|
||||
status = axis->readEncoderType();
|
||||
if (status != asynSuccess) {
|
||||
return status;
|
||||
}
|
||||
|
||||
status = getStringParam(axis->axisNo_, encoderType_, encoderType);
|
||||
if (status != asynSuccess) {
|
||||
return paramLibAccessFailed(status, functionName, "encoderType_");
|
||||
}
|
||||
|
||||
// Abort if the axis is incremental
|
||||
if (strcmp(encoderType, IncrementalEncoder) == 0) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_WARNING,
|
||||
"%s: Trying to reread absolute encoder of axis %d on "
|
||||
"controller %s, but it is a relative encoder.\n",
|
||||
functionName, axis->axisNo_, portName);
|
||||
status = setStringParam(messageText_,
|
||||
"Cannot reread an incremental encoder.");
|
||||
if (status != asynSuccess) {
|
||||
return paramLibAccessFailed(status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
return asynError;
|
||||
}
|
||||
|
||||
// Check if the axis is disabled. If not, inform the user that this
|
||||
// is necessary
|
||||
int enabled = 0;
|
||||
status = getIntegerParam(axis->axisNo_, motorEnabled_, &enabled);
|
||||
if (status != asynSuccess) {
|
||||
return paramLibAccessFailed(status, functionName, "motorEnabled_");
|
||||
}
|
||||
|
||||
if (enabled == 1) {
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACE_WARNING,
|
||||
"%s: Axis %d on controller %s must be disabled before "
|
||||
"rereading the encoder.\n",
|
||||
functionName, axis->axisNo_, portName);
|
||||
status = setStringParam(
|
||||
messageText_,
|
||||
"Axis must be disabled before rereading the encoder.");
|
||||
if (status != asynSuccess) {
|
||||
return paramLibAccessFailed(status, functionName,
|
||||
"messageText_");
|
||||
}
|
||||
return asynError;
|
||||
}
|
||||
|
||||
// Switching on the axis while the rereading process is still ongoing
|
||||
// causes it to fail. We currently have no way to check if it is
|
||||
// actually finished, so we instead wait for 0.5 seconds.
|
||||
usleep(500000);
|
||||
|
||||
// turn off parameter as finished rereading
|
||||
// this will only be immediately noticed in the read back variable
|
||||
// though
|
||||
status = axis->setIntegerParam(rereadEncoderPosition_, 0);
|
||||
if (status != asynSuccess) {
|
||||
return paramLibAccessFailed(status, functionName,
|
||||
"rereadEncoderPosition_");
|
||||
}
|
||||
return asynSuccess;
|
||||
|
||||
} else {
|
||||
|
||||
return asynMotorController::writeInt32(pasynUser, value);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Overloaded from asynMotorController because the special cases "motor enabling"
|
||||
and "rereading the encoder" must be covered.
|
||||
*/
|
||||
asynStatus newPmacV3Controller::readInt32(asynUser *pasynUser,
|
||||
epicsInt32 *value) {
|
||||
|
||||
int function = pasynUser->reason;
|
||||
asynStatus status = asynError;
|
||||
static const char *functionName = "newPmacV3Controller::readInt32";
|
||||
|
||||
// =====================================================================
|
||||
|
||||
newPmacV3Axis *axis = getAxis(pasynUser);
|
||||
if (axis == nullptr) {
|
||||
// We already did the error logging directly in getAxis
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if (function == rereadEncoderPositionRBV_) {
|
||||
// Readback value for rereadEncoderPosition
|
||||
status = getIntegerParam(axis->axisNo_, rereadEncoderPosition_, value);
|
||||
if (status != asynSuccess) {
|
||||
return paramLibAccessFailed(status, functionName,
|
||||
"rereadEncoderPosition_");
|
||||
}
|
||||
status =
|
||||
setIntegerParam(axis->axisNo_, rereadEncoderPositionRBV_, *value);
|
||||
if (status != asynSuccess) {
|
||||
return paramLibAccessFailed(status, functionName,
|
||||
"rereadEncoderPositionRBV_");
|
||||
}
|
||||
|
||||
// Update the PVs from the parameter library
|
||||
return callParamCallbacks();
|
||||
} else {
|
||||
return asynMotorController::readInt32(pasynUser, value);
|
||||
}
|
||||
}
|
||||
|
||||
const char *newPmacV3Controller::stringifyAsynStatus(asynStatus status) {
|
||||
// See
|
||||
// https://github.com/epics-modules/asyn/blob/master/asyn/asynDriver/asynDriver.h
|
||||
// and
|
||||
// https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/paramErrors.h
|
||||
// for the definition of the error codes
|
||||
switch (status) {
|
||||
case asynSuccess:
|
||||
return asynSuccessStringified;
|
||||
case asynTimeout:
|
||||
return asynTimeoutStringified;
|
||||
case asynOverflow:
|
||||
return asynOverflowStringified;
|
||||
case asynError:
|
||||
return asynErrorStringified;
|
||||
case asynDisconnected:
|
||||
return asynDisconnectedStringified;
|
||||
case asynDisabled:
|
||||
return asynDisabledStringified;
|
||||
case asynParamAlreadyExists:
|
||||
return asynParamAlreadyExistsStringified;
|
||||
case asynParamNotFound:
|
||||
return asynParamNotFoundStringified;
|
||||
case asynParamWrongType:
|
||||
return asynParamWrongTypeStringified;
|
||||
case asynParamBadIndex:
|
||||
return asynParamBadIndexStringified;
|
||||
case asynParamUndefined:
|
||||
return asynParamUndefinedStringified;
|
||||
case asynParamInvalidList:
|
||||
return asynParamInvalidListStringified;
|
||||
}
|
||||
|
||||
asynPrint(this->lowLevelPortUser_, ASYN_TRACE_ERROR,
|
||||
"newPmacV3Controller::stringifyAsynStatus: FATAL error: Reached "
|
||||
"unreachable code. Terminating\n");
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
asynStatus newPmacV3Controller::paramLibAccessFailed(asynStatus status,
|
||||
const char *functionName,
|
||||
const char *parameter) {
|
||||
char message[MAXBUF_] = {0};
|
||||
snprintf(message, sizeof(message),
|
||||
"Accessing the parameter library failed for parameter %s (%s). "
|
||||
"This is a bug, please inform the software support.\n",
|
||||
parameter, stringifyAsynStatus(status));
|
||||
|
||||
// Log the error message and try to propagate it
|
||||
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, "%s: %s", functionName,
|
||||
message);
|
||||
setStringParam(messageText_, message);
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus newPmacV3Controller::checkNumExpectedReads(int expected, int read,
|
||||
const char *functionName,
|
||||
const char *command,
|
||||
const char *response,
|
||||
int axisNo_) {
|
||||
if (expected == read) {
|
||||
return asynSuccess;
|
||||
} else {
|
||||
char message[MAXBUF_] = {0};
|
||||
snprintf(message, sizeof(message),
|
||||
"Could not interpret response %s for command %s (axis %d). "
|
||||
"This is a bug.",
|
||||
response, command, axisNo_);
|
||||
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, "%s: %s", functionName,
|
||||
message);
|
||||
setStringParam(messageText_, message);
|
||||
setIntegerParam(motorStatusCommsError_, 1);
|
||||
return asynError;
|
||||
}
|
||||
}
|
||||
|
||||
/*************************************************************************************/
|
||||
/** The following functions are C-wrappers, and can be called directly from
|
||||
* iocsh */
|
||||
|
||||
extern "C" {
|
||||
|
||||
/*
|
||||
C wrapper for the Controller constructor.
|
||||
*/
|
||||
asynStatus newPmacV3CreateController(const char *portName,
|
||||
const char *lowLevelPortName, int numAxes,
|
||||
double movingPollPeriod,
|
||||
double idlePollPeriod) {
|
||||
/*
|
||||
We create a new instance of CreateController, using the "new" keyword to
|
||||
allocate it on the heap while avoiding RAII. TBD: Where is the pointer to
|
||||
the controller stored?
|
||||
https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorController.cpp
|
||||
https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/asynPortDriver.cpp
|
||||
|
||||
Setting the pointer to nullptr / NULL immediately after construction is
|
||||
simply done to avoid compiler warnings, see page 7 of this document:
|
||||
https://subversion.xray.aps.anl.gov/synApps/measComp/trunk/documentation/measCompTutorial.pdf
|
||||
*/
|
||||
newPmacV3Controller *pController = new newPmacV3Controller(
|
||||
portName, lowLevelPortName, numAxes, movingPollPeriod, idlePollPeriod);
|
||||
pController = NULL;
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
/*
|
||||
C wrapper for the Axis constructor.
|
||||
See Axis::Axis.
|
||||
*/
|
||||
asynStatus newPmacV3CreateAxis(const char *port, int axis) {
|
||||
newPmacV3Axis *pAxis;
|
||||
|
||||
static const char *functionName = "newPmacV3CreateAxis";
|
||||
|
||||
/*
|
||||
findAsynPortDriver is a asyn library FFI function which uses the C ABI.
|
||||
Therefore it returns a void pointer instead of e.g. a pointer to a
|
||||
superclass of the controller such as asynPortDriver. Type-safe upcasting via
|
||||
dynamic_cast is therefore not possible directly. However, we do know that
|
||||
the void pointer is either a pointer to asynPortDriver (if a driver with the
|
||||
specified name exists) or a nullptr. Therefore, we first do a nullptr check,
|
||||
then a cast to asynPortDriver and lastly a (typesafe) dynamic_upcast to
|
||||
Controller
|
||||
https://stackoverflow.com/questions/70906749/is-there-a-safe-way-to-cast-void-to-class-pointer-in-c
|
||||
*/
|
||||
void *ptr = findAsynPortDriver(port);
|
||||
if (ptr == nullptr) {
|
||||
/*
|
||||
We can't use asynPrint here since this macro would require us
|
||||
to get a lowLevelPortUser_ from a pointer to an asynPortDriver.
|
||||
However, the given pointer is a nullptr and therefore doesn't
|
||||
have a lowLevelPortUser_! printf is an EPICS alternative which
|
||||
works w/o that, but doesn't offer the comfort provided
|
||||
by the asynTrace-facility
|
||||
*/
|
||||
printf("%s:%s: Error port %s not found\n", driverName, functionName,
|
||||
port);
|
||||
return asynError;
|
||||
}
|
||||
// Unsafe cast of the pointer to an asynPortDriver
|
||||
asynPortDriver *apd = (asynPortDriver *)(ptr);
|
||||
|
||||
// Safe downcast
|
||||
newPmacV3Controller *pC = dynamic_cast<newPmacV3Controller *>(apd);
|
||||
if (pC == nullptr) {
|
||||
printf("%s: controller on port %s is not a Controller\n", functionName,
|
||||
port);
|
||||
return asynError;
|
||||
}
|
||||
|
||||
// Prevent manipulation of the controller from other threads while we create
|
||||
// the new axis.
|
||||
pC->lock();
|
||||
|
||||
/*
|
||||
We create a new instance of Axis, using the "new" keyword to allocate it
|
||||
on the heap while avoiding RAII. In the constructor, a pointer to the new
|
||||
object is stored in the controller object "pC". Therefore, the axis instance
|
||||
can still be reached later by quering "pC".
|
||||
|
||||
Setting the pointer to nullptr / NULL immediately after construction is
|
||||
simply done to avoid compiler warnings, see page 7 of this document:
|
||||
https://subversion.xray.aps.anl.gov/synApps/measComp/trunk/documentation/measCompTutorial.pdf
|
||||
*/
|
||||
pAxis = new newPmacV3Axis(pC, axis);
|
||||
pAxis = nullptr;
|
||||
|
||||
// Allow manipulation of the controller again
|
||||
pC->unlock();
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
/*
|
||||
This is boilerplate code which is used to make the FFI functions
|
||||
CreateController and CreateAxis "known" to the IOC shell (iocsh).
|
||||
TBD: If the code is compiled for running on vxWorks, this registration is
|
||||
apparently not necessary?
|
||||
*/
|
||||
|
||||
#ifdef vxWorks
|
||||
#else
|
||||
|
||||
/*
|
||||
Define name and type of the arguments for the CreateController function
|
||||
in the iocsh. This is done by creating structs with the argument names and types
|
||||
and then providing "factory" functions (configCreateControllerCallFunc).
|
||||
These factory functions are used to register the constructors during
|
||||
compilation.
|
||||
*/
|
||||
static const iocshArg CreateControllerArg0 = {"Controller port name",
|
||||
iocshArgString};
|
||||
static const iocshArg CreateControllerArg1 = {"Low level port name",
|
||||
iocshArgString};
|
||||
static const iocshArg CreateControllerArg2 = {"Number of axes", iocshArgInt};
|
||||
static const iocshArg CreateControllerArg3 = {"Moving poll rate (s)",
|
||||
iocshArgDouble};
|
||||
static const iocshArg CreateControllerArg4 = {"Idle poll rate (s)",
|
||||
iocshArgDouble};
|
||||
static const iocshArg *const CreateControllerArgs[] = {
|
||||
&CreateControllerArg0, &CreateControllerArg1, &CreateControllerArg2,
|
||||
&CreateControllerArg3, &CreateControllerArg4};
|
||||
static const iocshFuncDef configNewPmacV3CreateController = {
|
||||
"newPmacV3CreateController", 5, CreateControllerArgs};
|
||||
static void configNewPmacV3CreateControllerCallFunc(const iocshArgBuf *args) {
|
||||
newPmacV3CreateController(args[0].sval, args[1].sval, args[2].ival,
|
||||
args[3].dval, args[4].dval);
|
||||
}
|
||||
|
||||
/*
|
||||
Same procedure as for the CreateController function, but for the axis
|
||||
itself.
|
||||
*/
|
||||
static const iocshArg CreateAxisArg0 = {"Controller port name", iocshArgString};
|
||||
static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt};
|
||||
static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0,
|
||||
&CreateAxisArg1};
|
||||
static const iocshFuncDef configNewPmacV3CreateAxis = {"newPmacV3CreateAxis", 2,
|
||||
CreateAxisArgs};
|
||||
static void configNewPmacV3CreateAxisCallFunc(const iocshArgBuf *args) {
|
||||
newPmacV3CreateAxis(args[0].sval, args[1].ival);
|
||||
}
|
||||
|
||||
// This function is made known to EPICS in sinq.dbd and is called by EPICS
|
||||
// in order to register both functions in the IOC shell
|
||||
// TBD: Does this happen during compilation?
|
||||
static void newPmacV3ControllerRegister(void) {
|
||||
iocshRegister(&configNewPmacV3CreateController,
|
||||
configNewPmacV3CreateControllerCallFunc);
|
||||
iocshRegister(&configNewPmacV3CreateAxis,
|
||||
configNewPmacV3CreateAxisCallFunc);
|
||||
}
|
||||
epicsExportRegistrar(newPmacV3ControllerRegister);
|
||||
|
||||
#endif
|
||||
|
||||
} // extern "C"
|
@ -1,92 +0,0 @@
|
||||
/********************************************
|
||||
* pmacV3Controller.h
|
||||
*
|
||||
* PMAC V3 controller driver based on the asynMotorController class
|
||||
*
|
||||
* Stefan Mathis, September 2024
|
||||
********************************************/
|
||||
|
||||
#ifndef pmacV3Controller_H
|
||||
#define pmacV3Controller_H
|
||||
#include "asynMotorAxis.h"
|
||||
#include "asynMotorController.h"
|
||||
#include "newPmacV3Axis.h"
|
||||
|
||||
#define IncrementalEncoder "Incremental encoder"
|
||||
#define AbsoluteEncoder "Absolute encoder"
|
||||
|
||||
class newPmacV3Controller : public asynMotorController {
|
||||
|
||||
public:
|
||||
newPmacV3Controller(const char *portName, const char *lowLevelPortName,
|
||||
int numAxes, double movingPollPeriod,
|
||||
double idlePollPeriod, const int &extraParams = 2);
|
||||
|
||||
virtual ~newPmacV3Controller();
|
||||
|
||||
/* Overloaded methods methods */
|
||||
newPmacV3Axis *getAxis(asynUser *pasynUser);
|
||||
newPmacV3Axis *getAxis(int axisNo);
|
||||
|
||||
// overloaded because we want to enable/disable the motor
|
||||
asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
|
||||
|
||||
// overloaded because we want to read the axis state
|
||||
asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
|
||||
|
||||
// // Overloaded to read configuration details from the motor records
|
||||
// asynStatus drvUserCreate(asynUser *pasynUser, const char *drvInfo,
|
||||
// const char **pptypeName, size_t *psize);
|
||||
|
||||
protected:
|
||||
asynUser *lowLevelPortUser_;
|
||||
|
||||
asynStatus writeRead(int axisNo, const char *command, char *response,
|
||||
bool expect_response);
|
||||
|
||||
// Create a descriptive string out of an asynStatus which can be used for
|
||||
// logging or communicating with the user
|
||||
const char *stringifyAsynStatus(asynStatus status);
|
||||
asynStatus paramLibAccessFailed(asynStatus status, const char *functionName,
|
||||
const char *parameter);
|
||||
asynStatus checkNumExpectedReads(int expected, int read,
|
||||
const char *functionName,
|
||||
const char *command, const char *response,
|
||||
int axisNo_);
|
||||
newPmacV3Axis *castToAxis(asynMotorAxis *asynAxis);
|
||||
|
||||
private:
|
||||
// Set the maximum buffer size. This is an empirical value which must be
|
||||
// large enough to avoid overflows for all commands to the device /
|
||||
// responses from it.
|
||||
static const uint32_t MAXBUF_ = 200;
|
||||
|
||||
/*
|
||||
When trying to communicate with the device, the underlying asynOctetSyncIO
|
||||
interface waits for a response until this time (in seconds) has passed,
|
||||
then it declares a timeout. This variable has to be specified in the
|
||||
.cpp-file.
|
||||
*/
|
||||
static const double TIMEOUT_;
|
||||
|
||||
// Indices of additional PVs
|
||||
int messageText_;
|
||||
int enableMotor_;
|
||||
int motorEnabled_;
|
||||
int rereadEncoderPosition_;
|
||||
int rereadEncoderPositionRBV_;
|
||||
int readConfig_;
|
||||
int encoderType_;
|
||||
int motorPositionRBV_;
|
||||
|
||||
/*
|
||||
If the time between two sent messages is too short, the MCU communication
|
||||
module might "lose" an answer. To prevent this, a small delay is introduced
|
||||
in EPICS after each message exchange. Unit is microseconds.
|
||||
*/
|
||||
int afterMessageSleep_;
|
||||
|
||||
friend class newPmacV3Axis;
|
||||
};
|
||||
|
||||
#endif /* pmacV3Controller_H */
|
@ -1186,6 +1186,8 @@ AmorDetectorAxis::AmorDetectorAxis(pmacController *pC, int axisNo, int function)
|
||||
pC_->debugFlow(functionName);
|
||||
|
||||
_function = function;
|
||||
det_starting = false;
|
||||
det_startTime = time(NULL);
|
||||
|
||||
callParamCallbacks();
|
||||
|
||||
|
@ -1,169 +1,180 @@
|
||||
/********************************************
|
||||
* pmacAxis.cpp
|
||||
*
|
||||
* PMAC Asyn motor based on the
|
||||
*
|
||||
* PMAC Asyn motor based on the
|
||||
* asynMotorAxis class.
|
||||
*
|
||||
*
|
||||
* Matthew Pearson
|
||||
* 23 May 2012
|
||||
*
|
||||
*
|
||||
* Modified to use the MsgTxt field for SINQ
|
||||
*
|
||||
* Mark Koennecke, January 2019
|
||||
*
|
||||
* EXtended with special motor axis for the Selene
|
||||
* EXtended with special motor axis for the Selene
|
||||
* guide, Mark Koennecke, February 2020
|
||||
********************************************/
|
||||
|
||||
#ifndef pmacAxis_H
|
||||
#define pmacAxis_H
|
||||
|
||||
#include "SINQController.h"
|
||||
#include "SINQAxis.h"
|
||||
#include "SINQController.h"
|
||||
|
||||
class pmacController;
|
||||
class SeleneController;
|
||||
|
||||
class pmacAxis : public SINQAxis
|
||||
{
|
||||
class pmacAxis : public SINQAxis {
|
||||
public:
|
||||
/* These are the methods we override from the base class */
|
||||
pmacAxis(pmacController *pController, int axisNo, bool enable=true);
|
||||
virtual ~pmacAxis();
|
||||
asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration);
|
||||
asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration);
|
||||
asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards);
|
||||
asynStatus stop(double acceleration);
|
||||
asynStatus poll(bool *moving);
|
||||
asynStatus setPosition(double position);
|
||||
asynStatus enable(int on);
|
||||
/* These are the methods we override from the base class */
|
||||
pmacAxis(pmacController *pController, int axisNo, bool enable = true);
|
||||
virtual ~pmacAxis();
|
||||
asynStatus move(double position, int relative, double min_velocity,
|
||||
double max_velocity, double acceleration);
|
||||
asynStatus moveVelocity(double min_velocity, double max_velocity,
|
||||
double acceleration);
|
||||
asynStatus home(double min_velocity, double max_velocity,
|
||||
double acceleration, int forwards);
|
||||
asynStatus stop(double acceleration);
|
||||
asynStatus poll(bool *moving);
|
||||
asynStatus setPosition(double position);
|
||||
asynStatus enable(int on);
|
||||
|
||||
protected:
|
||||
pmacController *pC_;
|
||||
|
||||
asynStatus getAxisStatus(bool *moving);
|
||||
asynStatus getAxisInitialStatus(void);
|
||||
protected:
|
||||
pmacController *pC_;
|
||||
|
||||
double setpointPosition_;
|
||||
double encoderPosition_;
|
||||
double currentVelocity_;
|
||||
double velocity_;
|
||||
double accel_;
|
||||
double highLimit_;
|
||||
double lowLimit_;
|
||||
double scale_;
|
||||
double previous_position_;
|
||||
int previous_direction_;
|
||||
int encoder_axis_;
|
||||
int axisErrorCount;
|
||||
asynStatus getAxisStatus(bool *moving);
|
||||
asynStatus getAxisInitialStatus(void);
|
||||
|
||||
time_t startTime;
|
||||
time_t status6Time;
|
||||
int starting;
|
||||
int homing;
|
||||
double statusPos;
|
||||
double setpointPosition_;
|
||||
double encoderPosition_;
|
||||
double currentVelocity_;
|
||||
double velocity_;
|
||||
double accel_;
|
||||
double highLimit_;
|
||||
double lowLimit_;
|
||||
double scale_;
|
||||
double previous_position_;
|
||||
int previous_direction_;
|
||||
int encoder_axis_;
|
||||
int axisErrorCount;
|
||||
|
||||
time_t next_poll;
|
||||
time_t startTime;
|
||||
time_t status6Time;
|
||||
int starting;
|
||||
int homing;
|
||||
double statusPos;
|
||||
|
||||
bool autoEnable;
|
||||
|
||||
friend class pmacController;
|
||||
friend class pmacV3Controller;
|
||||
time_t next_poll;
|
||||
|
||||
bool autoEnable;
|
||||
|
||||
friend class pmacController;
|
||||
friend class pmacV3Controller;
|
||||
};
|
||||
/*--------------------------------------------------------------------------------------------*/
|
||||
class SeleneAxis : public pmacAxis
|
||||
{
|
||||
class SeleneAxis : public pmacAxis {
|
||||
public:
|
||||
SeleneAxis(SeleneController *pController, int axisNo, double limitTarget);
|
||||
asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration);
|
||||
asynStatus setPosition(double position);
|
||||
asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards);
|
||||
SeleneAxis(SeleneController *pController, int axisNo, double limitTarget);
|
||||
asynStatus move(double position, int relative, double min_velocity,
|
||||
double max_velocity, double acceleration);
|
||||
asynStatus setPosition(double position);
|
||||
asynStatus home(double min_velocity, double max_velocity,
|
||||
double acceleration, int forwards);
|
||||
|
||||
protected:
|
||||
friend class SeleneController;
|
||||
friend class pmacController;
|
||||
|
||||
private:
|
||||
double limitTarget;
|
||||
asynStatus getSeleneAxisInitialStatus(void);
|
||||
protected:
|
||||
friend class SeleneController;
|
||||
friend class pmacController;
|
||||
|
||||
private:
|
||||
double limitTarget;
|
||||
asynStatus getSeleneAxisInitialStatus(void);
|
||||
};
|
||||
|
||||
/*
|
||||
Yet another special set of motors for the Selene Guide at AMOR. Each segment can be lifted or tilted. This is
|
||||
two motors. One acts as a slave and only writes a new target, the other also sets a new target and sends the
|
||||
actual movement command. Both motors are coordianted in the motor controller in order to avoid tension on
|
||||
the guide elements. This gaves rise to the function code LIFTSLAVE and LIFTMASTER.
|
||||
Yet another special set of motors for the Selene Guide at AMOR. Each segment
|
||||
can be lifted or tilted. This is two motors. One acts as a slave and only
|
||||
writes a new target, the other also sets a new target and sends the actual
|
||||
movement command. Both motors are coordianted in the motor controller in order
|
||||
to avoid tension on the guide elements. This gaves rise to the function code
|
||||
LIFTSLAVE and LIFTMASTER.
|
||||
|
||||
In another mode the whole guide can be lifted or tilted. Then motor 1 and 6 get new values and one of them
|
||||
sends the drive command. This causes all 6 motors to drive synchronously to their new targets. This is
|
||||
implemented through the LIFTSEGMENT function code.
|
||||
In another mode the whole guide can be lifted or tilted. Then motor 1 and 6
|
||||
get new values and one of them sends the drive command. This causes all 6
|
||||
motors to drive synchronously to their new targets. This is implemented
|
||||
through the LIFTSEGMENT function code.
|
||||
|
||||
Mark Koennecke, February 2020
|
||||
Mark Koennecke, February 2020
|
||||
|
||||
The axis should not be enabled automatically
|
||||
|
||||
Michele Brambilla, February 2020
|
||||
Michele Brambilla, February 2020
|
||||
|
||||
*/
|
||||
class LiftAxis : public pmacAxis
|
||||
{
|
||||
public:
|
||||
LiftAxis(pmacController *pController, int axisNo) : pmacAxis((pmacController *)pController,axisNo) {};
|
||||
asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration);
|
||||
asynStatus poll(bool *moving);
|
||||
asynStatus stop(double acceleration);
|
||||
private:
|
||||
int waitStart;
|
||||
class LiftAxis : public pmacAxis {
|
||||
public:
|
||||
/*
|
||||
The default constructor of LiftAxis just forwards to the pmacAxis
|
||||
constructor, which has an optional argument "autoenable" with the default
|
||||
value "true". However, we want that argument to be false, hence we provide
|
||||
an explicit constructor.
|
||||
*/
|
||||
LiftAxis(pmacController *pController, int axisNo)
|
||||
: pmacAxis((pmacController *)pController, axisNo, false) {};
|
||||
asynStatus move(double position, int relative, double min_velocity,
|
||||
double max_velocity, double acceleration);
|
||||
asynStatus poll(bool *moving);
|
||||
asynStatus stop(double acceleration);
|
||||
|
||||
private:
|
||||
int waitStart;
|
||||
};
|
||||
|
||||
/********************************************
|
||||
/********************************************
|
||||
* Protocol version 3 requires just some minor change
|
||||
*
|
||||
* Michele Brambilla, February 2022
|
||||
********************************************/
|
||||
|
||||
class pmacV3Axis : public pmacAxis {
|
||||
public:
|
||||
public:
|
||||
pmacV3Axis(pmacController *pController, int axisNo);
|
||||
|
||||
pmacV3Axis(pmacController *pController, int axisNo);
|
||||
asynStatus move(double position, int relative, double min_velocity,
|
||||
double max_velocity, double acceleration);
|
||||
asynStatus poll(bool *moving);
|
||||
|
||||
asynStatus move(double position, int relative, double min_velocity,
|
||||
double max_velocity, double acceleration);
|
||||
asynStatus poll(bool *moving);
|
||||
protected:
|
||||
int IsEnable;
|
||||
double Speed;
|
||||
protected:
|
||||
int IsEnable;
|
||||
double Speed;
|
||||
|
||||
asynStatus getAxisStatus(bool *moving);
|
||||
asynStatus getAxisStatus(bool *moving);
|
||||
|
||||
friend class pmacController;
|
||||
friend class pmacV3Controller;
|
||||
};
|
||||
|
||||
/*----------------------------------------------------------------------------------------------*/
|
||||
class pmacHRPTAxis : public pmacV3Axis
|
||||
{
|
||||
public:
|
||||
pmacHRPTAxis(pmacController *pController, int axisNo) : pmacV3Axis(pController,axisNo) {};
|
||||
/**
|
||||
* Override getAxisStatus in order to read the special parameter indicating a
|
||||
* slit blade crash at HRPT
|
||||
*/
|
||||
asynStatus getAxisStatus(bool *moving);
|
||||
|
||||
protected:
|
||||
friend class pmacController;
|
||||
|
||||
friend class pmacController;
|
||||
friend class pmacV3Controller;
|
||||
};
|
||||
|
||||
/*----------------------------------------------------------------------------------------------*/
|
||||
class pmacHRPTAxis : public pmacV3Axis {
|
||||
public:
|
||||
pmacHRPTAxis(pmacController *pController, int axisNo)
|
||||
: pmacV3Axis(pController, axisNo) {};
|
||||
/**
|
||||
* Override getAxisStatus in order to read the special parameter indicating
|
||||
* a slit blade crash at HRPT
|
||||
*/
|
||||
asynStatus getAxisStatus(bool *moving);
|
||||
|
||||
protected:
|
||||
friend class pmacController;
|
||||
};
|
||||
|
||||
/*
|
||||
* Special motors for the AMOR detector movement. The whole
|
||||
* command set is different but on a pmac controller. This implements
|
||||
* a coordinated movement of cox, coz and ftz in order not to break
|
||||
* the flight tube which may have been mounted. This is mapped to three
|
||||
* motors selected via the function code: com, the detector omega, coz,
|
||||
* motors selected via the function code: com, the detector omega, coz,
|
||||
* the detector z offset and park, for parking the flightpath.
|
||||
*/
|
||||
|
||||
@ -171,39 +182,41 @@ class pmacHRPTAxis : public pmacV3Axis
|
||||
#define ADCOZ 2
|
||||
#define ADPARK 3
|
||||
|
||||
class AmorDetectorAxis: public pmacAxis {
|
||||
public:
|
||||
AmorDetectorAxis(pmacController *pController, int axisNo, int function);
|
||||
class AmorDetectorAxis : public pmacAxis {
|
||||
public:
|
||||
AmorDetectorAxis(pmacController *pController, int axisNo, int function);
|
||||
|
||||
asynStatus move(double position, int relative, double min_velocity,
|
||||
double max_velocity, double acceleration);
|
||||
asynStatus poll(bool *moving);
|
||||
asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration);
|
||||
asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards);
|
||||
asynStatus stop(double acceleration);
|
||||
asynStatus setPosition(double position);
|
||||
asynStatus move(double position, int relative, double min_velocity,
|
||||
double max_velocity, double acceleration);
|
||||
asynStatus poll(bool *moving);
|
||||
asynStatus moveVelocity(double min_velocity, double max_velocity,
|
||||
double acceleration);
|
||||
asynStatus home(double min_velocity, double max_velocity,
|
||||
double acceleration, int forwards);
|
||||
asynStatus stop(double acceleration);
|
||||
asynStatus setPosition(double position);
|
||||
|
||||
protected:
|
||||
int _function;
|
||||
int det_starting;
|
||||
time_t det_startTime;
|
||||
protected:
|
||||
int _function;
|
||||
int det_starting;
|
||||
time_t det_startTime;
|
||||
};
|
||||
|
||||
/*----------------------------------------------------------------------------------------------*/
|
||||
|
||||
|
||||
class GirderAxis: public pmacV3Axis {
|
||||
class GirderAxis : public pmacV3Axis {
|
||||
public:
|
||||
GirderAxis(pmacController *pController, int axisNo);
|
||||
asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration);
|
||||
asynStatus move(double position, int relative, double min_velocity,
|
||||
double max_velocity, double acceleration);
|
||||
asynStatus stop(double acceleration);
|
||||
asynStatus poll(bool *moving);
|
||||
|
||||
protected:
|
||||
int IsEnable;
|
||||
|
||||
friend class pmacController;
|
||||
friend class pmacV3Controller;
|
||||
friend class pmacController;
|
||||
friend class pmacV3Controller;
|
||||
};
|
||||
|
||||
#endif /* pmacAxis_H */
|
||||
|
@ -5,12 +5,11 @@ registrar(EL734Register)
|
||||
registrar(PhytronRegister)
|
||||
registrar(EuroMoveRegister)
|
||||
registrar(NanotecRegister)
|
||||
registrar(pmacControllerRegister)
|
||||
# registrar(pmacControllerRegister)
|
||||
registrar(C804ControllerRegister)
|
||||
registrar(pmacAsynIPPortRegister)
|
||||
# registrar(pmacAsynIPPortRegister)
|
||||
registrar(MasterMACSRegister)
|
||||
registrar(SINQControllerRegister)
|
||||
registrar(newPmacV3ControllerRegister)
|
||||
|
||||
#--------------------------------------------------------
|
||||
# With the PSI module build system, including these items actually
|
||||
|
213
utils/decodeMasterMACStatusR10.py
Executable file
213
utils/decodeMasterMACStatusR10.py
Executable file
@ -0,0 +1,213 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# List of tuples which encodes the states given in the file description.
|
||||
# Index first with the bit index, then with the bit value
|
||||
interpretation = [
|
||||
("Not ready to be switched on", "Ready to be switched on"), # Bit 0
|
||||
("Not switched on", "Switched on"), # Bit 1
|
||||
("Disabled", "Enabled"), # Bit 2
|
||||
("Ok", "Fault condition set"), # Bit 3
|
||||
("Motor supply voltage absent ", "Motor supply voltage present"), # Bit 4
|
||||
("Motor performs quick stop", "Ok"), # Bit 5
|
||||
("Switch on enabled", "Switch on disabled"), # Bit 6
|
||||
("Ok", "RWarning: Movement function was called while motor is still moving. The function call is ignored"), # Bit 7
|
||||
("Motor is idle", "Motor is currently moving"), # Bit 8
|
||||
("Motor does not execute command messages (local mode)", "Motor does execute command messages (remote mode)"), # Bit 9
|
||||
("Target not reached", "Target reached"), # Bit 10
|
||||
("Ok", "Internal limit active"), # Bit 11
|
||||
("Not specified", "Not specified"), # Bit 12
|
||||
("Not specified", "Not specified"), # Bit 13
|
||||
("No event set or event has not occurred yet", "Set event has occurred"), # Bit 14
|
||||
("Axis off (power disabled)", "Axis on (power enabled)"), # Bit 15
|
||||
]
|
||||
|
||||
def decode(value, big_endian: bool = False):
|
||||
|
||||
interpreted = []
|
||||
|
||||
bit_list = [(value >> shift_ind) & 1
|
||||
for shift_ind in range(value.bit_length())] # little endian
|
||||
|
||||
if big_endian:
|
||||
bit_list.reverse() # big endian
|
||||
|
||||
for (bit, interpretations) in zip(bit_list, interpretation):
|
||||
interpreted.append(interpretations[bit])
|
||||
return (bit_list, interpreted)
|
||||
|
||||
def print_decoded(bit_list, interpreted):
|
||||
for (idx, (bit_value, msg)) in enumerate(zip(bit_list, interpreted)):
|
||||
print(f"Bit {idx} = {bit_value}: {msg}")
|
||||
|
||||
def interactive():
|
||||
|
||||
# Imported here, because curses is not available in Windows. Using the
|
||||
# interactive mode therefore fails on Windows, but at least the single
|
||||
# command mode can be used (which would not be possible if we would import
|
||||
# curses at the top level)
|
||||
import curses
|
||||
|
||||
stdscr = curses.initscr()
|
||||
curses.noecho()
|
||||
curses.cbreak()
|
||||
stdscr.keypad(True)
|
||||
stdscr.scrollok(True)
|
||||
|
||||
stdscr.addstr(">> ")
|
||||
stdscr.refresh()
|
||||
|
||||
history = [""]
|
||||
ptr = len(history) - 1
|
||||
|
||||
while True:
|
||||
c = stdscr.getch()
|
||||
if c == curses.KEY_RIGHT:
|
||||
(y, x) = stdscr.getyx()
|
||||
if x < len(history[ptr]) + 3:
|
||||
stdscr.move(y, x+1)
|
||||
stdscr.refresh()
|
||||
elif c == curses.KEY_LEFT:
|
||||
(y, x) = stdscr.getyx()
|
||||
if x > 3:
|
||||
stdscr.move(y, x-1)
|
||||
stdscr.refresh()
|
||||
elif c == curses.KEY_UP:
|
||||
if ptr > 0:
|
||||
ptr -= 1
|
||||
stdscr.addch("\r")
|
||||
stdscr.clrtoeol()
|
||||
stdscr.addstr(">> " + history[ptr])
|
||||
elif c == curses.KEY_DOWN:
|
||||
if ptr < len(history) - 1:
|
||||
ptr += 1
|
||||
stdscr.addch("\r")
|
||||
stdscr.clrtoeol()
|
||||
stdscr.addstr(">> " + history[ptr])
|
||||
elif c == curses.KEY_ENTER or c == ord('\n') or c == ord('\r'):
|
||||
if history[ptr] == 'quit':
|
||||
break
|
||||
|
||||
# because of arrow keys move back to the end of the line
|
||||
(y, x) = stdscr.getyx()
|
||||
stdscr.move(y, 3+len(history[ptr]))
|
||||
|
||||
if history[ptr]:
|
||||
result = interpret_inputs(history[ptr].split())
|
||||
if result is None:
|
||||
stdscr.addstr(f"\nBAD INPUT: Expected input of 'value [big_endian]', where 'value' is an int or a float and 'big_endian' is an optional boolean argument.")
|
||||
else:
|
||||
(arg, big_endian) = result
|
||||
(bit_list, interpreted) = decode(arg, big_endian)
|
||||
for (idx, (bit_value, msg)) in enumerate(zip(bit_list, interpreted)):
|
||||
stdscr.addstr(f"\nBit {idx} = {bit_value}: {msg}")
|
||||
stdscr.refresh()
|
||||
|
||||
if ptr == len(history) - 1 and history[ptr] != "":
|
||||
history += [""]
|
||||
else:
|
||||
history[-1] = ""
|
||||
ptr = len(history) - 1
|
||||
|
||||
stdscr.addstr("\n>> ")
|
||||
stdscr.refresh()
|
||||
|
||||
else:
|
||||
if ptr < len(history) - 1: # Modifying previous input
|
||||
if len(history[-1]) == 0:
|
||||
history[-1] = history[ptr]
|
||||
ptr = len(history) - 1
|
||||
|
||||
else:
|
||||
history += [history[ptr]]
|
||||
ptr = len(history) - 1
|
||||
|
||||
if c == curses.KEY_BACKSPACE:
|
||||
if len(history[ptr]) == 0:
|
||||
continue
|
||||
(y, x) = stdscr.getyx()
|
||||
history[ptr] = history[ptr][0:x-4] + history[ptr][x-3:]
|
||||
stdscr.addch("\r")
|
||||
stdscr.clrtoeol()
|
||||
stdscr.addstr(">> " + history[ptr])
|
||||
stdscr.move(y, x-1)
|
||||
stdscr.refresh()
|
||||
|
||||
else:
|
||||
(y, x) = stdscr.getyx()
|
||||
history[ptr] = history[ptr][0:x-3] + chr(c) + history[ptr][x-3:]
|
||||
stdscr.addch("\r")
|
||||
stdscr.clrtoeol()
|
||||
stdscr.addstr(">> " + history[ptr])
|
||||
stdscr.move(y, x+1)
|
||||
stdscr.refresh()
|
||||
|
||||
# to quit
|
||||
curses.nocbreak()
|
||||
stdscr.keypad(False)
|
||||
curses.echo()
|
||||
curses.endwin()
|
||||
|
||||
def interpret_inputs(inputs):
|
||||
|
||||
number = None
|
||||
big_endian = False
|
||||
try:
|
||||
number = int(float(inputs[0]))
|
||||
if len(inputs) > 1:
|
||||
second_arg = inputs[1]
|
||||
if second_arg == "True" or second_arg == "true":
|
||||
big_endian = True
|
||||
elif second_arg == "False" or second_arg == "false":
|
||||
big_endian = False
|
||||
else:
|
||||
big_endian = bool(int(second_arg))
|
||||
return (number, big_endian)
|
||||
except:
|
||||
return None
|
||||
|
||||
if __name__ == "__main__":
|
||||
from sys import argv
|
||||
|
||||
if len(argv) == 1:
|
||||
# Start interactive mode
|
||||
interactive()
|
||||
else:
|
||||
|
||||
result = interpret_inputs(argv[1:])
|
||||
|
||||
if result is None:
|
||||
print("""
|
||||
Decode R10 message of MasterMACs
|
||||
------------------
|
||||
|
||||
MasterMACs returns its status message (R10) as a floating-point number.
|
||||
The bits of this float encode different states. These states are stored
|
||||
in the interpretation variable.
|
||||
|
||||
This script can be used in two different ways:
|
||||
|
||||
Option 1: Single Command
|
||||
------------------------
|
||||
|
||||
Usage: decodeMasterMACStatusR10.py value [big_endian]
|
||||
|
||||
'value' is the return value of a R10 command. This value is interpreted
|
||||
bit-wise and the result is printed out. The optional second argument can
|
||||
be used to specify whether the input value needs to be interpreted as
|
||||
little or big endian. Default is False.
|
||||
|
||||
Option 2: CLI Mode
|
||||
------------------
|
||||
|
||||
Usage: decodeMasterMACStatusR10.py
|
||||
|
||||
A prompt will be opened. Type in the return value of a R10 command, hit
|
||||
enter and the interpretation will be printed in the prompt. After that,
|
||||
the next value can be typed in. Type 'quit' to close the prompt.
|
||||
""")
|
||||
else:
|
||||
print("Motor status")
|
||||
print("============")
|
||||
(arg, big_endian) = result
|
||||
(bit_list, interpreted) = decode(arg, big_endian)
|
||||
print_decoded(bit_list, interpreted)
|
@ -64,6 +64,7 @@ if __name__ == "__main__":
|
||||
curses.noecho()
|
||||
curses.cbreak()
|
||||
stdscr.keypad(True)
|
||||
stdscr.scrollok(True)
|
||||
|
||||
stdscr.addstr(">> ")
|
||||
stdscr.refresh()
|
||||
|
Reference in New Issue
Block a user