diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..da3868c --- /dev/null +++ b/.clang-format @@ -0,0 +1,246 @@ +--- +Language: Cpp +# BasedOnStyle: LLVM +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignArrayOfStructures: None +AlignConsecutiveAssignments: + Enabled: false + 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 + AcrossEmptyLines: false + AcrossComments: false + AlignCaseColons: false +AlignEscapedNewlines: Right +AlignOperands: Align +AlignTrailingComments: + Kind: Always + OverEmptyLines: 0 +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowBreakBeforeNoexceptSpecifier: Never +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortCompoundRequirementOnASingleLine: true +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: MultiLine +AttributeMacros: + - __capability +BinPackArguments: true +BinPackParameters: true +BitFieldColonSpacing: Both +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterExternBlock: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakAdjacentStringLiterals: true +BreakAfterAttributes: Leave +BreakAfterJavaFieldAnnotations: false +BreakArrays: true +BreakBeforeBinaryOperators: None +BreakBeforeConceptDeclarations: Always +BreakBeforeBraces: Attach +BreakBeforeInlineASMColon: OnlyMultiline +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeColon +BreakInheritanceList: BeforeColon +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + SortPriority: 0 + CaseSensitive: false + - Regex: '.*' + Priority: 1 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: '(Test)?$' +IncludeIsMainSourceRegex: '' +IndentAccessModifiers: false +IndentCaseBlocks: false +IndentCaseLabels: false +IndentExternBlock: AfterExternBlock +IndentGotoLabels: true +IndentPPDirectives: None +IndentRequiresClause: true +IndentWidth: 4 +IndentWrappedFunctionNames: false +InsertBraces: false +InsertNewlineAtEOF: false +InsertTrailingCommas: None +IntegerLiteralSeparator: + Binary: 0 + BinaryMinDigits: 0 + Decimal: 0 + DecimalMinDigits: 0 + Hex: 0 + HexMinDigits: 0 +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +KeepEmptyLinesAtEOF: false +LambdaBodyIndentation: Signature +LineEnding: DeriveLF +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Auto +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PackConstructorInitializers: BinPack +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakScopeResolution: 500 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyIndentedWhitespace: 0 +PenaltyReturnTypeOnItsOwnLine: 60 +PointerAlignment: Right +PPIndentWidth: -1 +QualifierAlignment: Leave +ReferenceAlignment: Pointer +ReflowComments: true +RemoveBracesLLVM: false +RemoveParentheses: Leave +RemoveSemicolon: false +RequiresClausePosition: OwnLine +RequiresExpressionIndentation: OuterScope +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SkipMacroDefinitionBody: false +SortIncludes: CaseSensitive +SortJavaStaticImport: Before +SortUsingDeclarations: LexicographicNumeric +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceAroundPointerQualifiers: Default +SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeJsonColon: false +SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDefinitionName: false + AfterFunctionDeclarationName: false + AfterIfMacros: true + AfterOverloadedOperator: false + AfterPlacementOperator: true + AfterRequiresInClause: false + AfterRequiresInExpression: false + BeforeNonEmptyParentheses: false +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: Never +SpacesInContainerLiterals: true +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParens: Never +SpacesInParensOptions: + InCStyleCasts: false + InConditionalStatements: false + InEmptyParentheses: false + Other: false +SpacesInSquareBrackets: false +Standard: Latest +StatementAttributeLikeMacros: + - Q_EMIT +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseTab: Never +VerilogBreakBetweenInstancePorts: true +WhitespaceSensitiveMacros: + - BOOST_PP_STRINGIZE + - CF_SWIFT_NAME + - NS_SWIFT_NAME + - PP_STRINGIZE + - STRINGIZE +... + diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000..83a7633 --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,58 @@ +default: + image: docker.psi.ch:5000/sinqdev/sinqepics:latest + +stages: + - lint + - build + - test + +cppcheck: + stage: lint + script: + - cppcheck --std=c++17 --addon=cert --addon=misc --error-exitcode=1 src/*.cpp + artifacts: + expire_in: 1 week + tags: + - sinq + +formatting: + stage: lint + script: + - clang-format --style=file --Werror --dry-run src/*.cpp + artifacts: + expire_in: 1 week + tags: + - sinq + +# clangtidy: +# stage: lint +# 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: +# # - sinq + +build_module: + stage: build + script: + - export SINQMOTOR_VERSION="$(grep 'sinqMotor_VERSION=' Makefile | cut -d= -f2)" + - git clone --depth 1 --branch "${SINQMOTOR_VERSION}" https://gitlab-ci-token:${CI_JOB_TOKEN}@git.psi.ch/sinq-epics-modules/sinqmotor.git + - pushd sinqmotor + - sed -i 's/ARCH_FILTER=.*/ARCH_FILTER=linux%/' Makefile + - echo "LIBVERSION=${SINQMOTOR_VERSION}" >> Makefile + - make install + - popd + - sed -i 's/ARCH_FILTER=.*/ARCH_FILTER=linux%/' Makefile + - echo "LIBVERSION=${CI_COMMIT_TAG:-0.0.1}" >> Makefile + - make install + - cp -rT "/ioc/modules/seleneLift/$(ls -U /ioc/modules/seleneLift/ | head -1)" "./seleneLift-${CI_COMMIT_TAG:-$CI_COMMIT_SHORT_SHA}" + artifacts: + name: "seleneLift-${CI_COMMIT_TAG:-$CI_COMMIT_SHORT_SHA}" + paths: + - "seleneLift-${CI_COMMIT_TAG:-$CI_COMMIT_SHORT_SHA}/*" + expire_in: 1 week + when: always + tags: + - sinq diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..c6f8178 --- /dev/null +++ b/Makefile @@ -0,0 +1,31 @@ +# Use the PSI build system +include /ioc/tools/driver.makefile + +MODULE=turboPmac +BUILDCLASSES=Linux +EPICS_VERSIONS=7.0.7 +ARCH_FILTER=RHEL% + +# Additional module dependencies +REQUIRED+=motorBase +REQUIRED+=sinqMotor + +# Specify the version of motorBase we want to build against +motorBase_VERSION=7.2.2 + +# Specify the version of sinqMotor we want to build against +sinqMotor_VERSION=mathis_s + +# Specify the version of turboPmac we want to build against +turboPmac_VERSION=mathis_s + +# These headers allow to depend on this library for derived drivers. +HEADERS += src/seleneLiftAxis.h + +# Source files to build +SOURCES += src/seleneLiftAxis.cpp + +# This file registers the motor-specific functions in the IOC shell. +DBDS += src/seleneLift.dbd + +USR_CFLAGS += -Wall -Wextra -Weffc++ -Wunused-result -Wextra -Werror # -Wpedantic // Does not work because EPICS macros trigger warnings diff --git a/README.md b/README.md index f30ddd1..6483063 100644 --- a/README.md +++ b/README.md @@ -1,93 +1,67 @@ -# seleneLift +# turboPmac + +## Overview + +This is a driver for the Turbo PMAC motion controller with the SINQ communication protocol. It is based on the sinqMotor shared library (https://git.psi.ch/sinq-epics-modules/sinqmotor). The header files contain detailed documentation for all public functions. The headers themselves are exported when building the library to allow other drivers to depend on this one. + +## User guide + +This driver is a standard sinqMotor-derived driver and does not need any specific configuration. For the general configuration, please see https://git.psi.ch/sinq-epics-modules/sinqmotor/-/blob/main/README.md. + +The folder "utils" contains utility scripts for working with pmac motor controllers. To read their manual, run the scripts without any arguments. +- writeRead.py: Allows sending commands to and receiving commands from a pmac controller over an ethernet connection. +- analyzeTcpDump.py: Parse the TCP communication between an IOC and a MCU and format it into a dictionary. "demo.py" shows how this data can be easily visualized for analysis. +## Developer guide -## Getting started +### Usage in IOC shell -To make it easy for you to get started with GitLab, here's a list of recommended next steps. +turboPmac exports the following IOC shell functions: +- `turboPmacController`: Create a new controller object. +- `turboPmacAxis`: Create a new axis object. -Already a pro? Just edit this README.md and make it your own. Want to make it easy? [Use the template at the bottom](#editing-this-readme)! - -## Add your files - -- [ ] [Create](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#create-a-file) or [upload](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#upload-a-file) files -- [ ] [Add files using the command line](https://docs.gitlab.com/ee/gitlab-basics/add-file.html#add-a-file-using-the-command-line) or push an existing Git repository with the following command: +The full mcu.cmd file looks like this: ``` -cd existing_repo -git remote add origin https://git.psi.ch/sinq-epics-modules/selenelift.git -git branch -M main -git push -uf origin main +# Define the name of the controller and the corresponding port +epicsEnvSet("NAME","mcu") +epicsEnvSet("ASYN_PORT","p$(NAME)") + +# Create the TCP/IP socket used to talk with the controller. The socket can be adressed from within the IOC shell via the port name +drvAsynIPPortConfigure("$(ASYN_PORT)","172.28.101.24:1025") + +# Create the controller object with the defined name and connect it to the socket via the port name. +# The other parameters are as follows: +# 8: Maximum number of axes +# 0.05: Busy poll period in seconds +# 1: Idle poll period in seconds +# 1: Socket communication timeout in seconds +turboPmacController("$(NAME)", "$(ASYN_PORT)", 8, 0.05, 1, 1); + +# Define some axes for the specified MCU at the given slot (1, 2 and 5). No slot may be used twice! +turboPmacAxis("$(NAME)",1); +turboPmacAxis("$(NAME)",2); +turboPmacAxis("$(NAME)",5); + +# Set the number of subsequent timeouts +setMaxSubsequentTimeouts("$(NAME)", 20); + +# Configure the timeout frequency watchdog: +setThresholdComTimeout("$(NAME)", 100, 1); + +# Parametrize the EPICS record database with the substitution file named after the MCU. +epicsEnvSet("SINQDBPATH","$(sinqMotor_DB)/sinqMotor.db") +dbLoadTemplate("$(TOP)/$(NAME).substitutions", "INSTR=$(INSTR)$(NAME):,CONTROLLER=$(NAME)") +epicsEnvSet("SINQDBPATH","$(turboPmac_DB)/turboPmac.db") +dbLoadTemplate("$(TOP)/$(NAME).substitutions", "INSTR=$(INSTR)$(NAME):,CONTROLLER=$(NAME)") +dbLoadRecords("$(sinqMotor_DB)/asynRecord.db","P=$(INSTR)$(NAME),PORT=$(ASYN_PORT)") ``` -## Integrate with your tools +### Versioning -- [ ] [Set up project integrations](https://git.psi.ch/sinq-epics-modules/selenelift/-/settings/integrations) +Please see the documentation for the module sinqMotor: https://git.psi.ch/sinq-epics-modules/sinqmotor/-/blob/main/README.md. -## Collaborate with your team +### How to build it -- [ ] [Invite team members and collaborators](https://docs.gitlab.com/ee/user/project/members/) -- [ ] [Create a new merge request](https://docs.gitlab.com/ee/user/project/merge_requests/creating_merge_requests.html) -- [ ] [Automatically close issues from merge requests](https://docs.gitlab.com/ee/user/project/issues/managing_issues.html#closing-issues-automatically) -- [ ] [Enable merge request approvals](https://docs.gitlab.com/ee/user/project/merge_requests/approvals/) -- [ ] [Set auto-merge](https://docs.gitlab.com/ee/user/project/merge_requests/merge_when_pipeline_succeeds.html) - -## Test and Deploy - -Use the built-in continuous integration in GitLab. - -- [ ] [Get started with GitLab CI/CD](https://docs.gitlab.com/ee/ci/quick_start/index.html) -- [ ] [Analyze your code for known vulnerabilities with Static Application Security Testing (SAST)](https://docs.gitlab.com/ee/user/application_security/sast/) -- [ ] [Deploy to Kubernetes, Amazon EC2, or Amazon ECS using Auto Deploy](https://docs.gitlab.com/ee/topics/autodevops/requirements.html) -- [ ] [Use pull-based deployments for improved Kubernetes management](https://docs.gitlab.com/ee/user/clusters/agent/) -- [ ] [Set up protected environments](https://docs.gitlab.com/ee/ci/environments/protected_environments.html) - -*** - -# Editing this README - -When you're ready to make this README your own, just edit this file and use the handy template below (or feel free to structure it however you want - this is just a starting point!). Thanks to [makeareadme.com](https://www.makeareadme.com/) for this template. - -## Suggestions for a good README - -Every project is different, so consider which of these sections apply to yours. The sections used in the template are suggestions for most open source projects. Also keep in mind that while a README can be too long and detailed, too long is better than too short. If you think your README is too long, consider utilizing another form of documentation rather than cutting out information. - -## Name -Choose a self-explaining name for your project. - -## Description -Let people know what your project can do specifically. Provide context and add a link to any reference visitors might be unfamiliar with. A list of Features or a Background subsection can also be added here. If there are alternatives to your project, this is a good place to list differentiating factors. - -## Badges -On some READMEs, you may see small images that convey metadata, such as whether or not all the tests are passing for the project. You can use Shields to add some to your README. Many services also have instructions for adding a badge. - -## Visuals -Depending on what you are making, it can be a good idea to include screenshots or even a video (you'll frequently see GIFs rather than actual videos). Tools like ttygif can help, but check out Asciinema for a more sophisticated method. - -## Installation -Within a particular ecosystem, there may be a common way of installing things, such as using Yarn, NuGet, or Homebrew. However, consider the possibility that whoever is reading your README is a novice and would like more guidance. Listing specific steps helps remove ambiguity and gets people to using your project as quickly as possible. If it only runs in a specific context like a particular programming language version or operating system or has dependencies that have to be installed manually, also add a Requirements subsection. - -## Usage -Use examples liberally, and show the expected output if you can. It's helpful to have inline the smallest example of usage that you can demonstrate, while providing links to more sophisticated examples if they are too long to reasonably include in the README. - -## Support -Tell people where they can go to for help. It can be any combination of an issue tracker, a chat room, an email address, etc. - -## Roadmap -If you have ideas for releases in the future, it is a good idea to list them in the README. - -## Contributing -State if you are open to contributions and what your requirements are for accepting them. - -For people who want to make changes to your project, it's helpful to have some documentation on how to get started. Perhaps there is a script that they should run or some environment variables that they need to set. Make these steps explicit. These instructions could also be useful to your future self. - -You can also document commands to lint the code or run tests. These steps help to ensure high code quality and reduce the likelihood that the changes inadvertently break something. Having instructions for running tests is especially helpful if it requires external setup, such as starting a Selenium server for testing in a browser. - -## Authors and acknowledgment -Show your appreciation to those who have contributed to the project. - -## License -For open source projects, say how it is licensed. - -## Project status -If you have run out of energy or time for your project, put a note at the top of the README saying that development has slowed down or stopped completely. Someone may choose to fork your project or volunteer to step in as a maintainer or owner, allowing your project to keep going. You can also make an explicit request for maintainers. +Please see the documentation for the module sinqMotor: https://git.psi.ch/sinq-epics-modules/sinqmotor/-/blob/main/README.md. diff --git a/images/Geometry.odg b/images/Geometry.odg new file mode 100644 index 0000000..90c2bfa Binary files /dev/null and b/images/Geometry.odg differ diff --git a/src/offsetAxis.cpp b/src/offsetAxis.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/offsetAxis.h b/src/offsetAxis.h new file mode 100644 index 0000000..cb4ca8c --- /dev/null +++ b/src/offsetAxis.h @@ -0,0 +1,16 @@ +#include "turboPmacAxis.h" + +class offsetAxis : public turboPmacAxis { + public: + /** + * @brief Implementation of the `doPoll` function from sinqAxis. The + * parameters are described in the documentation of `sinqAxis::doPoll`. + * + * @param moving + * @return asynStatus + */ + asynStatus doPoll(bool *moving); + + private: + seleneLiftAxis liftAxis_; +} \ No newline at end of file diff --git a/src/seleneAngleAxis.cpp b/src/seleneAngleAxis.cpp new file mode 100644 index 0000000..e8b0091 --- /dev/null +++ b/src/seleneAngleAxis.cpp @@ -0,0 +1,12 @@ +#include "seleneAngleAxis.h" +#include "seleneLiftAxis.h" + +seleneAngleAxis::seleneAngleAxis(turboPmacController *pController, int axisNo) { + // Initialize the associated lift axis as a nullptr. It is populated in the + // constructor of seleneLiftAxis. + liftAxis_ = nullptr; +} + +asynStatus seleneAngleAxis::stop(double acceleration) { + return liftAxis_->stop(acceleration); +} \ No newline at end of file diff --git a/src/seleneAngleAxis.h b/src/seleneAngleAxis.h new file mode 100644 index 0000000..fb914df --- /dev/null +++ b/src/seleneAngleAxis.h @@ -0,0 +1,117 @@ +#ifndef seleneAngleAxis_H +#define seleneAngleAxis_H +#include "turboPmacAxis.h" +#include "turboPmacController.h" + +// Forward declaration of the seleneLiftAxis class to resolve the cyclic +// dependency between the seleneLiftAxis and the seleneAngleAxis .h-file. +// See https://en.cppreference.com/w/cpp/language/class. +class seleneLiftAxis; + +/** + * @brief Virtual axis for setting the angle of a Selene guide + */ +class seleneAngleAxis : public turboPmacAxis { + public: + /** + * @brief Destroy the turboPmacAxis + * + */ + virtual ~seleneAngleAxis(); + + /** + * @brief Implementation of the `stop` function from asynMotorAxis + * + * @param acceleration Acceleration ACCEL from the motor record. This + * value is currently not used. + * @return asynStatus + */ + asynStatus stop(double acceleration); + + /** + * @brief This function does (almost) nothing, since the poll is done + * directly in seleneLiftAxis::doPoll(). It justs sets *moving. + * + * @param moving + * @return asynStatus + */ + asynStatus poll(bool *moving); + + /** + * @brief Set the target value for the guide angle and trigger a position + * collection cycle + * + * @param position + * @param relative + * @param min_velocity + * @param maxOffset_velocity + * @param acceleration + * @return asynStatus + */ + asynStatus doMove(double position, int relative, double min_velocity, + double maxOffset_velocity, double acceleration); + + /** + * @brief Readout of some values from the controller at IOC startup + * + * The following steps are performed: + * - Read out the motor status, motor position, velocity and acceleration + * from the MCU and store this information in the parameter library. + * - Set the enable PV according to the initial status of the axis. + * + * @return asynStatus + */ + asynStatus init(); + + /** + * @brief Reset the error(s) of both physical motors + * + * @param on + * @return asynStatus + */ + asynStatus reset(); + + /** + * @brief Call the stop() method of the associated liftAxis. + * + * @param on + * @return asynStatus + */ + asynStatus enable(bool on); + + /** + * @brief The encoder of both physical motors is absolute, hence the encoder + * of the virtual axis is also absolute + * + * @return asynStatus + */ + asynStatus readEncoderType(); + + /** + * @brief Trigger a rereading of the encoder position of both physical + * motors + * + * @return asynStatus + */ + asynStatus rereadEncoder(); + + protected: + turboPmacController *pC_; + seleneLiftAxis *liftAxis_; + + private: + /** + * @brief Construct a new seleneAngleAxis + * + * @param pController Pointer to the associated controller + * @param axisNo Index of the axis + */ + seleneAngleAxis(turboPmacController *pController, int axisNo, + seleneLiftAxis *liftAxis); + + // The friend class declaration here is necessary so the + // seleneLiftAxis cann call the constructor of seleneAngleAxis + friend class seleneLiftAxis; +}; + +#endif diff --git a/src/seleneLift.dbd b/src/seleneLift.dbd new file mode 100644 index 0000000..0031597 --- /dev/null +++ b/src/seleneLift.dbd @@ -0,0 +1,5 @@ +#--------------------------------------------- +# SINQ specific DB definitions +#--------------------------------------------- +registrar(turboPmacControllerRegister) +registrar(turboPmacAxisRegister) \ No newline at end of file diff --git a/src/seleneLiftAxis.cpp b/src/seleneLiftAxis.cpp new file mode 100644 index 0000000..f1786f3 --- /dev/null +++ b/src/seleneLiftAxis.cpp @@ -0,0 +1,445 @@ +#include "seleneLiftAxis.h" +#include "asynOctetSyncIO.h" +#include "epicsExport.h" +#include "iocsh.h" +#include "turboPmacController.h" +#include +#include +#include +#include +#include +#include +#include +#include + +/* +Contains all instances of turboPmacAxis which have been created and is used in +the initialization hook function. + */ +static std::vector axes; + +/** + * @brief Hook function to perform certain actions during the IOC initialization + * + * @param iState + */ +static void epicsInithookFunction(initHookState iState) { + if (iState == initHookAfterDatabaseRunning) { + // Iterate through all axes of each and call the initialization method + // on each one of them. + for (std::vector::iterator itA = axes.begin(); + itA != axes.end(); ++itA) { + turboPmacAxis *axis = *itA; + axis->init(); + } + } +} + +seleneLiftAxis::seleneLiftAxis(turboPmacController *pC, int liftAxisNo, + int angleAxisNo, int realAxis1No, + int realAxis2No) + : turboPmacAxis(pC, liftAxisNo, false), pC_(pC) { + + /* + The underlying physical / real axes need to be polled before the virtual + axes for lift and angle. To ensure this, the axis indices of the real axes + must be smaller than those of the virtual axes. If this is not the case, the + configuration is incorrect and therefore the IOC must be terminated. + */ + if (realAxis1No >= liftAxisNo || realAxis1No >= angleAxisNo || + realAxis2No >= liftAxisNo || realAxis2No >= angleAxisNo) { + asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR " + "(Axes indices of the real axes (set to %d and %d) must be " + "smaller than those of the virtual axes (set to %d and " + "%d))\n. Terminating IOC", + pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, axisNo, + realAxis1No, realAxis2No, liftAxisNo, angleAxisNo); + exit(-1); + } + + /* + The code below assumes that the first axis is the one with the lower x-value + on the fixed bearing and the second axis is the one on the loose bearing + with the higher x-value. This assumption is asserted here by the indices + */ + if (realAxis1No > realAxis2No) { + asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR " + "(Index of axis 1 (%d) must be smaller that that of axis 2 " + "(%d))\n. Terminating IOC", + pC->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, axisNo, + realAxis1No, realAxis2No); + exit(-1); + } + + // The axes are deallocated in the destructor of sinqController + angleAxis_ = new seleneAngleAxis(pC, angleAxisNo, this); + + // Initialize the parameters + xLift_ = 0.0; // Overwritten in the init function + receivedTarget_ = false; + + // Register the hook function during construction of the first axis + // object + if (axes.empty()) { + initHookRegister(&epicsInithookFunction); + } + + // Collect all axes into this list which will be used in the hook + // function + axes.push_back(this); +} + +seleneLiftAxis::~seleneLiftAxis() {} + +asynStatus seleneLiftAxis::init() { + + // Local variable declaration + asynStatus status = asynSuccess; + double motorRecResolution = 0.0; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + int nvals = 0; + + // The parameter library takes some time to be initialized. Therefore we + // wait until the status is not asynParamUndefined anymore. + time_t now = time(NULL); + time_t maxInitTime = 60; + while (1) { + status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(), + &motorRecResolution); + if (status == asynParamUndefined) { + if (now + maxInitTime < time(NULL)) { + asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line " + "%d\nInitializing the parameter library failed.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, + __LINE__); + return asynError; + } + } else if (status == asynSuccess) { + break; + } else if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorRecResolution_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + // Read out the horizontal distances of all other axes from axis 1 + xOffset_[0] = 0.0; // By definition + snprintf(command, sizeof(command), "Q652 Q653 Q654 Q655 Q656"); + status = pC_->writeRead(axisNo_, command, response, 5); + if (status != asynSuccess) { + return status; + } + nvals = sscanf(response, "%lf %lf %lf %lf %lf", &xOffset_[1], &xOffset_[2], + &xOffset_[3], &xOffset_[4], &xOffset_[5]); + + if (nvals != 5) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Center between axis 2 and 3 + xLift_ = 0.5 * (xOffset_[2] + xOffset_[3]); + + // Read out the vertical distances of all other axes from axis 1 + zOffset_[0] = 0.0; // By definition + zOffset_[1] = 0.0; + zOffset_[2] = 0.0; + zOffset_[3] = 0.0; + zOffset_[4] = 0.0; + zOffset_[5] = 0.0; + + return normalizeOffsets(); +} + +asynStatus seleneLiftAxis::normalizeOffsets() { + asynStatus status = asynSuccess; + char response[pC_->MAXBUF_]; + int nvals = 0; + + // Read out the absolute positions of all axes + const char *command = "Q0110 Q0210 Q0310 Q0410 Q0510 Q0610"; + status = pC_->writeRead(axisNo_, command, response, 6); + if (status != asynSuccess) { + return status; + } + + double z[6] = {0.0}; + nvals = sscanf(response, "%lf %lf %lf %lf %lf %lf", &z[0], &z[1], &z[2], + &z[3], &z[4], &z[5]); + if (nvals != 2) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Normalization + for (int i = 0; i < numAxes_; i++) { + z[i] = z[i] - zOffset_[i]; + } + + // Lift position is simply the mean of axis 2 and 3 + status = setDoubleParam(pC_->motorPosition(), 0.5 * (z[2] + z[3])); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorPosition_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // tan(angle) = (z3 - z2) / (x3 - x2) + double alpha = atan2(z[3] - z[2], xOffset_[3] - xOffset_[2]); + status = angleAxis_->setDoubleParam(pC_->motorPosition(), alpha); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorPosition_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Adjust the position values of all physical axes accordingly. The values + // of axes 2 and 3 should result in zero, if not, something went wrong. + double tanAlpha = tan(alpha); + for (int i = 0; i < numAxes_; i++) { + + // Calculate the new offset + double x = xOffset_[i] - xLift_; + double z = z[i] - x * tanAlpha; + + status = axes_[i].setDoubleParam(pC_->motorPosition(), z); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorPosition_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // TODO: Adjust limits + } + + // Update all axes + callParamCallbacks(); + angleAxis_->callParamCallbacks(); + for (int i = 0; i < numAxes_; i++) { + axes_[i].callParamCallbacks(); + } +} + +asynStatus seleneLiftAxis::doPoll(bool *moving) { + + // Return value for the poll + asynStatus poll_status = asynSuccess; + + // Status of read-write-operations of ASCII commands to the controller + asynStatus rw_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + char response[pC_->MAXBUF_], userMessage[pC_->MAXBUF_]; + + int isMoving = 0; + int nvals = 0; + int error = 0; + int axStatus = 0; + double posAx1 = 0.0; + double highLimitAx1 = 0.0; + double lowLimitAx1 = 0.0; + double posAx6 = 0.0; + double highLimitAx6 = 0.0; + double lowLimitAx6 = 0.0; + + // ========================================================================= + + // Read the positions and the limits of axis 1 and 6 as well as the status + // and the error of the virtual axis + const char *command = "status error Q0100 Q0113 Q0114 Q0600 Q0613 Q0614"; + rw_status = pC_->writeRead(axisNo_, command, response, 8); + if (rw_status != asynSuccess) { + return rw_status; + } + + nvals = sscanf(response, "%d %d %lf %lf %lf %lf %lf %lf", &axStatus, &error, + &posAx1, &highLimitAx1, &lowLimitAx1, &posAx6, &highLimitAx6, + &lowLimitAx6); + if (nvals != 8) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + /* + The following code converts from the vertical height readback values of axis + 1 and 6 to a virtual lift position and a corresponding angle. To do that, we + need first to correct for the different starting point of both axes (see + documentation of member variable yDistAx1ToAx6_) + */ + posAx6 = posAx6 - yDistAx1ToAx6_; + highLimitAx6 = highLimitAx6 - yDistAx1ToAx6_; + lowLimitAx6 = lowLimitAx6 - yDistAx1ToAx6_; + + /* + Convert the z-axis position values of axis 1 and 6 into an angle and a + lift, using the center position between axis 1 and 6 as the rotation point. + The angle is the arcustangens(opposite/adjacent), where the adjacent is + the distance between axis 1 and 6 and the opposite is the difference + between the height of axis 6 and axis 1. + */ + double liftPos = 0.5 * (posAx1 + posAx6); + double liftHighLimit = 0.5 * (highLimitAx1 + highLimitAx6); + double liftLowLimit = 0.5 * (lowLimitAx1 + lowLimitAx6); + double anglePos = atan2(posAx6 - posAx1, xOffset_[5]); + + /* + The angle limits are defined from the center position, hence the lift + position needs to be subtracted from the high and low limits of angle 6. See + the documentation in README.md for details. + */ + double angleHighLimit = atan2(highLimitAx6 - liftPos, xOffset_[5]); + double angleLowLimit = atan2(lowLimitAx6 - liftPos, xOffset_[5]); + + // Set the RBV values for both axes + + pl_status = setIntegerParam(pC_->motorStatusMoving(), isMoving); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + pl_status = angleAxis_->setIntegerParam(pC_->motorStatusMoving(), isMoving); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_", + angleAxis_->axisNo(), + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = setIntegerParam(pC_->motorStatusDone(), isMoving == 0); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_", axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + pl_status = + angleAxis_->setIntegerParam(pC_->motorStatusDone(), isMoving == 0); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_", + angleAxis_->axisNo(), + __PRETTY_FUNCTION__, __LINE__); + } + + // If any of the physical axes has a status problem, the two virtual axes + // have a status problem + + // Set the status of the two virtual axes + pl_status = setIntegerParam(pC_->motorStatusProblem(), + (realAxis1StatProb || realAxis2StatProb)); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + pl_status = angleAxis_->setIntegerParam( + pC_->motorStatusProblem(), (realAxis1StatProb || realAxis2StatProb)); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + angleAxis_->axisNo(), + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = setStringParam(pC_->motorMessageText(), userMessage); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + pl_status = + angleAxis_->setStringParam(pC_->motorMessageText(), userMessage); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + angleAxis_->axisNo(), + __PRETTY_FUNCTION__, __LINE__); + } + + // Calculate the position and the angle + double realAxis1Pos = 0; + double realAxis2Pos = 0; + + pl_status = pC_->getDoubleParam(realAxis1_->axisNo(), pC_->motorPosition(), + &realAxis1Pos); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", + realAxis1_->axisNo(), + __PRETTY_FUNCTION__, __LINE__); + } + pl_status = pC_->getDoubleParam(realAxis2_->axisNo(), pC_->motorPosition(), + &realAxis2Pos); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", + realAxis2_->axisNo(), + __PRETTY_FUNCTION__, __LINE__); + } + + // Update the parameter library for the seleneAngleAxis (the update for the + // lift axis itself is done in the body of the parent poll() method) + pl_status = angleAxis_->callParamCallbacks(); + if (pC_->getMsgPrintControl().shouldBePrinted( + pC_->portName, angleAxis_->axisNo(), __PRETTY_FUNCTION__, __LINE__, + (pl_status != asynSuccess), pC_->asynUserSelf())) { + asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line " + "%d:\ncallParamCallbacks failed with %s.%s\n", + pC_->portName, angleAxis_->axisNo(), __PRETTY_FUNCTION__, + __LINE__, pC_->stringifyAsynStatus(poll_status), + pC_->getMsgPrintControl().getSuffix()); + } +} + +asynStatus seleneLiftAxis::doMove(double position, int relative, + double min_velocity, double max_velocity, + double acceleration) { + + // Calculate the position of all 6 underlying physical motors and send a + // combined move command to the MCU + + double motorRecResolution = 0.0; + asynStatus pl_status = pC_->getDoubleParam( + axisNo_, pC_->motorRecResolution(), &motorRecResolution); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + + // Signal to the deferredMovementCollectorLoop that a movement should be + // started to the defined target position. + targetPosition_ = position * motorRecResolution; + + return asynSuccess; +} + +asynStatus seleneLiftAxis::stop(double acceleration) { + // Status of read-write-operations of ASCII commands to the controller + asynStatus rw_status = asynSuccess; + + // Status of parameter library operations + asynStatus pl_status = asynSuccess; + + char response[pC_->MAXBUF_]; + + // ========================================================================= + + // Stop all axes + const char *command = "P150=8"; + rw_status = pC_->writeRead(axisNo_, command, response, 0); + + if (rw_status != asynSuccess) { + asynPrint( + pC_->asynUserSelf(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nStopping the movement " + "failed.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + pl_status = setIntegerParam(pC_->motorStatusProblem(), true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + axisNo_, __PRETTY_FUNCTION__, + __LINE__); + } + } + + return rw_status; +} \ No newline at end of file diff --git a/src/seleneLiftAxis.h b/src/seleneLiftAxis.h new file mode 100644 index 0000000..a6158d2 --- /dev/null +++ b/src/seleneLiftAxis.h @@ -0,0 +1,147 @@ +#ifndef seleneLiftAxis_H +#define seleneLiftAxis_H +#include "seleneAngleAxis.h" +#include "turboPmacAxis.h" +#include "turboPmacController.h" + +/** + * @brief Virtual axis for setting the height of the center of a Selene guide + * + * This axis acts as the "master" for a partnering seleneAngleAxis (i.e. it + * controls the movement and polls the physical motors) + */ +class seleneLiftAxis : public turboPmacAxis { + public: + seleneLiftAxis(turboPmacController *pController, int liftAxisNo, + int angleAxisNo, int realAxis1No, int realAxis2No); + + /** + * @brief Destroy the seleneLiftAxis + * + */ + virtual ~seleneLiftAxis(); + + /** + * @brief Implementation of the `stop` function from asynMotorAxis + * + * @param acceleration Acceleration ACCEL from the motor record. This + * value is currently not used. + * @return asynStatus + */ + asynStatus stop(double acceleration); + + /** + * @brief Implementation of the `doPoll` function from sinqAxis. The + * parameters are described in the documentation of `sinqAxis::doPoll`. + * + * @param moving + * @return asynStatus + */ + asynStatus doPoll(bool *moving); + + /** + * @brief Set the target value for the detector angle and trigger a position + * collection cycle + * + * @param position + * @param relative + * @param min_velocity + * @param maxOffset_velocity + * @param acceleration + * @return asynStatus + */ + asynStatus doMove(double position, int relative, double min_velocity, + double maxOffset_velocity, double acceleration); + + /** + * @brief Start a movement to the target positions of this axis and the + * attached seleneAngleAxis. + * + * @return asynStatus + */ + asynStatus startCombinedMove(); + + /** + * @brief Readout of some values from the controller at IOC startup + * + * The following steps are performed: + * - Read out the motor status, motor position, velocity and acceleration + * from the MCU and store this information in the parameter library. + * - Set the enable PV according to the initial status of the axis. + * + * @return asynStatus + */ + asynStatus init(); + + /** + * @brief Reset the error(s) of both physical motors + * + * @param on + * @return asynStatus + */ + asynStatus reset(); + + /** + * @brief Enable / disable both physical motors + * + * @param on + * @return asynStatus + */ + asynStatus enable(bool on); + + /** + * @brief Override of the home function of asynMotorAxis, which does nothing + * + * @param on + * @return asynStatus + */ + asynStatus home(double min_velocity, double maxOffset_velocity, + double acceleration, int forwards) { + return asynSuccess; + } + + /** + * @brief The encoder of both physical motors is absolute, hence the encoder + * of the virtual axis is also absolute + * + * @return asynStatus + */ + asynStatus readEncoderType(); + + /** + * @brief Trigger a rereading of the encoder position of both physical + * motors + * + * @return asynStatus + */ + asynStatus rereadEncoder(); + + // TODO + asynStatus normalizeOffsets(); + + /** + * @brief Calculate the + * + * @param lift + * @param angle + * @return asynStatus + */ + double[6] calculateMotorPositions(double lift, double angle); + + protected: + static const int numAxes_ = 6; + + turboPmacController *pC_; + seleneAngleAxis *angleAxis_; + turboPmacAxis axes_[numAxes_]; + + // Horizontal distance from the first axis in mm (which is located at 0 mm) + double xOffset_[numAxes_]; + + // Vertical distance from the first axis in mm (which is located at 0 mm) + double zOffset_[numAxes_]; + + double xLift_ = 0.0; +}; + +#endif