Initial commit for masterMacs
This commit is contained in:
27
Makefile
Normal file
27
Makefile
Normal file
@ -0,0 +1,27 @@
|
||||
# Use the PSI build system
|
||||
include /ioc/tools/driver.makefile
|
||||
|
||||
MODULE=masterMacs
|
||||
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=mathis_s
|
||||
|
||||
# These headers allow to depend on this library for derived drivers.
|
||||
HEADERS += src/masterMacsAxis.h
|
||||
HEADERS += src/masterMacsController.h
|
||||
|
||||
# Source files to build
|
||||
SOURCES += src/masterMacsAxis.cpp
|
||||
SOURCES += src/masterMacsController.cpp
|
||||
|
||||
# This file registers the motor-specific functions in the IOC shell.
|
||||
DBDS += src/masterMacs.dbd
|
||||
|
||||
USR_CFLAGS += -Wall -Wextra -Weffc++ -Wunused-result # -Werror
|
||||
45
README.md
Normal file
45
README.md
Normal file
@ -0,0 +1,45 @@
|
||||
# masterMacs
|
||||
|
||||
## Overview
|
||||
|
||||
This is a driver for the masterMacs 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 masterMacs motor controllers:
|
||||
- decodeStatus.py: Take the return message of a R10 (read status) command and print it in human-readable form.
|
||||
|
||||
## Developer guide
|
||||
|
||||
### Usage in IOC shell
|
||||
|
||||
masterMacs exposes the following IOC shell functions (all in masterMacsController.cpp):
|
||||
- `masterMacsController`: Create a new controller object.
|
||||
- `masterMacsAxis`: Create a new axis object.
|
||||
These functions are parametrized as follows:
|
||||
```
|
||||
masterMacsController(
|
||||
"$(NAME)", # Name of the MCU, e.g. mcu1. This parameter should be provided by an environment variable.
|
||||
"$(ASYN_PORT)", # IP-Port of the MCU. This parameter should be provided by an environment variable.
|
||||
8, # Maximum number of axes
|
||||
0.05, # Busy poll period in seconds
|
||||
1, # Idle poll period in seconds
|
||||
0.05 # Communication timeout in seconds
|
||||
);
|
||||
```
|
||||
```
|
||||
masterMacsAxis(
|
||||
"$(NAME)", # Name of the associated MCU, e.g. mcu1. This parameter should be provided by an environment variable.
|
||||
1 # Index of the axis.
|
||||
);
|
||||
```
|
||||
|
||||
### Versioning
|
||||
|
||||
Please see the documentation for the module sinqMotor: https://git.psi.ch/sinq-epics-modules/sinqmotor/-/blob/main/README.md.
|
||||
|
||||
### How to build it
|
||||
|
||||
Please see the documentation for the module sinqMotor: https://git.psi.ch/sinq-epics-modules/sinqmotor/-/blob/main/README.md.
|
||||
4
src/masterMacs.dbd
Normal file
4
src/masterMacs.dbd
Normal file
@ -0,0 +1,4 @@
|
||||
#---------------------------------------------
|
||||
# SINQ specific DB definitions
|
||||
#---------------------------------------------
|
||||
registrar(masterMacsRegister)
|
||||
794
src/masterMacsAxis.cpp
Normal file
794
src/masterMacsAxis.cpp
Normal file
@ -0,0 +1,794 @@
|
||||
#include "masterMacsAxis.h"
|
||||
#include "asynOctetSyncIO.h"
|
||||
#include "masterMacsController.h"
|
||||
#include <bitset>
|
||||
#include <cmath>
|
||||
#include <errlog.h>
|
||||
#include <limits>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo)
|
||||
: 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;
|
||||
axisStatus_ = std::bitset<16>(0);
|
||||
axisError_ = std::bitset<16>(0);
|
||||
|
||||
// Default values for the watchdog timeout mechanism
|
||||
offsetMovTimeout_ = 30; // seconds
|
||||
scaleMovTimeout_ = 2.0;
|
||||
|
||||
// masterMacs motors can always be disabled
|
||||
status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable_, 1);
|
||||
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);
|
||||
}
|
||||
|
||||
// Assume that the motor is initially not moving
|
||||
status = pC_->setIntegerParam(axisNo_, pC_->motorStatusMoving_, false);
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
masterMacsAxis::~masterMacsAxis(void) {
|
||||
// Since the controller memory is managed somewhere else, we don't need to
|
||||
// clean up the pointer pC here.
|
||||
}
|
||||
|
||||
/**
|
||||
Read out the following values:
|
||||
- current position
|
||||
- current velocity
|
||||
- maximum velocity
|
||||
- acceleration
|
||||
- Software limits
|
||||
*/
|
||||
asynStatus masterMacsAxis::atFirstPoll() {
|
||||
|
||||
// Local variable declaration
|
||||
asynStatus pl_status = asynSuccess;
|
||||
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
int nvals = 0;
|
||||
double motorRecResolution = 0.0;
|
||||
double motorPosition = 0.0;
|
||||
double motorVelocity = 0.0;
|
||||
double motorVmax = 0.0;
|
||||
double motorAccel = 0.0;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
|
||||
&motorRecResolution);
|
||||
if (pl_status == asynParamUndefined) {
|
||||
return asynParamUndefined;
|
||||
} else if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
// Read out the current position
|
||||
snprintf(command, sizeof(command), "%dR12", axisNo_);
|
||||
pl_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pl_status;
|
||||
}
|
||||
nvals = sscanf(response, "%lf", &motorPosition);
|
||||
if (nvals != 1) {
|
||||
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
// Read out the current velocity
|
||||
snprintf(command, sizeof(command), "%dR05", axisNo_);
|
||||
pl_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pl_status;
|
||||
}
|
||||
nvals = sscanf(response, "%lf", &motorVelocity);
|
||||
if (nvals != 1) {
|
||||
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
// Read out the maximum velocity
|
||||
// snprintf(command, sizeof(command), "%dR26", axisNo_);
|
||||
// pl_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
// if (pl_status != asynSuccess) {
|
||||
// return pl_status;
|
||||
// }
|
||||
// nvals = sscanf(response, "%lf", &motorVmax);
|
||||
// if (nvals != 1) {
|
||||
// return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
|
||||
// __PRETTY_FUNCTION__,
|
||||
// __LINE__);
|
||||
// }
|
||||
// TODO: Temporary workaround until R26 is implemented
|
||||
motorVmax = motorVelocity;
|
||||
|
||||
// Read out the acceleration
|
||||
snprintf(command, sizeof(command), "%dR06", axisNo_);
|
||||
pl_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pl_status;
|
||||
}
|
||||
nvals = sscanf(response, "%lf", &motorAccel);
|
||||
if (nvals != 1) {
|
||||
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
// Transform from motor to parameter library coordinates
|
||||
motorPosition = motorPosition / motorRecResolution;
|
||||
|
||||
// Store these values in the parameter library
|
||||
pl_status =
|
||||
pC_->setDoubleParam(axisNo_, pC_->motorPosition_, motorPosition);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorPosition_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
// Write to the motor record fields
|
||||
pl_status = setVeloFields(motorVelocity, 0.0, motorVmax);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pl_status;
|
||||
}
|
||||
pl_status = setAcclField(motorAccel);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pl_status;
|
||||
}
|
||||
|
||||
pl_status = readEncoderType();
|
||||
if (pl_status != asynSuccess) {
|
||||
return pl_status;
|
||||
}
|
||||
|
||||
// Update the parameter library immediately
|
||||
pl_status = callParamCallbacks();
|
||||
if (pl_status != asynSuccess) {
|
||||
// If we can't communicate with the parameter library, it doesn't
|
||||
// make sense to try and upstream this to the user -> Just log the
|
||||
// error
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\ncallParamCallbacks failed with %s for "
|
||||
"axis %d.\n",
|
||||
__PRETTY_FUNCTION__, __LINE__,
|
||||
pC_->stringifyAsynStatus(pl_status), axisNo_);
|
||||
return pl_status;
|
||||
}
|
||||
return pl_status;
|
||||
}
|
||||
|
||||
// Perform the actual poll
|
||||
asynStatus masterMacsAxis::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_];
|
||||
int nvals = 0;
|
||||
|
||||
int direction = 0;
|
||||
bool hasError = false;
|
||||
double currentPosition = 0.0;
|
||||
double previousPosition = 0.0;
|
||||
double motorRecResolution = 0.0;
|
||||
double highLimit = 0.0;
|
||||
double lowLimit = 0.0;
|
||||
double limitsOffset = 0.0;
|
||||
int wasMoving = 0;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
// 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
|
||||
// masterMacsAxis::readConfig())
|
||||
previousPosition = previousPosition * motorRecResolution;
|
||||
|
||||
// Update the axis status
|
||||
rw_status = readAxisStatus();
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
// Update the movement status
|
||||
*moving = isMoving();
|
||||
|
||||
// Read the current position
|
||||
snprintf(command, sizeof(command), "%dR12", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
nvals = sscanf(response, "%lf", ¤tPosition);
|
||||
if (nvals != 1) {
|
||||
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
// Read the low limit
|
||||
snprintf(command, sizeof(command), "%dR34", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
nvals = sscanf(response, "%lf", &lowLimit);
|
||||
if (nvals != 1) {
|
||||
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
// Read the high limit
|
||||
snprintf(command, sizeof(command), "%dR33", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
nvals = sscanf(response, "%lf", &highLimit);
|
||||
if (nvals != 1) {
|
||||
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
// Did we just finish a movement? If yes, read out the error.
|
||||
pl_status =
|
||||
pC_->getIntegerParam(axisNo_, pC_->motorStatusMoving_, &wasMoving);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorStatusMoving_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
if (!isMoving() && wasMoving == 1) {
|
||||
rw_status = readAxisError();
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
// TODO> Error handling
|
||||
}
|
||||
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
pl_status =
|
||||
pC_->getDoubleParam(axisNo_, pC_->motorLimitsOffset_, &limitsOffset);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorLimitsOffset_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
highLimit = highLimit - limitsOffset;
|
||||
lowLimit = lowLimit + limitsOffset;
|
||||
|
||||
//
|
||||
|
||||
// Update the enablement PV
|
||||
pl_status = setIntegerParam(pC_->motorEnableRBV_, enabled());
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorEnableRBV_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
if (*moving) {
|
||||
// If the axis is moving, evaluate the movement direction
|
||||
if ((currentPosition - previousPosition) > 0) {
|
||||
direction = 1;
|
||||
} else {
|
||||
direction = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Update the parameter library
|
||||
if (hasError) {
|
||||
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
}
|
||||
|
||||
if (!*moving) {
|
||||
pl_status = setIntegerParam(pC_->motorMoveToHome_, 0);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorMoveToHome_",
|
||||
__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
|
||||
// masterMacsAxis::atFirstPoll())
|
||||
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 masterMacsAxis::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;
|
||||
double motorRecResolution = 0.0;
|
||||
double motorVelocity = 0.0;
|
||||
int enabled = 0;
|
||||
int motorCanSetSpeed = 0;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableRBV_, &enabled);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_",
|
||||
__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;
|
||||
motorVelocity = maxVelocity * motorRecResolution;
|
||||
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s => line %d:\nStart of axis %d to position %lf.\n",
|
||||
__PRETTY_FUNCTION__, __LINE__, axisNo_, position);
|
||||
|
||||
// Check if the speed is allowed to be changed
|
||||
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorCanSetSpeed_,
|
||||
&motorCanSetSpeed);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorCanSetSpeed_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
// Set the new motor speed, if the user is allowed to do so.
|
||||
if (motorCanSetSpeed != 0) {
|
||||
snprintf(command, sizeof(command), "%dS05=%lf", axisNo_, motorVelocity);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, false);
|
||||
if (rw_status != asynSuccess) {
|
||||
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status,
|
||||
"motorStatusProblem_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
||||
"%s => line %d:\nSetting speed of axis %d to %lf.\n",
|
||||
__PRETTY_FUNCTION__, __LINE__, axisNo_, motorVelocity);
|
||||
}
|
||||
|
||||
// Set the target position
|
||||
snprintf(command, sizeof(command), "%dS02=%lf", axisNo_,
|
||||
motorCoordinatesPosition);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, false);
|
||||
if (rw_status != asynSuccess) {
|
||||
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
// Start the move
|
||||
if (relative) {
|
||||
snprintf(command, sizeof(command), "%dS00=2", axisNo_);
|
||||
} else {
|
||||
snprintf(command, sizeof(command), "%dS00=1", axisNo_);
|
||||
}
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, false);
|
||||
if (rw_status != asynSuccess) {
|
||||
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "motorStatusProblem_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
// Waiting for a handshake is already part of the movement procedure =>
|
||||
// Start the watchdog
|
||||
if (startMovTimeoutWatchdog() != asynSuccess) {
|
||||
return asynError;
|
||||
}
|
||||
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
asynStatus masterMacsAxis::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 command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
|
||||
// =========================================================================
|
||||
|
||||
snprintf(command, sizeof(command), "%dS00=8", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, false);
|
||||
if (rw_status != asynSuccess) {
|
||||
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 masterMacsAxis::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), "%dS00=9", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, false);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
pl_status = setIntegerParam(pC_->motorMoveToHome_, 1);
|
||||
if (pl_status != asynSuccess) {
|
||||
return pC_->paramLibAccessFailed(pl_status, "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 masterMacsAxis::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), "%dR60", axisNo_);
|
||||
rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
nvals = sscanf(response, "%d", &encoder_id);
|
||||
if (nvals != 1) {
|
||||
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
/*
|
||||
Defined encoder IDs:
|
||||
0=INC (Incremental)
|
||||
1=SSI (Absolute encoder with SSI interface)
|
||||
2=SSI (Absolute encoder with BiSS interface)
|
||||
*/
|
||||
if (encoder_id == 0) {
|
||||
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;
|
||||
}
|
||||
|
||||
asynStatus masterMacsAxis::enable(bool on) {
|
||||
|
||||
int timeout_enable_disable = 2;
|
||||
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
|
||||
// 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;
|
||||
doPoll(&moving);
|
||||
|
||||
// =========================================================================
|
||||
|
||||
// If the axis is currently moving, it cannot be disabled. Ignore the
|
||||
// command and inform the user. We check the last known status of the
|
||||
// axis instead of "moving", since status -6 is also moving, but the
|
||||
// motor can actually be disabled in this state!
|
||||
if (moving) {
|
||||
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%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__);
|
||||
}
|
||||
|
||||
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), "%dS04=%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, false);
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
// Query the axis status every few milliseconds until the axis has been
|
||||
// enabled or until the timeout has been reached
|
||||
int startTime = time(NULL);
|
||||
while (time(NULL) < startTime + timeout_enable_disable) {
|
||||
|
||||
// Read the axis status
|
||||
usleep(100000);
|
||||
rw_status = readAxisStatus();
|
||||
if (rw_status != asynSuccess) {
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
if (switchedOn() == 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;
|
||||
}
|
||||
|
||||
asynStatus masterMacsAxis::readAxisStatus() {
|
||||
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
|
||||
// =========================================================================
|
||||
|
||||
snprintf(command, sizeof(command), "%dR10", axisNo_);
|
||||
asynStatus rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status == asynSuccess) {
|
||||
|
||||
int axisStatus = 0;
|
||||
int nvals = sscanf(response, "%d", &axisStatus);
|
||||
if (nvals != 1) {
|
||||
return pC_->errMsgCouldNotParseResponse(
|
||||
command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
axisStatus_ = std::bitset<16>(axisStatus);
|
||||
}
|
||||
return rw_status;
|
||||
}
|
||||
|
||||
asynStatus masterMacsAxis::readAxisError() {
|
||||
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
|
||||
|
||||
// =========================================================================
|
||||
|
||||
snprintf(command, sizeof(command), "%dR11", axisNo_);
|
||||
asynStatus rw_status = pC_->writeRead(axisNo_, command, response, true);
|
||||
if (rw_status == asynSuccess) {
|
||||
|
||||
int axisError = 0;
|
||||
int nvals = sscanf(response, "%d", &axisError);
|
||||
if (nvals != 1) {
|
||||
return pC_->errMsgCouldNotParseResponse(
|
||||
command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
axisError_ = std::bitset<16>(axisError);
|
||||
}
|
||||
return rw_status;
|
||||
}
|
||||
215
src/masterMacsAxis.h
Normal file
215
src/masterMacsAxis.h
Normal file
@ -0,0 +1,215 @@
|
||||
#ifndef masterMacsAXIS_H
|
||||
#define masterMacsAXIS_H
|
||||
#include "sinqAxis.h"
|
||||
#include <bitset>
|
||||
|
||||
// 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 masterMacsController;
|
||||
|
||||
class masterMacsAxis : public sinqAxis {
|
||||
public:
|
||||
/**
|
||||
* @brief Construct a new masterMacsAxis
|
||||
*
|
||||
* @param pController Pointer to the associated controller
|
||||
* @param axisNo Index of the axis
|
||||
*/
|
||||
masterMacsAxis(masterMacsController *pController, int axisNo);
|
||||
|
||||
/**
|
||||
* @brief Destroy the masterMacsAxis
|
||||
*
|
||||
*/
|
||||
virtual ~masterMacsAxis();
|
||||
|
||||
/**
|
||||
* @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();
|
||||
|
||||
protected:
|
||||
masterMacsController *pC_;
|
||||
|
||||
asynStatus readConfig();
|
||||
bool initial_poll_;
|
||||
|
||||
/*
|
||||
The axis status and axis error of MasterMACS are given as an integer from
|
||||
the controller. The 16 individual bits contain the actual information.
|
||||
*/
|
||||
std::bitset<16> axisStatus_;
|
||||
std::bitset<16> axisError_;
|
||||
|
||||
/**
|
||||
* @brief Read the Master MACS status with the xR10 command and store the
|
||||
* result in axisStatus_
|
||||
*
|
||||
*/
|
||||
asynStatus readAxisStatus();
|
||||
|
||||
/*
|
||||
The functions below read the specified status bit from the axisStatus_
|
||||
bitset. Since a bit can either be 0 or 1, the return value is given as a
|
||||
boolean.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool readyToBeSwitchedOn() { return axisStatus_[0]; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool switchedOn() { return axisStatus_[1]; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool enabled() { return axisStatus_[2]; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool faultConditionSet() { return axisStatus_[3]; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool voltagePresent() { return axisStatus_[4]; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool quickStopping() { return axisStatus_[5] == 0; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool switchOnDisabled() { return axisStatus_[6]; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool newMoveCommandWhileMoving() { return axisStatus_[7]; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool isMoving() { return axisStatus_[8]; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool remoteMode() { return axisStatus_[9]; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool targetReached() { return axisStatus_[10]; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool internalLimitActive() { return axisStatus_[11]; }
|
||||
|
||||
// Bits 12 and 13 are unused
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool setEventHasOcurred() { return axisStatus_[14]; }
|
||||
|
||||
/**
|
||||
* @brief Read the property from axisStatus_
|
||||
*/
|
||||
bool powerEnabled() { return axisStatus_[15]; }
|
||||
|
||||
/**
|
||||
* @brief Read the Master MACS status with the xR10 command and store the
|
||||
* result in axisStatus_
|
||||
*
|
||||
*/
|
||||
asynStatus readAxisError();
|
||||
|
||||
/*
|
||||
The functions below read the specified error bit from the axisError_
|
||||
bitset. Since a bit can either be 0 or 1, the return value is given as a
|
||||
boolean.
|
||||
*/
|
||||
|
||||
private:
|
||||
friend class masterMacsController;
|
||||
};
|
||||
|
||||
#endif
|
||||
652
src/masterMacsController.cpp
Normal file
652
src/masterMacsController.cpp
Normal file
@ -0,0 +1,652 @@
|
||||
|
||||
#include "masterMacsController.h"
|
||||
#include "asynMotorController.h"
|
||||
#include "asynOctetSyncIO.h"
|
||||
#include "masterMacsAxis.h"
|
||||
#include <epicsExport.h>
|
||||
#include <errlog.h>
|
||||
#include <iocsh.h>
|
||||
#include <netinet/in.h>
|
||||
#include <registryFunction.h>
|
||||
#include <string.h>
|
||||
#include <string>
|
||||
#include <unistd.h>
|
||||
|
||||
/**
|
||||
* @brief Copy src into dst and replace all NULL terminators up to the carriage
|
||||
* return with spaces. This allows to print *dst with asynPrint.
|
||||
*
|
||||
* @param dst Buffer for the modified string
|
||||
* @param src Original string
|
||||
*/
|
||||
void adjustForPrint(char *dst, const char *src, size_t buf_length) {
|
||||
for (size_t i = 0; i < buf_length; i++) {
|
||||
if (src[i] == '\x0d') {
|
||||
dst[i] = ' ';
|
||||
break;
|
||||
} else if (src[i] == '\x00') {
|
||||
dst[i] = ' ';
|
||||
} else {
|
||||
dst[i] = src[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Construct a new masterMacsController::masterMacsController 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
|
||||
*/
|
||||
masterMacsController::masterMacsController(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:
|
||||
- REREAD_ENCODER_POSITION
|
||||
- READ_CONFIG
|
||||
*/
|
||||
NUM_masterMacs_DRIVER_PARAMS)
|
||||
|
||||
{
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
/*
|
||||
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 = "\x03"; // Hex-code for ETX
|
||||
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.
|
||||
*/
|
||||
masterMacsAxis *masterMacsController::getAxis(asynUser *pasynUser) {
|
||||
asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser);
|
||||
return masterMacsController::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
|
||||
*/
|
||||
masterMacsAxis *masterMacsController::getAxis(int axisNo) {
|
||||
asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo);
|
||||
return masterMacsController::castToAxis(asynAxis);
|
||||
}
|
||||
|
||||
masterMacsAxis *masterMacsController::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
|
||||
masterMacsAxis *axis = dynamic_cast<masterMacsAxis *>(asynAxis);
|
||||
if (axis == nullptr) {
|
||||
asynPrint(
|
||||
this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nAxis %d is not an instance of masterMacsAxis",
|
||||
__PRETTY_FUNCTION__, __LINE__, axis->axisNo_);
|
||||
}
|
||||
return axis;
|
||||
}
|
||||
|
||||
asynStatus masterMacsController::writeRead(int axisNo, const char *command,
|
||||
char *response,
|
||||
bool expectResponse) {
|
||||
|
||||
// Definition of local variables.
|
||||
asynStatus status = asynSuccess;
|
||||
asynStatus pl_status = asynSuccess;
|
||||
std::string fullCommand = "";
|
||||
char fullResponse[MAXBUF_] = {0};
|
||||
char printableCommand[MAXBUF_] = {0};
|
||||
char printableResponse[MAXBUF_] = {0};
|
||||
char drvMessageText[MAXBUF_] = {0};
|
||||
int motorStatusProblem = 0;
|
||||
|
||||
// Send the message and block the thread until either a response has
|
||||
// been received or the timeout is triggered
|
||||
int eomReason = 0; // Flag indicating why the message has ended
|
||||
|
||||
// Number of bytes of the outgoing message (which is command + the
|
||||
// end-of-string terminator defined in the constructor)
|
||||
size_t nbytesOut = 0;
|
||||
|
||||
// Number of bytes of the incoming message (which is response + the
|
||||
// end-of-string terminator defined in the constructor)
|
||||
size_t nbytesIn = 0;
|
||||
|
||||
// These parameters are used to remove not-needed information from the
|
||||
// response.
|
||||
int dataStartIndex = 0;
|
||||
int validIndex = 0;
|
||||
bool responseValid = false;
|
||||
|
||||
// =========================================================================
|
||||
|
||||
masterMacsAxis *axis = getAxis(axisNo);
|
||||
if (axis == nullptr) {
|
||||
// We already did the error logging directly in getAxis
|
||||
return asynError;
|
||||
}
|
||||
|
||||
/*
|
||||
PSI SINQ uses a custom protocol which is described in
|
||||
PSI_TCP_Interface_V1-8.pdf (p. // 4-17).
|
||||
A special case is the message length, which is specified by two bytes LSB
|
||||
and MSB:
|
||||
MSB = message length / 256
|
||||
LSB = message length % 256.
|
||||
For example, a message length of 47 chars would result in MSB = 0, LSB = 47,
|
||||
whereas a message length of 356 would result in MSB = 1, LSB = 100.
|
||||
|
||||
The full protocol looks as follows:
|
||||
0x05 -> Start of protocol frame ENQ
|
||||
[LSB]
|
||||
[MSB]
|
||||
0x19 -> Data type PDO1
|
||||
value [Actual message] It is not necessary to append a terminator, since
|
||||
this protocol encodes the message length in LSB and MSB.
|
||||
0x0D -> Carriage return (ASCII alias \r)
|
||||
0x03 -> End of text ETX
|
||||
|
||||
The rest of the telegram length is filled with 0x00 as specified in the
|
||||
protocol.
|
||||
*/
|
||||
|
||||
// Command has four additional bytes between ENQ and ETX:
|
||||
// LSB, MSB, PDO1 and CR
|
||||
const size_t commandLength = strlen(command) + 4;
|
||||
|
||||
// Perform both division and modulo operation at once.
|
||||
div_t commandLengthSep = std::div(commandLength, 256);
|
||||
|
||||
/*
|
||||
Build the actual command. LSB and MSB need to be converted directly to hex,
|
||||
hence they are interpolated as characters. For example, the eight hex value
|
||||
is 0x08, but interpolating 8 directly via %x or %d returns the 38th hex
|
||||
value, since 8 is interpreted as ASCII in those cases.
|
||||
*/
|
||||
fullCommand.push_back('\x05');
|
||||
fullCommand.push_back(commandLengthSep.rem);
|
||||
fullCommand.push_back(commandLengthSep.quot);
|
||||
fullCommand.push_back('\x19');
|
||||
for (size_t i = 0; i < strlen(command); i++) {
|
||||
fullCommand.push_back(command[i]);
|
||||
}
|
||||
fullCommand.push_back('\x0D');
|
||||
fullCommand.push_back('\x03');
|
||||
// snprintf(fullCommand, MAXBUF_, "\x05%c%c\x19%s\x0D\x03",
|
||||
// commandLengthSep.rem, commandLengthSep.quot, command);
|
||||
|
||||
adjustForPrint(printableCommand, fullCommand.c_str(), MAXBUF_);
|
||||
asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER,
|
||||
"%s => line %d:\nSending command %s\n", __PRETTY_FUNCTION__,
|
||||
__LINE__, printableCommand);
|
||||
|
||||
// Perform the actual writeRead
|
||||
status = pasynOctetSyncIO->writeRead(
|
||||
lowLevelPortUser_, fullCommand.c_str(), fullCommand.length(),
|
||||
fullResponse, MAXBUF_, comTimeout_, &nbytesOut, &nbytesIn, &eomReason);
|
||||
|
||||
// ___________________________________________________________DEBUG
|
||||
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
// "=================== COMMAND ===================\n");
|
||||
|
||||
// for (int i = 0; i < 12; ++i) {
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
|
||||
// fullCommand[i], fullCommand[i]);
|
||||
// }
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "\n");
|
||||
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
// "=================== RESPONSE ===================\n");
|
||||
|
||||
// for (int i = 0; i < 40; ++i) {
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
|
||||
// fullResponse[i], fullResponse[i]);
|
||||
// }
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "\n");
|
||||
|
||||
// ___________________________________________________________DEBUG
|
||||
|
||||
/*
|
||||
As of 19.12.2025, there is a bug in the MasterMACS hardware: If two commands
|
||||
are sent in rapid succession, MasterMACS sends garbage as answer for the
|
||||
second command. Crucially, this garbage does not contain an CR. Hence, the
|
||||
following strategy is implemented here:
|
||||
- Wait 1 ms after a pasynOctetSyncIO->writeRead
|
||||
- If the message does not contain an CR, wait 50 ms and then try again. If
|
||||
we receive garbage again, propagate the error.
|
||||
*/
|
||||
// Check for CR
|
||||
bool hasEtx = false;
|
||||
for (ulong i = 0; i < sizeof(fullResponse); i++) {
|
||||
if (fullResponse[i] == '\x0d') {
|
||||
hasEtx = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!hasEtx) {
|
||||
usleep(50000); // 50 ms
|
||||
|
||||
// Try again ...
|
||||
status = pasynOctetSyncIO->writeRead(
|
||||
lowLevelPortUser_, fullCommand.c_str(), fullCommand.length(),
|
||||
fullResponse, MAXBUF_, comTimeout_, &nbytesOut, &nbytesIn,
|
||||
&eomReason);
|
||||
|
||||
// Does this message contain an CR?
|
||||
for (ulong i = 0; i < sizeof(fullResponse); i++) {
|
||||
if (fullResponse[i] == '\x0d') {
|
||||
hasEtx = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!hasEtx) {
|
||||
adjustForPrint(printableCommand, fullCommand.c_str(), MAXBUF_);
|
||||
adjustForPrint(printableResponse, fullResponse, MAXBUF_);
|
||||
// Failed for the second time => Give up and propagate the error.
|
||||
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nReceived garbage response '%s' for "
|
||||
"command '%s' two times in a row.\n",
|
||||
__PRETTY_FUNCTION__, __LINE__, printableResponse,
|
||||
printableCommand);
|
||||
status = asynError;
|
||||
} else {
|
||||
usleep(1000); // 1 ms
|
||||
}
|
||||
} else {
|
||||
usleep(50000); // 1 ms
|
||||
}
|
||||
|
||||
// Create custom error messages for different failure modes
|
||||
switch (status) {
|
||||
case asynSuccess:
|
||||
|
||||
/*
|
||||
A response looks like this (note the spaces, they are part of the
|
||||
message!):
|
||||
- [ENQ][LSB][MSB][PDO1] 1 R 2=12.819[ACK][CR] (No error)
|
||||
- [ENQ][LSB][MSB][PDO1] 1 R 2=12.819[NAK][CR] (Communication failed)
|
||||
- [ENQ][LSB][MSB][PDO1] 1 S 10 [CAN][CR] (Driver tried to write with a
|
||||
read-only command)
|
||||
Read out the second-to-last char of the response and check if it is NAK
|
||||
or CAN.
|
||||
*/
|
||||
|
||||
// We don't use strlen here since the C string terminator 0x00 occurs in
|
||||
// the middle of the char array.
|
||||
for (ulong i = 0; i < sizeof(fullResponse); i++) {
|
||||
if (fullResponse[i] == '=') {
|
||||
dataStartIndex = i + 1;
|
||||
}
|
||||
if (fullResponse[i] == '\x06') {
|
||||
validIndex = i;
|
||||
responseValid = true;
|
||||
break;
|
||||
} else if (fullResponse[i] == '\x15') {
|
||||
// NAK
|
||||
snprintf(drvMessageText, sizeof(drvMessageText),
|
||||
"Communication failed.");
|
||||
responseValid = false;
|
||||
break;
|
||||
} else if (fullResponse[i] == '\x18') {
|
||||
// CAN
|
||||
snprintf(drvMessageText, sizeof(drvMessageText),
|
||||
"Tried to write with a read-only command. This is a "
|
||||
"bug, please call the support.");
|
||||
responseValid = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (responseValid) {
|
||||
/*
|
||||
If a property has been read, we need just the part between the "="
|
||||
(0x3D) and the [ACK] (0x06). Therefore, we remove all non-needed
|
||||
parts after evaluating the second-to-last char before returning the
|
||||
response.
|
||||
*/
|
||||
for (int i = 0; i + dataStartIndex < validIndex; i++) {
|
||||
response[i] = fullResponse[i + dataStartIndex];
|
||||
}
|
||||
} else {
|
||||
status = asynError;
|
||||
}
|
||||
|
||||
break; // Communicate nothing
|
||||
case asynTimeout:
|
||||
snprintf(drvMessageText, sizeof(drvMessageText),
|
||||
"connection timeout for axis %d", axisNo);
|
||||
break;
|
||||
case asynDisconnected:
|
||||
snprintf(drvMessageText, sizeof(drvMessageText),
|
||||
"axis is not connected");
|
||||
break;
|
||||
case asynDisabled:
|
||||
snprintf(drvMessageText, sizeof(drvMessageText), "axis is disabled");
|
||||
break;
|
||||
default:
|
||||
snprintf(drvMessageText, sizeof(drvMessageText),
|
||||
"Communication failed (%s)", stringifyAsynStatus(status));
|
||||
break;
|
||||
}
|
||||
|
||||
// ___________________________________________________________DEBUG
|
||||
|
||||
// adjustForPrint(printableCommand, fullCommand, MAXBUF_);
|
||||
// adjustForPrint(printableResponse, fullResponse, MAXBUF_);
|
||||
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
// "\n------------------c\n"); for (int i = 0; i < 12; ++i) {
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
|
||||
// fullCommand[i], fullCommand[i]);
|
||||
// }
|
||||
// for (int i = 0; i < 12; ++i) {
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
|
||||
// printableCommand[i], printableCommand[i]);
|
||||
// }
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
// "\n=================\n"); for (int i = 0; i < 12; ++i) {
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
|
||||
// fullResponse[i], fullResponse[i]);
|
||||
// }
|
||||
// for (int i = 0; i < 12; ++i) {
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "%x: %c\n",
|
||||
// printableResponse[i], printableResponse[i]);
|
||||
// }
|
||||
// asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
// "\n++++++++++++++++++++\n");
|
||||
|
||||
// asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR,
|
||||
// "%s => line %d:\nResponse '%s' for command '%s'.\n",
|
||||
// __PRETTY_FUNCTION__, __LINE__, printableResponse,
|
||||
// printableCommand);
|
||||
// ___________________________________________________________DEBUG
|
||||
|
||||
// Log the overall status (communication successfull or not)
|
||||
if (status == asynSuccess) {
|
||||
asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER,
|
||||
"%s => line %d:\nReturn value: %s\n", __PRETTY_FUNCTION__,
|
||||
__LINE__, response);
|
||||
pl_status = axis->setIntegerParam(this->motorStatusCommsError_, 0);
|
||||
} else {
|
||||
adjustForPrint(printableCommand, fullCommand.c_str(), MAXBUF_);
|
||||
asynPrint(
|
||||
lowLevelPortUser_, ASYN_TRACE_ERROR,
|
||||
"%s => line %d:\nCommunication failed for command '%s' (%s)\n",
|
||||
__PRETTY_FUNCTION__, __LINE__, printableCommand,
|
||||
stringifyAsynStatus(status));
|
||||
|
||||
// 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(motorMessageText_, drvMessageText);
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, "motorMessageText_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
pl_status = axis->setIntegerParam(motorStatusProblem_, 1);
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, "motorStatusProblem",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
|
||||
pl_status = axis->setIntegerParam(motorStatusProblem_, 1);
|
||||
if (pl_status != asynSuccess) {
|
||||
return paramLibAccessFailed(pl_status, "motorStatusCommsError_",
|
||||
__PRETTY_FUNCTION__, __LINE__);
|
||||
}
|
||||
}
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus sinqController::readInt32(asynUser *pasynUser, epicsInt32 *value) {
|
||||
// masterMacs can be disabled
|
||||
if (pasynUser->reason == motorCanDisable_) {
|
||||
*value = 1;
|
||||
return asynSuccess;
|
||||
} 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
|
||||
masterMacsController constructor documentation.
|
||||
*/
|
||||
asynStatus masterMacsCreateController(const char *portName,
|
||||
const char *ipPortConfigName, 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"
|
||||
masterMacsController *pController =
|
||||
new masterMacsController(portName, ipPortConfigName, numAxes,
|
||||
movingPollPeriod, idlePollPeriod, comTimeout);
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
/*
|
||||
C wrapper for the axis constructor. Please refer to the masterMacsAxis
|
||||
constructor documentation. The controller is read from the portName.
|
||||
*/
|
||||
asynStatus masterMacsCreateAxis(const char *portName, int axis) {
|
||||
masterMacsAxis *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
|
||||
masterMacsController *pC = dynamic_cast<masterMacsController *>(apd);
|
||||
if (pC == nullptr) {
|
||||
errlogPrintf("%s => line %d:\ncontroller on port %s is not a "
|
||||
"masterMacsController.",
|
||||
__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 masterMacsAxis(pC, axis);
|
||||
|
||||
// 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 name (e.g. mmacs1)",
|
||||
iocshArgString};
|
||||
static const iocshArg CreateControllerArg1 = {
|
||||
"Asyn IP port name (e.g. pmmacs1)", 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 configMasterMacsCreateController = {
|
||||
"masterMacsController", 6, CreateControllerArgs};
|
||||
static void configMasterMacsCreateControllerCallFunc(const iocshArgBuf *args) {
|
||||
masterMacsCreateController(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 name (e.g. mmacs1)",
|
||||
iocshArgString};
|
||||
static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt};
|
||||
static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0,
|
||||
&CreateAxisArg1};
|
||||
static const iocshFuncDef configMasterMacsCreateAxis = {"masterMacsAxis", 2,
|
||||
CreateAxisArgs};
|
||||
static void configMasterMacsCreateAxisCallFunc(const iocshArgBuf *args) {
|
||||
masterMacsCreateAxis(args[0].sval, args[1].ival);
|
||||
}
|
||||
|
||||
// This function is made known to EPICS in masterMacs.dbd and is called by EPICS
|
||||
// in order to register both functions in the IOC shell
|
||||
static void masterMacsRegister(void) {
|
||||
iocshRegister(&configMasterMacsCreateController,
|
||||
configMasterMacsCreateControllerCallFunc);
|
||||
iocshRegister(&configMasterMacsCreateAxis,
|
||||
configMasterMacsCreateAxisCallFunc);
|
||||
}
|
||||
epicsExportRegistrar(masterMacsRegister);
|
||||
|
||||
#endif
|
||||
|
||||
} // extern "C"
|
||||
105
src/masterMacsController.h
Normal file
105
src/masterMacsController.h
Normal file
@ -0,0 +1,105 @@
|
||||
/********************************************
|
||||
* masterMacsController.h
|
||||
*
|
||||
* PMAC V3 controller driver based on the asynMotorController class
|
||||
*
|
||||
* Stefan Mathis, September 2024
|
||||
********************************************/
|
||||
|
||||
#ifndef masterMacsController_H
|
||||
#define masterMacsController_H
|
||||
#include "masterMacsAxis.h"
|
||||
#include "sinqAxis.h"
|
||||
#include "sinqController.h"
|
||||
|
||||
class masterMacsController : public sinqController {
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Construct a new masterMacsController 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.
|
||||
*/
|
||||
masterMacsController(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 masterMacsAxis* If no axis could be found, this is a
|
||||
* nullptr
|
||||
*/
|
||||
masterMacsAxis *getAxis(asynUser *pasynUser);
|
||||
|
||||
/**
|
||||
* @brief Get the axis object
|
||||
*
|
||||
* @param axisNo Specify the axis via its index
|
||||
* @return masterMacsAxis* If no axis could be found, this is a
|
||||
* nullptr
|
||||
*/
|
||||
masterMacsAxis *getAxis(int axisNo);
|
||||
|
||||
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,
|
||||
bool expectResponse);
|
||||
|
||||
/**
|
||||
* @brief Save cast of the given asynAxis pointer to a masterMacsAxis
|
||||
* pointer. If the cast fails, this function returns a nullptr.
|
||||
*
|
||||
* @param asynAxis
|
||||
* @return masterMacsAxis*
|
||||
*/
|
||||
masterMacsAxis *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
|
||||
#define FIRST_masterMacs_PARAM rereadEncoderPosition_
|
||||
int rereadEncoderPosition_;
|
||||
int readConfig_;
|
||||
#define LAST_masterMacs_PARAM readConfig_
|
||||
|
||||
friend class masterMacsAxis;
|
||||
};
|
||||
#define NUM_masterMacs_DRIVER_PARAMS \
|
||||
(&LAST_masterMacs_PARAM - &FIRST_masterMacs_PARAM + 1)
|
||||
|
||||
#endif /* masterMacsController_H */
|
||||
223
utils/decodeStatus.py
Normal file
223
utils/decodeStatus.py
Normal file
@ -0,0 +1,223 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
The R10 status read command returns an integer, which needs to be interpreted
|
||||
bitwise for various status flags. This script prints out these status flags in
|
||||
human-readable formatting.
|
||||
|
||||
To read the manual, simply run this script without any arguments.
|
||||
|
||||
Stefan Mathis, December 2024
|
||||
"""
|
||||
|
||||
|
||||
# List of tuples which encodes the states given in the file description.
|
||||
# Index first with the bit index, then with the bit value
|
||||
interpretation = [
|
||||
("Not ready to be switched on", "Ready to be switched on"), # Bit 0
|
||||
("Not switched on", "Switched on"), # Bit 1
|
||||
("Disabled", "Enabled"), # Bit 2
|
||||
("Ok", "Fault condition set"), # Bit 3
|
||||
("Motor supply voltage absent ", "Motor supply voltage present"), # Bit 4
|
||||
("Motor performs quick stop", "Ok"), # Bit 5
|
||||
("Switch on enabled", "Switch on disabled"), # Bit 6
|
||||
("Ok", "RWarning: Movement function was called while motor is still moving. The function call is ignored"), # Bit 7
|
||||
("Motor is idle", "Motor is currently moving"), # Bit 8
|
||||
("Motor does not execute command messages (local mode)", "Motor does execute command messages (remote mode)"), # Bit 9
|
||||
("Target not reached", "Target reached"), # Bit 10
|
||||
("Ok", "Internal limit active"), # Bit 11
|
||||
("Not specified", "Not specified"), # Bit 12
|
||||
("Not specified", "Not specified"), # Bit 13
|
||||
("No event set or event has not occurred yet", "Set event has occurred"), # Bit 14
|
||||
("Axis off (power disabled)", "Axis on (power enabled)"), # Bit 15
|
||||
]
|
||||
|
||||
def decode(value, big_endian: bool = False):
|
||||
|
||||
interpreted = []
|
||||
|
||||
bit_list = [(value >> shift_ind) & 1
|
||||
for shift_ind in range(value.bit_length())] # little endian
|
||||
|
||||
if big_endian:
|
||||
bit_list.reverse() # big endian
|
||||
|
||||
for (bit, interpretations) in zip(bit_list, interpretation):
|
||||
interpreted.append(interpretations[bit])
|
||||
return (bit_list, interpreted)
|
||||
|
||||
def print_decoded(bit_list, interpreted):
|
||||
for (idx, (bit_value, msg)) in enumerate(zip(bit_list, interpreted)):
|
||||
print(f"Bit {idx} = {bit_value}: {msg}")
|
||||
|
||||
def interactive():
|
||||
|
||||
# Imported here, because curses is not available in Windows. Using the
|
||||
# interactive mode therefore fails on Windows, but at least the single
|
||||
# command mode can be used (which would not be possible if we would import
|
||||
# curses at the top level)
|
||||
import curses
|
||||
|
||||
stdscr = curses.initscr()
|
||||
curses.noecho()
|
||||
curses.cbreak()
|
||||
stdscr.keypad(True)
|
||||
stdscr.scrollok(True)
|
||||
|
||||
stdscr.addstr(">> ")
|
||||
stdscr.refresh()
|
||||
|
||||
history = [""]
|
||||
ptr = len(history) - 1
|
||||
|
||||
while True:
|
||||
c = stdscr.getch()
|
||||
if c == curses.KEY_RIGHT:
|
||||
(y, x) = stdscr.getyx()
|
||||
if x < len(history[ptr]) + 3:
|
||||
stdscr.move(y, x+1)
|
||||
stdscr.refresh()
|
||||
elif c == curses.KEY_LEFT:
|
||||
(y, x) = stdscr.getyx()
|
||||
if x > 3:
|
||||
stdscr.move(y, x-1)
|
||||
stdscr.refresh()
|
||||
elif c == curses.KEY_UP:
|
||||
if ptr > 0:
|
||||
ptr -= 1
|
||||
stdscr.addch("\r")
|
||||
stdscr.clrtoeol()
|
||||
stdscr.addstr(">> " + history[ptr])
|
||||
elif c == curses.KEY_DOWN:
|
||||
if ptr < len(history) - 1:
|
||||
ptr += 1
|
||||
stdscr.addch("\r")
|
||||
stdscr.clrtoeol()
|
||||
stdscr.addstr(">> " + history[ptr])
|
||||
elif c == curses.KEY_ENTER or c == ord('\n') or c == ord('\r'):
|
||||
if history[ptr] == 'quit':
|
||||
break
|
||||
|
||||
# because of arrow keys move back to the end of the line
|
||||
(y, x) = stdscr.getyx()
|
||||
stdscr.move(y, 3+len(history[ptr]))
|
||||
|
||||
if history[ptr]:
|
||||
result = interpret_inputs(history[ptr].split())
|
||||
if result is None:
|
||||
stdscr.addstr(f"\nBAD INPUT: Expected input of 'value [big_endian]', where 'value' is an int or a float and 'big_endian' is an optional boolean argument.")
|
||||
else:
|
||||
(arg, big_endian) = result
|
||||
(bit_list, interpreted) = decode(arg, big_endian)
|
||||
for (idx, (bit_value, msg)) in enumerate(zip(bit_list, interpreted)):
|
||||
stdscr.addstr(f"\nBit {idx} = {bit_value}: {msg}")
|
||||
stdscr.refresh()
|
||||
|
||||
if ptr == len(history) - 1 and history[ptr] != "":
|
||||
history += [""]
|
||||
else:
|
||||
history[-1] = ""
|
||||
ptr = len(history) - 1
|
||||
|
||||
stdscr.addstr("\n>> ")
|
||||
stdscr.refresh()
|
||||
|
||||
else:
|
||||
if ptr < len(history) - 1: # Modifying previous input
|
||||
if len(history[-1]) == 0:
|
||||
history[-1] = history[ptr]
|
||||
ptr = len(history) - 1
|
||||
|
||||
else:
|
||||
history += [history[ptr]]
|
||||
ptr = len(history) - 1
|
||||
|
||||
if c == curses.KEY_BACKSPACE:
|
||||
if len(history[ptr]) == 0:
|
||||
continue
|
||||
(y, x) = stdscr.getyx()
|
||||
history[ptr] = history[ptr][0:x-4] + history[ptr][x-3:]
|
||||
stdscr.addch("\r")
|
||||
stdscr.clrtoeol()
|
||||
stdscr.addstr(">> " + history[ptr])
|
||||
stdscr.move(y, x-1)
|
||||
stdscr.refresh()
|
||||
|
||||
else:
|
||||
(y, x) = stdscr.getyx()
|
||||
history[ptr] = history[ptr][0:x-3] + chr(c) + history[ptr][x-3:]
|
||||
stdscr.addch("\r")
|
||||
stdscr.clrtoeol()
|
||||
stdscr.addstr(">> " + history[ptr])
|
||||
stdscr.move(y, x+1)
|
||||
stdscr.refresh()
|
||||
|
||||
# to quit
|
||||
curses.nocbreak()
|
||||
stdscr.keypad(False)
|
||||
curses.echo()
|
||||
curses.endwin()
|
||||
|
||||
def interpret_inputs(inputs):
|
||||
|
||||
number = None
|
||||
big_endian = False
|
||||
try:
|
||||
number = int(float(inputs[0]))
|
||||
if len(inputs) > 1:
|
||||
second_arg = inputs[1]
|
||||
if second_arg == "True" or second_arg == "true":
|
||||
big_endian = True
|
||||
elif second_arg == "False" or second_arg == "false":
|
||||
big_endian = False
|
||||
else:
|
||||
big_endian = bool(int(second_arg))
|
||||
return (number, big_endian)
|
||||
except:
|
||||
return None
|
||||
|
||||
if __name__ == "__main__":
|
||||
from sys import argv
|
||||
|
||||
if len(argv) == 1:
|
||||
# Start interactive mode
|
||||
interactive()
|
||||
else:
|
||||
|
||||
result = interpret_inputs(argv[1:])
|
||||
|
||||
if result is None:
|
||||
print("""
|
||||
Decode R10 message of MasterMACs
|
||||
------------------
|
||||
|
||||
MasterMACs returns its status message (R10) as a floating-point number.
|
||||
The bits of this float encode different states. These states are stored
|
||||
in the interpretation variable.
|
||||
|
||||
This script can be used in two different ways:
|
||||
|
||||
Option 1: Single Command
|
||||
------------------------
|
||||
|
||||
Usage: decodeMasterMACStatusR10.py value [big_endian]
|
||||
|
||||
'value' is the return value of a R10 command. This value is interpreted
|
||||
bit-wise and the result is printed out. The optional second argument can
|
||||
be used to specify whether the input value needs to be interpreted as
|
||||
little or big endian. Default is False.
|
||||
|
||||
Option 2: CLI Mode
|
||||
------------------
|
||||
|
||||
Usage: decodeMasterMACStatusR10.py
|
||||
|
||||
A prompt will be opened. Type in the return value of a R10 command, hit
|
||||
enter and the interpretation will be printed in the prompt. After that,
|
||||
the next value can be typed in. Type 'quit' to close the prompt.
|
||||
""")
|
||||
else:
|
||||
print("Motor status")
|
||||
print("============")
|
||||
(arg, big_endian) = result
|
||||
(bit_list, interpreted) = decode(arg, big_endian)
|
||||
print_decoded(bit_list, interpreted)
|
||||
Reference in New Issue
Block a user