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..d4c97d4 --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,51 @@ +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: + - sed -i 's/ARCH_FILTER=.*/ARCH_FILTER=linux%/' Makefile + - echo "LIBVERSION=${CI_COMMIT_TAG:-0.0.1}" >> Makefile + - make install + - cp -rT "/ioc/modules/pmacV3/$(ls -U /ioc/modules/pmacV3/ | head -1)" "./pmacV3-${CI_COMMIT_TAG:-$CI_COMMIT_SHORT_SHA}" + artifacts: + name: "pmacV3-${CI_COMMIT_TAG:-$CI_COMMIT_SHORT_SHA}" + paths: + - "pmacV3-${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..aa7c4c3 --- /dev/null +++ b/Makefile @@ -0,0 +1,27 @@ +# Use the PSI build system +include /ioc/tools/driver.makefile + +MODULE=pmacV3 +BUILDCLASSES=Linux +EPICS_VERSIONS=7.0.7 +ARCH_FILTER=RHEL% + +# Additional module dependencies +REQUIRED+=asynMotor +REQUIRED+=sinqMotor + +# Specify the version of sinqMotor we want to build against +sinqMotor_VERSION=0.1 + +# These headers allow to depend on this library for derived drivers. +HEADERS += src/pmacV3Axis.h +HEADERS += src/pmacV3Controller.h + +# Source files to build +SOURCES += src/pmacV3Axis.cpp +SOURCES += src/pmacV3Controller.cpp + +# This file registers the motor-specific functions in the IOC shell. +DBDS += src/pmacV3.dbd + +USR_CFLAGS += -Wall -Wextra # -Werror diff --git a/README.md b/README.md index a6b5117..2f886d1 100644 --- a/README.md +++ b/README.md @@ -1,93 +1,20 @@ # pmacV3 +## Overview +This is a driver for the PMac V3 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. -## 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. +pmacV3 exposes the following IOC shell functions (all in pmacV3Controller.cpp): +- `pmacV3CreateController`: Create a new controller object. +- `pmacV3CreateAxis`: Create a new axis object. +The function arguments are documented directly within the source code. -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)! +## Versioning -## Add your files +Please see the documentation for the module sinqMotor: https://git.psi.ch/sinq-epics-modules/sinqmotor/-/blob/main/README.md. -- [ ] [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: +## How to build it -``` -cd existing_repo -git remote add origin https://git.psi.ch/sinq-epics-modules/pmacv3.git -git branch -M main -git push -uf origin main -``` - -## Integrate with your tools - -- [ ] [Set up project integrations](https://git.psi.ch/sinq-epics-modules/pmacv3/-/settings/integrations) - -## Collaborate with your team - -- [ ] [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/src/pmacV3.dbd b/src/pmacV3.dbd new file mode 100644 index 0000000..d2209d4 --- /dev/null +++ b/src/pmacV3.dbd @@ -0,0 +1,4 @@ +#--------------------------------------------- +# SINQ specific DB definitions +#--------------------------------------------- +registrar(pmacV3Register) diff --git a/src/pmacV3Axis.cpp b/src/pmacV3Axis.cpp new file mode 100644 index 0000000..de3d9ea --- /dev/null +++ b/src/pmacV3Axis.cpp @@ -0,0 +1,1163 @@ +#include "pmacV3Axis.h" +#include "asynOctetSyncIO.h" +#include "pmacV3Controller.h" +#include +#include +#include +#include +#include +#include + +pmacV3Axis::pmacV3Axis(pmacV3Controller *pC, int axisNo, double offsetLimit) + : sinqAxis(pC, axisNo), pC_(pC) { + + 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. 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 => line %d:: FATAL ERROR: Axis index %d must be smaller " + "than the total number of axes %d. Call the support.", + __PRETTY_FUNCTION__, __LINE__, axisNo_, pC->numAxes_); + exit(-1); + } + + // Initialize all member variables + initial_poll_ = true; + waitForHandshake_ = false; + axisStatus_ = 0; + offsetLimits_ = offsetLimit; + + // 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 => line %d:\nFATAL ERROR (setting a parameter value failed " + "with %s)\n. Terminating IOC", + __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status)); + exit(-1); + } + + status = pC_->setDoubleParam(axisNo_, pC_->motorPosition_, 0.0); + if (status != asynSuccess) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (setting a parameter value failed " + "with %s)\n. Terminating IOC", + __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status)); + exit(-1); + } + + // Default values for the watchdog timeout mechanism + offsetMovTimeout_ = 30; // seconds + scaleMovTimeout_ = 2.0; +} + +pmacV3Axis::~pmacV3Axis(void) { + // Since the controller memory is managed somewhere else, we don't need to + // clean up the pointer pC here. +} + +/** +Read the configuration at the first poll + */ +asynStatus pmacV3Axis::atFirstPoll() { return readConfig(); } + +/* +Read the configuration from the motor control unit and the parameter library. +*/ +asynStatus pmacV3Axis::readConfig() { + + // Local variable declaration + asynStatus status = asynSuccess; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + int nvals = 0; + double motorRecResolution = 0.0; + double motorPosition = 0.0; + double motorVelBase = 0.0; + double motorAccel = 0.0; + int axStatus = 0; + + /* + The motor record resolution (index motorRecResolution_ in the parameter + library, MRES in the motor record) is NOT a conversion factor between user + units (e.g. mm) and motor units (e.g. encoder steps), but a scaling factor + defining the resolution of the position readback field RRBV. This is due to + an implementation detail inside EPICS described here: + https://epics.anl.gov/tech-talk/2018/msg00089.php + https://github.com/epics-modules/motor/issues/8 + + Basically, the position value in the parameter library is a double which is + then truncated to an integer in devMotorAsyn.c. Therefore, if we want a + precision of 1 millimeter, we need to set MRES to 1. If we want one of 1 + micrometer, we need to set MRES to 0.001. The readback value needs to be + multiplied with MRES to get the actual value. + + In the driver, we use user units. Therefore, when we interact with the + parameter library, we need to account for MRES. This means: + - When writing position or speed to the parameter library, we divide the + value by the motor record resolution. + - When reading position or speed from the parameter library, we multiply the + value with the motor record resolution. + + Index and motor record field are coupled as follows: + The parameter motorRecResolution_ is coupled to the field MRES of the motor + record in the following manner: + - In sinqMotor.db, the PV (motor_record_pv_name) MOTOR_REC_RESOLUTION + is defined as a copy of the field (motor_record_pv_name).MRES: + + record(ao,"$(P)$(M):Resolution") { + field(DESC, "$(M) resolution") + field(DOL, "$(P)$(M).MRES CP MS") + field(OMSL, "closed_loop") + field(DTYP, "asynFloat64") + field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_REC_RESOLUTION") + field(PREC, "$(PREC)") + } + + - The PV name MOTOR_REC_RESOLUTION is coupled in asynMotorController.h to + the constant motorRecResolutionString + - ... which in turn is assigned to motorRecResolution_ in + asynMotorController.cpp This way of making the field visible to the driver + is described here: https://epics.anl.gov/tech-talk/2020/msg00378.php This is + a one-way coupling, changes to the parameter library via setDoubleParam are + NOT transferred to (motor_record_pv_name).MRES or to + (motor_record_pv_name):Resolution. + + NOTE: This function must not be called in the constructor (e.g. in order to + save the read result to the member variable earlier), since the parameter + library is updated at a later stage! + */ + status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution); + if (status == asynParamUndefined) { + return asynParamUndefined; + } else if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorRecResolution_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Software limits and current position + snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10 Q%2.2d04 Q%2.2d06", + axisNo_, axisNo_, axisNo_, axisNo_); + status = pC_->writeRead(axisNo_, command, response, 4); + nvals = sscanf(response, "%d %lf %lf %lf", &axStatus, &motorPosition, + &motorVelBase, &motorAccel); + if (nvals != 4) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Transform from motor to parameter library coordinates + motorPosition = motorPosition / motorRecResolution; + motorVelBase = motorVelBase / motorRecResolution; + motorAccel = motorAccel / motorRecResolution; + + // Store these values in the parameter library + status = pC_->setDoubleParam(axisNo_, pC_->motorPosition_, motorPosition); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorPosition_", + __PRETTY_FUNCTION__, __LINE__); + } + + status = pC_->setDoubleParam(axisNo_, pC_->motorVelBase_, motorVelBase); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorVelBase_", + __PRETTY_FUNCTION__, __LINE__); + } + + status = pC_->setDoubleParam(axisNo_, pC_->motorAccel_, motorAccel); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "motorAccel_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Set the initial enable based on the motor status value + status = + setIntegerParam(pC_->enableMotor_, (axStatus != -3 && axStatus != -5)); + if (status != asynSuccess) { + return pC_->paramLibAccessFailed(status, "enableMotor_", + __PRETTY_FUNCTION__, __LINE__); + } + + // 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 => line %d:\ncallParamCallbacks failed with %s for axis %d.\n", + __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status), + axisNo_); + return status; + } + + return this->readEncoderType(); +} + +// Perform the actual poll +asynStatus pmacV3Axis::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 command[pC_->MAXBUF_], response[pC_->MAXBUF_], + userMessage[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; + double highLimit = 0.0; + double lowLimit = 0.0; + + // ========================================================================= + + // Are we currently waiting for a handshake? + if (waitForHandshake_) { + + snprintf(command, sizeof(command), "P%2.2d23 P%2.2d01", axisNo_, + axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, 2); + if (rw_status != asynSuccess) { + return rw_status; + } + + nvals = sscanf(response, "%d %d", &handshakePerformed, &error); + if (nvals != 2) { + return pC_->errMsgCouldNotParseResponse( + command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__); + } + + if (error != 0) { + // If an error occurred while waiting for the handshake, abort the + // waiting procedure and continue with the poll in order to handle + // the error. + waitForHandshake_ = false; + } else 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! + *moving = true; + return asynSuccess; + } + } + + // Motor resolution from parameter library + pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Read the previous motor position + pl_status = + pC_->getDoubleParam(axisNo_, pC_->motorPosition_, &previousPosition); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Transform from EPICS to motor coordinates (see comment in + // pmacV3Axis::readConfig()) + previousPosition = previousPosition * motorRecResolution; + + // Query the axis status + snprintf(command, sizeof(command), + "P%2.2d00 Q%2.2d10 P%2.2d01 Q%2.2d13 Q%2.2d14", axisNo_, axisNo_, + axisNo_, axisNo_, axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, 5); + if (rw_status != asynSuccess) { + return rw_status; + } + + nvals = sscanf(response, "%d %lf %d %lf %lf", &axStatus, ¤tPosition, + &error, &highLimit, &lowLimit); + if (nvals != 5) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + /* + 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 - offsetLimits_; + lowLimit = lowLimit + offsetLimits_; + + // Store the axis status + axisStatus_ = axStatus; + + // Intepret the status + switch (axStatus) { + case -6: + *moving = true; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nAxis %d is stopping\n", __PRETTY_FUNCTION__, + __LINE__, axisNo_); + break; + case -5: + *moving = false; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nAxis %d is deactivated\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + + pl_status = setStringParam(pC_->motorMessageText_, "Deactivated"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + // No further evaluation of the axis status is necessary + return asynSuccess; + case -4: + *moving = false; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nEmergency stop activated\n", + __PRETTY_FUNCTION__, __LINE__); + + pl_status = setStringParam(pC_->motorMessageText_, "Emergency stop"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + // No further evaluation of the axis status is necessary + return asynSuccess; + case -3: + *moving = false; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nAxis %d is disabled\n", __PRETTY_FUNCTION__, + __LINE__, axisNo_); + + pl_status = setStringParam(pC_->motorMessageText_, "Disabled"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + // No further evaluation of the axis status is necessary + return asynSuccess; + case 0: + *moving = false; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nAxis %d is idle\n", __PRETTY_FUNCTION__, + __LINE__, axisNo_); + break; + case 1: + *moving = true; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nMove order for axis %d acknowledged\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + break; + case 2: + *moving = true; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nMove order for axis %d is possible\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + break; + case 3: + *moving = true; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nAxis %d in Air Cushion Output status\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + break; + case 4: + *moving = true; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nAxis %d in Air Cushion Input status\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + break; + case 5: + *moving = true; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nAxis %d is moving\n", __PRETTY_FUNCTION__, + __LINE__, axisNo_); + + break; + default: + *moving = false; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nReached unreachable state P%2.2d00 = %d\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, axStatus); + pl_status = setStringParam( + pC_->motorMessageText_, + "Unreachable state has been reached. Please call the support."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + } + + if (*moving) { + // If the axis is moving, evaluate the movement direction + if ((currentPosition - previousPosition) > 0) { + direction = 1; + } else { + direction = 0; + } + } + + // Check and update the watchdog + if (checkMovTimeoutWatchdog(*moving) != asynSuccess) { + return asynError; + } + + // 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 => line %d:\nTarget position would exceed user limits in " + "axis %d\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + + pl_status = setStringParam(pC_->motorMessageText_, + "Target position would exceed software " + "limits. Please call the support."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + poll_status = asynError; + break; + case 5: + // Command not possible + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nAxis %d is still moving, but received " + "another move command. EPICS should prevent this, check if " + "*moving is set correctly.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + + pl_status = setStringParam(pC_->motorMessageText_, + "Axis received move command while it is " + "still moving. Please call the support."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + poll_status = asynError; + break; + case 8: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nAir cushion feedback stopped during " + "movement (P%2.2d01 = %d).\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, error); + + snprintf(userMessage, sizeof(userMessage), + "Air cushion feedback stopped during movement (P%2.2d01 = " + "%d). Please call the support.", + axisNo_, error); + + pl_status = setStringParam(pC_->motorMessageText_, userMessage); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + break; + case 9: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nNo air cushion feedback before movement " + "start (P%2.2d01 = %d).\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, error); + + snprintf(userMessage, sizeof(userMessage), + "No air cushion feedback before movement start (P%2.2d01 = " + "%d). Please call the support.", + axisNo_, error); + pl_status = setStringParam(pC_->motorMessageText_, userMessage); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + 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_ERROR, + "%s => line %d:\nAxis %d hit the controller limits.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + + snprintf(userMessage, sizeof(userMessage), + "Software limits or end switch hit (P%2.2d01 = %d). Try " + "homing the motor, moving in the opposite direction or check " + "the SPS for errors (if available). " + "Otherwise please call the support.", + axisNo_, error); + + pl_status = setStringParam(pC_->motorMessageText_, userMessage); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + poll_status = asynError; + break; + case 11: + // Following error + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nMaximum allowed following error exceeded " + "for axis %d.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + + snprintf(command, sizeof(command), + "Maximum allowed following error exceeded (P%2.2d01 = %d). " + "Check if movement range is blocked." + "Otherwise please call the support.", + axisNo_, error); + pl_status = setStringParam(pC_->motorMessageText_, command); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + poll_status = asynError; + break; + + case 12: + snprintf(command, sizeof(command), + "Security input is triggered (P%2.2d01 = %d). Check the SPS " + "for errors (if available). Otherwise please call " + "the support.", + axisNo_, error); + break; + + case 13: + // Driver hardware error triggered + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nDriver hardware error triggered for axis %d.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + snprintf(command, sizeof(command), + "Driver hardware error (P%2.2d01 = 13). " + "Please call the support.", + axisNo_); + pl_status = setStringParam(pC_->motorMessageText_, command); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + 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 => line %d:\nMove command exceeds hardware limits " + "(P%2.2d01 = %d).\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, error); + + snprintf(command, sizeof(command), + "Move command exceeds hardware limits (P%2.2d01 = %d). Please " + "call the support.", + axisNo_, error); + pl_status = setStringParam(pC_->motorMessageText_, command); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + poll_status = asynError; + break; + default: + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nAxis %d reached an unreachable state " + "(P%2.2d01 = %d).\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, axisNo_, error); + + pl_status = setStringParam( + pC_->motorMessageText_, + "Axis reached an unreachable state (P%2.2d01 = %d). Please " + "call the support."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + 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, "motorStatusProblem_", + __PRETTY_FUNCTION__, __LINE__); + } + } + + if (*moving == false) { + pl_status = setIntegerParam(pC_->motorMoveToHome_, 0); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_", + __PRETTY_FUNCTION__, __LINE__); + } + } + + pl_status = + setIntegerParam(pC_->motorEnabled_, (axStatus != -3 && axStatus != -5)); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorEnabled_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = setIntegerParam(pC_->motorStatusMoving_, *moving); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = setIntegerParam(pC_->motorStatusDone_, !(*moving)); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDone_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = setIntegerParam(pC_->motorStatusDirection_, direction); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusDirection_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = + pC_->setDoubleParam(axisNo_, pC_->motorHighLimitFromDriver_, highLimit); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorHighLimitFromDriver_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = + pC_->setDoubleParam(axisNo_, pC_->motorLowLimitFromDriver_, lowLimit); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorLowLimit_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Transform from motor to EPICS coordinates (see comment in + // pmacV3Axis::readConfig()) + currentPosition = currentPosition / motorRecResolution; + + pl_status = setDoubleParam(pC_->motorPosition_, currentPosition); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorPosition_", + __PRETTY_FUNCTION__, __LINE__); + } + + return poll_status; +} + +asynStatus pmacV3Axis::doMove(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; + + 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, "motorEnabled_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_, + &motorRecResolution); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", + __PRETTY_FUNCTION__, __LINE__); + } + + if (enabled == 0) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nAxis %d is disabled.\n", __PRETTY_FUNCTION__, + __LINE__, axisNo_); + return asynSuccess; + } + + // Convert from EPICS to user / motor units + motorCoordinatesPosition = position * motorRecResolution; + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nStart of axis %d to position %lf.\n", + __PRETTY_FUNCTION__, __LINE__, 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, 0); + if (rw_status != asynSuccess) { + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nStarting movement to target position %lf " + "failed for axis %d.\n", + __PRETTY_FUNCTION__, __LINE__, motorCoordinatesPosition, + axisNo_); + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_", + __PRETTY_FUNCTION__, __LINE__); + } + 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); + + // Waiting for a handshake is already part of the movement procedure => + // Start the watchdog + if (startMovTimeoutWatchdog() != asynSuccess) { + return asynError; + } + + return rw_status; +} + +asynStatus pmacV3Axis::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; + + bool moving = false; + char command[pC_->MAXBUF_], response[pC_->MAXBUF_]; + + // ========================================================================= + + pl_status = doPoll(&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, 0); + + if (rw_status != asynSuccess) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nStopping the movement failed for axis %d.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + + pl_status = setIntegerParam(pC_->motorStatusProblem_, true); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, + "motorStatusProblem_", + __PRETTY_FUNCTION__, __LINE__); + } + } + } + return rw_status; +} + +/* +Home the axis. On absolute encoder systems, this is a no-op +*/ +asynStatus pmacV3Axis::doHome(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_]; + + // ========================================================================= + + pl_status = pC_->getStringParam(axisNo_, pC_->encoderType_, + sizeof(response), response); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "encoderType_", + __PRETTY_FUNCTION__, __LINE__); + } + + // 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, 0); + if (rw_status != asynSuccess) { + return rw_status; + } + + pl_status = setIntegerParam(pC_->motorMoveToHome_, 1); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_", + __PRETTY_FUNCTION__, __LINE__); + } + + pl_status = setStringParam(pC_->motorMessageText_, "Homing"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + } else { + pl_status = setStringParam(pC_->motorMessageText_, + "Can't home a motor with absolute encoder"); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + } + + return rw_status; +} + +/* +Read the encoder type and update the parameter library accordingly +*/ +asynStatus pmacV3Axis::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_]; + int nvals = 0; + int encoder_id = 0; + + // ========================================================================= + + // Check if this is an absolute encoder + snprintf(command, sizeof(command), "I%2.2X04", axisNo_); + rw_status = pC_->writeRead(axisNo_, command, response, 1); + if (rw_status != asynSuccess) { + return rw_status; + } + + int reponse_length = strlen(response); + if (reponse_length < 3) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nUnexpected reponse '%s' from axis %d on " + "controller %s while reading the encoder type. Aborting....\n", + __PRETTY_FUNCTION__, __LINE__, 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 (nvals != 1) { + return pC_->errMsgCouldNotParseResponse(command, response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + snprintf(command, sizeof(command), "P46"); + rw_status = pC_->writeRead(axisNo_, command, response, 1); + 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, "encoderType_", + __PRETTY_FUNCTION__, __LINE__); + } + return asynSuccess; +} + +/* +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. +*/ +asynStatus pmacV3Axis::rereadEncoder() { + char command[pC_->MAXBUF_] = {0}; + char response[pC_->MAXBUF_] = {0}; + char encoderType[pC_->MAXBUF_] = {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 this is an absolute encoder + rw_status = readEncoderType(); + if (rw_status != asynSuccess) { + return rw_status; + } + pl_status = pC_->getStringParam(axisNo_, pC_->encoderType_, + sizeof(encoderType), encoderType); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "encoderType_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Abort if the axis is incremental + if (strcmp(encoderType, IncrementalEncoder) == 1) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "%s => line %d:\nTrying to reread absolute encoder of " + "axis %d on controller %s, but it is a relative encoder.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, pC_->portName); + pl_status = setStringParam(pC_->motorMessageText_, + "Cannot reread an incremental encoder."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + return asynError; + } + + // Check if the axis is disabled. If not, inform the user that this + // is necessary + int enabled = 0; + pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnabled_, &enabled); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorEnabled_", + __PRETTY_FUNCTION__, __LINE__); + } + + if (enabled == 1) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "%s => line %d:\nAxis %d on controller %s must be " + "disabled before rereading the encoder.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, pC_->portName); + pl_status = setStringParam( + pC_->motorMessageText_, + "Axis must be disabled before rereading the encoder."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + return asynError; + } else { + snprintf(command, sizeof(command), "M%2.2d=15", axisNo_); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\nRereading absolute encoder of axis %d on " + "controller %s via command %s.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, pC_->portName, + command); + pC_->writeRead(axisNo_, command, response, 0); + } + + // 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 + pl_status = pC_->setIntegerParam(pC_->rereadEncoderPosition_, 0); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "rereadEncoderPosition_", + __PRETTY_FUNCTION__, __LINE__); + } + return asynSuccess; +} + +asynStatus pmacV3Axis::enable(bool on) { + + 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; + + bool moving = false; + + // ========================================================================= + + // If the axis is currently moving, it cannot be disabled. Ignore the + // command and inform the user + doPoll(&moving); // Muss gehen wenn Status -6 + if (moving) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "%s => line %d:\nAxis %d is not idle and can therefore not " + "be enabled / disabled.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_); + + pl_status = + setStringParam(pC_->motorMessageText_, + "Axis cannot be disabled while it is moving."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Reset the value in the param lib. + pl_status = setIntegerParam(pC_->enableMotor_, 1); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "enableMotor_", + __PRETTY_FUNCTION__, __LINE__); + } + + return asynError; + } + + // Axis is already enabled / disabled and a new enable / disable command + // was sent => Do nothing + if ((axisStatus_ != -3) == on) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "%s => line %d:\nAxis %d on controller %s is already %s.\n", + __PRETTY_FUNCTION__, __LINE__, axisNo_, pC_->portName, + on ? "enabled" : "disabled"); + return asynSuccess; + } + + // Enable / disable the axis if it is not moving + snprintf(command, sizeof(command), "M%2.2d14=%d", axisNo_, on); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, + "%s => line %d:\n%s axis %d on controller %s\n", + __PRETTY_FUNCTION__, __LINE__, on ? "Enable" : "Disable", axisNo_, + pC_->portName); + if (on == 0) { + pl_status = setStringParam(pC_->motorMessageText_, "Disabling ..."); + } else { + pl_status = setStringParam(pC_->motorMessageText_, "Enabling ..."); + } + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + + rw_status = pC_->writeRead(axisNo_, command, response, 0); + 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, 1); + if (rw_status != asynSuccess) { + return rw_status; + } + nvals = sscanf(response, "%d", &axisStatus_); + if (nvals != 1) { + return pC_->errMsgCouldNotParseResponse( + command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__); + } + + if ((axisStatus_ != -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 => line %d:\nFailed to %s axis %d on controller %s within %d " + "seconds\n", + __PRETTY_FUNCTION__, __LINE__, 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_->motorMessageText_, "Enabling ..."); + if (pl_status != asynSuccess) { + return pC_->paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + return asynError; +} diff --git a/src/pmacV3Axis.h b/src/pmacV3Axis.h new file mode 100644 index 0000000..be78dec --- /dev/null +++ b/src/pmacV3Axis.h @@ -0,0 +1,131 @@ +#ifndef pmacV3AXIS_H +#define pmacV3AXIS_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. +class pmacV3Controller; + +class pmacV3Axis : public sinqAxis { + public: + /** + * @brief Construct a new pmacV3Axis + * + * @param pController Pointer to the associated controller + * @param axisNo Index of the axis + * @param offsetLimit The high and low limits of the axis are read + * out directly from the MCU. However, since the axis might slightly + "overshoot" when moving to a position next to the limits, the MCU might go + into the "limits hit" error state. To prevent this, this value allows adding + a small offset, which is subtracted from the high limit and added to the + low limit. + */ + pmacV3Axis(pmacV3Controller *pController, int axisNo, double offsetLimit); + + /** + * @brief Destroy the pmacV3Axis + * + */ + virtual ~pmacV3Axis(); + + /** + * @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 `doHome` function from sinqAxis. The + * parameters are described in the documentation of `sinqAxis::doHome`. + * + * @param minVelocity + * @param maxVelocity + * @param acceleration + * @param forwards + * @return asynStatus + */ + asynStatus doHome(double minVelocity, double maxVelocity, + double acceleration, int forwards); + + /** + * @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 Implementation of the `doMove` function from sinqAxis. The + * parameters are described in the documentation of `sinqAxis::doMove`. + * + * @param position + * @param relative + * @param min_velocity + * @param max_velocity + * @param acceleration + * @return asynStatus + */ + asynStatus doMove(double position, int relative, double min_velocity, + double max_velocity, double acceleration); + + /** + * @brief Implementation of the `atFirstPoll` function from sinqAxis. + * + * 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 accordint to the initial status of the axis. + * + * @return asynStatus + */ + asynStatus atFirstPoll(); + + /** + * @brief Enable / disable the axis. + * + * @param on + * @return asynStatus + */ + asynStatus enable(bool on); + + /** + * @brief Read the encoder type (incremental or absolute) for this axis from + * the MCU and store the information in the PV ENCODER_TYPE. + * + * @return asynStatus + */ + asynStatus readEncoderType(); + + /** + * @brief Trigger a rereading of the encoder position. + * + * @return asynStatus + */ + asynStatus rereadEncoder(); + + protected: + pmacV3Controller *pC_; + + asynStatus readConfig(); + bool initial_poll_; + bool waitForHandshake_; + time_t timeAtHandshake_; + + // The axis status is used when enabling / disabling the motor + int axisStatus_; + /* + Stores the constructor input offsetLimits + */ + double offsetLimits_; + + private: + friend class pmacV3Controller; +}; + +#endif diff --git a/src/pmacV3Controller.cpp b/src/pmacV3Controller.cpp new file mode 100644 index 0000000..69c1ef7 --- /dev/null +++ b/src/pmacV3Controller.cpp @@ -0,0 +1,678 @@ + +#include "pmacV3Controller.h" +#include "asynMotorController.h" +#include "asynOctetSyncIO.h" +#include "pmacV3Axis.h" +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Construct a new pmacV3Controller::pmacV3Controller object + * + * @param portName See documentation of sinqController + * @param ipPortConfigName See documentation of sinqController + * @param numAxes See documentation of sinqController + * @param movingPollPeriod See documentation of sinqController + * @param idlePollPeriod See documentation of sinqController + * @param comTimeout Time after which a communication timeout error + * is declared in writeRead (in seconds) + * @param extraParams See documentation of sinqController + */ +pmacV3Controller::pmacV3Controller(const char *portName, + const char *ipPortConfigName, int numAxes, + double movingPollPeriod, + double idlePollPeriod, double comTimeout) + : sinqController( + portName, ipPortConfigName, numAxes, movingPollPeriod, idlePollPeriod, + /* + The following parameter library entries are added in this driver: + - ENABLE_AXIS + - AXIS_ENABLED + - ENCODER_TYPE + - REREAD_ENCODER_POSITION + - REREAD_ENCODER_POSITION_RBV + - READ_CONFIG + - MOTOR_HIGH_LIMIT_FROM_DRIVER + - MOTOR_LOW_LIMIT_FROM_DRIVER + */ + 8) + +{ + + // Initialization of local variables + asynStatus status = asynSuccess; + + // Initialization of all member variables + lowLevelPortUser_ = nullptr; + comTimeout_ = comTimeout; + + // =========================================================================; + + /* + 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(ipPortConfigName, 0, &lowLevelPortUser_, NULL); + if (status != asynSuccess || lowLevelPortUser_ == nullptr) { + errlogPrintf( + "%s => line %d:\nFATAL ERROR (cannot connect to MCU controller).\n" + "Terminating IOC", + __PRETTY_FUNCTION__, __LINE__); + exit(-1); + } + + // ========================================================================= + // Create additional parameter library entries + + status = createParam("ENABLE_AXIS", asynParamInt32, &enableMotor_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (creating a parameter failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); + exit(-1); + } + + status = createParam("AXIS_ENABLED", asynParamInt32, &motorEnabled_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (creating a parameter failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); + exit(-1); + } + + status = createParam("ENCODER_TYPE", asynParamOctet, &encoderType_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (creating a parameter failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); + exit(-1); + } + + status = createParam("REREAD_ENCODER_POSITION", asynParamInt32, + &rereadEncoderPosition_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (creating a parameter failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); + exit(-1); + } + + status = createParam("REREAD_ENCODER_POSITION_RBV", asynParamInt32, + &rereadEncoderPositionRBV_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (creating a parameter failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); + exit(-1); + } + + status = createParam("READ_CONFIG", asynParamInt32, &readConfig_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (creating a parameter failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); + exit(-1); + } + + /* + We need to introduce 2 new parameters in order to write the limits from the + driver to the EPICS record. See the comment in pmacV3Controller.h next to + the declaration of motorHighLimitFromDriver_. + */ + status = createParam("MOTOR_HIGH_LIMIT_FROM_DRIVER", asynParamFloat64, + &motorHighLimitFromDriver_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (creating a parameter failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); + exit(-1); + } + + status = createParam("MOTOR_LOW_LIMIT_FROM_DRIVER", asynParamFloat64, + &motorLowLimitFromDriver_); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (creating a parameter failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, 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 => line %d:\nFATAL ERROR (setting input EOS failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); + pasynOctetSyncIO->disconnect(lowLevelPortUser_); + exit(-1); + } + + status = callParamCallbacks(); + if (status != asynSuccess) { + asynPrint( + this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFATAL ERROR (executing ParamLib callbacks failed " + "with %s).\nTerminating IOC", + __PRETTY_FUNCTION__, __LINE__, stringifyAsynStatus(status)); + pasynOctetSyncIO->disconnect(lowLevelPortUser_); + exit(-1); + } +} + +/* +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. +*/ +pmacV3Axis *pmacV3Controller::getAxis(asynUser *pasynUser) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser); + return pmacV3Controller::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 +*/ +pmacV3Axis *pmacV3Controller::getAxis(int axisNo) { + asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo); + return pmacV3Controller::castToAxis(asynAxis); +} + +pmacV3Axis *pmacV3Controller::castToAxis(asynMotorAxis *asynAxis) { + + // ========================================================================= + + // 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 + pmacV3Axis *axis = dynamic_cast(asynAxis); + if (axis == nullptr) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nAxis %d is not an instance of pmacV3Axis", + __PRETTY_FUNCTION__, __LINE__, axis->axisNo_); + } + return axis; +} + +asynStatus pmacV3Controller::writeRead(int axisNo, const char *command, + char *response, + int numExpectedResponses) { + + // Definition of local variables. + asynStatus status = asynSuccess; + asynStatus pl_status = asynSuccess; + char full_command[MAXBUF_] = {0}; + char user_message[MAXBUF_] = {0}; + int motorStatusProblem = 0; + int numReceivedResponses = 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; + + // ========================================================================= + + pmacV3Axis *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(this->pasynUserSelf, ASYN_TRACEIO_DRIVER, + "%s => line %d:\nSending command %s", __PRETTY_FUNCTION__, + __LINE__, full_command); + + // Perform the actual writeRead + status = pasynOctetSyncIO->writeRead( + lowLevelPortUser_, full_command, commandLength + offset, response, + MAXBUF_, comTimeout_, &nbytesOut, &nbytesIn, &eomReason); + + /* + Calculate the number of received responses by counting the number of + carriage returns "\r" in the response. + */ + for (size_t i = 0; i < strlen(response); i++) { + if (response[i] == '\r') { + numReceivedResponses++; + } + } + + /* + Check if we got the expected amount of responses. If we didn't, flush the + PMAC and try again. If that fails as well, return an error. + */ + if (numExpectedResponses != numReceivedResponses) { + // 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, + comTimeout_, &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_, comTimeout_, &nbytesOut, &nbytesIn, + &eomReason); + + // If the command returned a bad answer for the second time, give up + // and propagate the problem + numReceivedResponses = 0; + for (size_t i = 0; i < strlen(response); i++) { + if (response[i] == '\r') { + numReceivedResponses++; + } + } + + // Second check: If this fails, give up and propagate the error. + if (numExpectedResponses != numReceivedResponses) { + asynPrint( + this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nUnexpected response %s for command %s\n", + __PRETTY_FUNCTION__, __LINE__, response, command); + snprintf(user_message, sizeof(user_message), + "Received unexpected response %s for command %s. " + "Please call the support", + response, command); + pl_status = setStringParam(motorMessageText_, user_message); + if (pl_status != asynSuccess) { + return paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + status = asynError; + } + } else { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s => line %d:\nFlushing the MCU failed with %s\n", + __PRETTY_FUNCTION__, __LINE__, + stringifyAsynStatus(status)); + } + } + + // Create custom error messages for different failure modes + if (strlen(user_message) == 0) { + 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, "motorStatusProblem_", + __PRETTY_FUNCTION__, __LINE__); + } + + if (motorStatusProblem == 0) { + pl_status = + axis->setStringParam(this->motorMessageText_, user_message); + if (pl_status != asynSuccess) { + return paramLibAccessFailed(pl_status, "motorMessageText_", + __PRETTY_FUNCTION__, __LINE__); + } + } + } + + // Log the overall status (communication successfull or not) + if (status == asynSuccess) { + asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, + "%s => line %d:\nDevice response: %s\n", __PRETTY_FUNCTION__, + __LINE__, response); + pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 0); + } else { + if (status == asynSuccess) { + asynPrint( + lowLevelPortUser_, ASYN_TRACE_ERROR, + "%s => line %d:\nCommunication failed for command %s (%s)\n", + __PRETTY_FUNCTION__, __LINE__, full_command, + stringifyAsynStatus(status)); + pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 1); + } + + if (pl_status != asynSuccess) { + return paramLibAccessFailed(pl_status, "motorStatusCommsError_", + __PRETTY_FUNCTION__, __LINE__); + } + } + return asynSuccess; +} + +asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { + asynPrint(pasynUserSelf, ASYN_TRACE_ERROR, "enabling %d", value); + + pmacV3Axis *axis = getAxis(pasynUser); + if (axis == nullptr) { + // We already did the error logging directly in getAxis + return asynError; + } + + // Handle custom PVs + if (pasynUser->reason == enableMotor_) { + return axis->enable(value != 0); + } else if (pasynUser->reason == rereadEncoderPosition_) { + return axis->rereadEncoder(); + } else { + return asynMotorController::writeInt32(pasynUser, value); + } +} + +/* +Overloaded from asynMotorController because the special cases "motor +enabling" and "rereading the encoder" must be covered. +*/ +asynStatus pmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) { + + int function = pasynUser->reason; + asynStatus status = asynError; + + // ===================================================================== + + pmacV3Axis *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, "rereadEncoderPosition_", + __PRETTY_FUNCTION__, __LINE__); + } + status = + setIntegerParam(axis->axisNo_, rereadEncoderPositionRBV_, *value); + if (status != asynSuccess) { + return paramLibAccessFailed(status, "rereadEncoderPositionRBV_", + __PRETTY_FUNCTION__, __LINE__); + } + + // Update the PVs from the parameter library + return callParamCallbacks(); + } else if (function == motorEnabled_) { + + char command[MAXBUF_], response[MAXBUF_]; + int axStatus = 0; + + // Query the motor status from the affected axis + snprintf(command, sizeof(command), "P%2.2d00", axis->axisNo_); + status = writeRead(axis->axisNo_, command, response, 1); + if (status != asynSuccess) { + return status; + } + + int nvals = sscanf(response, "%d", &axStatus); + if (nvals != 1) { + return errMsgCouldNotParseResponse(command, response, axis->axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + // Update the parameter library + return setIntegerParam(motorEnabled_, + (axStatus != -3 && axStatus != -5)); + } else { + return asynMotorController::readInt32(pasynUser, value); + } +} + +/*************************************************************************************/ +/** The following functions are C-wrappers, and can be called directly from + * iocsh */ + +extern "C" { + +/* +C wrapper for the controller constructor. Please refer to the pmacV3Controller +constructor documentation. +*/ +asynStatus pmacV3CreateController(const char *portName, + const char *lowLevelPortName, int numAxes, + double movingPollPeriod, + double idlePollPeriod, double comTimeout) { + /* + We create a new instance of the controller, using the "new" keyword to + allocate it on the heap while avoiding RAII. + https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorController.cpp + https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/asynPortDriver.cpp + + The created object is registered in EPICS in its constructor and can safely + be "leaked" here. + */ +#pragma GCC diagnostic ignored "-Wunused-but-set-variable" +#pragma GCC diagnostic ignored "-Wunused-variable" + pmacV3Controller *pController = + new pmacV3Controller(portName, lowLevelPortName, numAxes, + movingPollPeriod, idlePollPeriod, comTimeout); + + return asynSuccess; +} + +/* +C wrapper for the axis constructor. Please refer to the pmacV3Axis constructor +documentation. The controller is read from the portName. +*/ +asynStatus pmacV3CreateAxis(const char *portName, int axis, + double offsetLimits) { + pmacV3Axis *pAxis; + + /* + 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(portName); + 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 + */ + errlogPrintf("%s => line %d:\nPort %s not found.", __PRETTY_FUNCTION__, + __LINE__, portName); + return asynError; + } + // Unsafe cast of the pointer to an asynPortDriver + asynPortDriver *apd = (asynPortDriver *)(ptr); + + // Safe downcast + pmacV3Controller *pC = dynamic_cast(apd); + if (pC == nullptr) { + errlogPrintf( + "%s => line %d:\ncontroller on port %s is not a pmacV3Controller.", + __PRETTY_FUNCTION__, __LINE__, portName); + return asynError; + } + + // Prevent manipulation of the controller from other threads while we + // create the new axis. + pC->lock(); + + /* + We create a new instance of the axis, using the "new" keyword to + allocate it on the heap while avoiding RAII. + https://github.com/epics-modules/motor/blob/master/motorApp/MotorSrc/asynMotorController.cpp + https://github.com/epics-modules/asyn/blob/master/asyn/asynPortDriver/asynPortDriver.cpp + + The created object is registered in EPICS in its constructor and can safely + be "leaked" here. + */ +#pragma GCC diagnostic ignored "-Wunused-but-set-variable" +#pragma GCC diagnostic ignored "-Wunused-variable" + pAxis = new pmacV3Axis(pC, axis, offsetLimits); + + // 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). +*/ + +#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 CreateControllerArg5 = {"Communication timeout (s)", + iocshArgDouble}; +static const iocshArg *const CreateControllerArgs[] = { + &CreateControllerArg0, &CreateControllerArg1, &CreateControllerArg2, + &CreateControllerArg3, &CreateControllerArg4, &CreateControllerArg5}; +static const iocshFuncDef configPmacV3CreateController = { + "revisedPmacV3CreateController", 6, CreateControllerArgs}; +static void configPmacV3CreateControllerCallFunc(const iocshArgBuf *args) { + pmacV3CreateController(args[0].sval, args[1].sval, args[2].ival, + args[3].dval, args[4].dval, args[5].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 CreateAxisArg2 = { + "Offset for the MCU limits in mm or degree", iocshArgDouble}; +static const iocshArg *const CreateAxisArgs[] = { + &CreateAxisArg0, &CreateAxisArg1, &CreateAxisArg2}; +static const iocshFuncDef configPmacV3CreateAxis = {"revisedPmacV3CreateAxis", + 3, CreateAxisArgs}; +static void configPmacV3CreateAxisCallFunc(const iocshArgBuf *args) { + pmacV3CreateAxis(args[0].sval, args[1].ival, args[2].dval); +} + +// This function is made known to EPICS in pmacV3.dbd and is called by EPICS +// in order to register both functions in the IOC shell +static void pmacV3Register(void) { + iocshRegister(&configPmacV3CreateController, + configPmacV3CreateControllerCallFunc); + iocshRegister(&configPmacV3CreateAxis, configPmacV3CreateAxisCallFunc); +} +epicsExportRegistrar(pmacV3Register); + +#endif + +} // extern "C" diff --git a/src/pmacV3Controller.h b/src/pmacV3Controller.h new file mode 100644 index 0000000..8a02b26 --- /dev/null +++ b/src/pmacV3Controller.h @@ -0,0 +1,140 @@ +/******************************************** + * pmacV3Controller.h + * + * PMAC V3 controller driver based on the asynMotorController class + * + * Stefan Mathis, September 2024 + ********************************************/ + +#ifndef pmacV3Controller_H +#define pmacV3Controller_H +#include "pmacV3Axis.h" +#include "sinqAxis.h" +#include "sinqController.h" + +#define IncrementalEncoder "Incremental encoder" +#define AbsoluteEncoder "Absolute encoder" + +class pmacV3Controller : public sinqController { + + public: + /** + * @brief Construct a new pmacV3Controller object + * + * @param portName See sinqController constructor + * @param ipPortConfigName See sinqController constructor + * @param numAxes See sinqController constructor + * @param movingPollPeriod See sinqController constructor + * @param idlePollPeriod See sinqController constructor + * @param comTimeout 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. + */ + pmacV3Controller(const char *portName, const char *ipPortConfigName, + int numAxes, double movingPollPeriod, + double idlePollPeriod, double comTimeout); + + /** + * @brief Get the axis object + * + * @param pasynUser Specify the axis via the asynUser + * @return pmacV3Axis* If no axis could be found, this is a nullptr + */ + pmacV3Axis *getAxis(asynUser *pasynUser); + + /** + * @brief Get the axis object + * + * @param axisNo Specify the axis via its index + * @return pmacV3Axis* If no axis could be found, this is a nullptr + */ + pmacV3Axis *getAxis(int axisNo); + + /** + * @brief Overloaded function of asynMotorController + * + * The function is overloaded to allow enabling / disabling the motor and + * rereading the encoder. + * + * @param pasynUser Specify the axis via the asynUser + * @param value New value + * @return asynStatus + */ + asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + + /** + * @brief Overloaded function of asynMotorController + * + * The function is overloaded to get readback values for the enabling / + * disabling status and the encoder. + * + * @param pasynUser Specify the axis via the asynUser + * @param value Read-out value + * @return asynStatus + */ + asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value); + + protected: + asynUser *lowLevelPortUser_; + + /** + * @brief Send a command to the hardware and receive a response + * + * @param axisNo Axis to which the command should be send + * @param command Command for the hardware + * @param response Buffer for the response. This buffer is + * expected to have the size MAXBUF_. + * @param numExpectedResponses The PMAC MCU can send multiple responses at + * once. The number of responses is determined by the number of + * "subcommands" within command. Therefore it is known in advance how many + * "subresponses" are expected. This can be used to check the integrity of + * the received response, since the subresponses are separated by carriage + * returns (/r). The number of carriage returns is compared to + * numExpectedResponses to determine if the communication was successfull. + * @return asynStatus + */ + asynStatus writeRead(int axisNo, const char *command, char *response, + int numExpectedResponses); + + /** + * @brief Save cast of the given asynAxis pointer to a pmacV3Axis pointer. + * If the cast fails, this function returns a nullptr. + * + * @param asynAxis + * @return pmacV3Axis* + */ + pmacV3Axis *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; + + /* + Stores the constructor input comTimeout + */ + double comTimeout_; + + // Indices of additional PVs + int enableMotor_; + int motorEnabled_; + int rereadEncoderPosition_; + int rereadEncoderPositionRBV_; + int readConfig_; + int encoderType_; + + /* + These parameters are here to write the high and low limits from the MCU to + the EPICS motor record. Using motorHighLimit_ / motorLowLimit_ does not + work: https://epics.anl.gov/tech-talk/2023/msg00576.php. + Therefore, some additional records are introduced which read from these + parameters and write into the motor record. See the sinq_asyn_motor.db file. + */ + int motorHighLimitFromDriver_; + int motorLowLimitFromDriver_; + + friend class pmacV3Axis; +}; + +#endif /* pmacV3Controller_H */