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

Added support for (optional) variable speed drive mode and refactored

Browse Source 
some records into sinqMotor
This commit is contained in:
mathis_s
2024-11-29 14:54:05 +01:00
parent dc70b560f7
commit e967e65d33
5 changed files with 77 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -11,7 +11,7 @@ REQUIRED+=asynMotor
REQUIRED+=sinqMotor
# Specify the version of sinqMotor we want to build against
sinqMotor_VERSION=0.3.0
sinqMotor_VERSION=mathis_s
# These headers allow to depend on this library for derived drivers.
HEADERS += src/pmacv3Axis.h

37
db/pmacv3.db
View File

@ -20,40 +20,3 @@ record(longout, "$(P)$(M):Read_Config") {
field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) READ_CONFIG")
field(PINI, "NO")
}
# ===================================================================
# The following records read acceleration and velocity from the driver and
# copy those values into the corresponding fields of the main motor record.
# This strategy is described here: https://epics.anl.gov/tech-talk/2022/msg00464.php
# Helper record for the high limit which is filled in by the driver
record(ai, "$(P)$(M):MOTOR_VELOCITY-RBV")
{
field(DTYP, "asynFloat64")
field(INP, "@asyn($(CONTROLLER),$(AXIS)) MOTOR_VELOCITY_FROM_DRIVER")
field(SCAN, "I/O Intr")
field(FLNK, "$(P)$(M):PUSH_VELO_TO_FIELD")
}
# Push the value into the field of the main motor record
record(ao, "$(P)$(M):PUSH_VELO_TO_FIELD") {
field(DOL, "$(P)$(M):MOTOR_VELOCITY-RBV CP")
field(OUT, "$(P)$(M).VELO")
field(OMSL, "closed_loop") # This configuration keeps the PV and the field in sync
}
# Helper record for the low limit which is filled in by the driver
record(ai, "$(P)$(M):MOTOR_ACCL-RBV")
{
field(DTYP, "asynFloat64")
field(INP, "@asyn($(CONTROLLER),$(AXIS)) MOTOR_ACCEL_FROM_DRIVER")
field(SCAN, "I/O Intr")
field(FLNK, "$(P)$(M):PUSH_ACCL_TO_FIELD")
}
# Push the value into the field of the main motor record
record(ao, "$(P)$(M):PUSH_ACCL_TO_FIELD") {
field(DOL, "$(P)$(M):MOTOR_ACCL-RBV CP")
field(OUT, "$(P)$(M).ACCL")
field(OMSL, "closed_loop") # This configuration keeps the PV and the field in sync
}

89
src/pmacv3Axis.cpp
View File

@ -87,6 +87,7 @@ asynStatus pmacv3Axis::readConfig() {
double motorRecResolution = 0.0;
double motorPosition = 0.0;
double motorVelocity = 0.0;
double motorVmax = 0.0;
double motorAccel = 0.0;
int acoDelay = 0.0; // Offset time for the movement watchdog caused by
// the air cushions in milliseconds.
@ -103,14 +104,14 @@ asynStatus pmacv3Axis::readConfig() {
// Software limits and current position
snprintf(command, sizeof(command),
"P%2.2d00 Q%2.2d10 Q%2.2d04 Q%2.2d06 P%2.2d22", axisNo_, axisNo_,
axisNo_, axisNo_, axisNo_);
status = pC_->writeRead(axisNo_, command, response, 5);
"P%2.2d00 Q%2.2d10 Q%2.2d03 Q%2.2d04 Q%2.2d06 P%2.2d22", axisNo_,
axisNo_, axisNo_, axisNo_, axisNo_, axisNo_);
status = pC_->writeRead(axisNo_, command, response, 6);
if (status != asynSuccess) {
return status;
}
nvals = sscanf(response, "%d %lf %lf %lf %d", &axStatus, &motorPosition,
&motorVelocity, &motorAccel, &acoDelay);
nvals = sscanf(response, "%d %lf %lf %lf %lf %d", &axStatus, &motorPosition,
&motorVmax, &motorVelocity, &motorAccel, &acoDelay);
// The acoDelay is given in milliseconds -> Convert to seconds, rounded up
offsetMovTimeout_ = std::ceil(acoDelay / 1000.0);
@ -120,7 +121,7 @@ asynStatus pmacv3Axis::readConfig() {
// here to mm/s^2.
motorAccel = motorAccel * 1000;
if (nvals != 5) {
if (nvals != 6) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
@ -135,25 +136,14 @@ asynStatus pmacv3Axis::readConfig() {
__PRETTY_FUNCTION__, __LINE__);
}
status =
pC_->setDoubleParam(axisNo_, pC_->motorVelocityRBV_, motorVelocity);
// Write to the motor record fields
status = setVeloFields(motorVelocity, 0.0, motorVmax);
if (status != asynSuccess) {
return pC_->paramLibAccessFailed(status, "motorVelocityRBV_",
__PRETTY_FUNCTION__, __LINE__);
return status;
}
status = pC_->setDoubleParam(axisNo_, pC_->motorAccelRBV_, motorAccel);
status = setAcclField(motorAccel);
if (status != asynSuccess) {
return pC_->paramLibAccessFailed(status, "motorAccelRBV_",
__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__);
return status;
}
// Update the parameter library immediately
@ -618,10 +608,10 @@ asynStatus pmacv3Axis::doPoll(bool *moving) {
}
}
pl_status = setIntegerParam(pC_->enableMotorRBV_,
pl_status = setIntegerParam(pC_->motorEnableRBV_,
(axStatus != -3 && axStatus != -5));
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_",
return pC_->paramLibAccessFailed(pl_status, "motorEnableRBV_",
__PRETTY_FUNCTION__, __LINE__);
}
@ -681,12 +671,15 @@ asynStatus pmacv3Axis::doMove(double position, int relative, double minVelocity,
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
double motorCoordinatesPosition = 0.0;
int enabled = 0;
double motorRecResolution = 0.0;
double motorVelocity = 0.0;
int enabled = 0;
int motorCanSetSpeed = 0;
int writeOffset = 0;
// =========================================================================
pl_status = pC_->getIntegerParam(axisNo_, pC_->enableMotorRBV_, &enabled);
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableRBV_, &enabled);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_",
__PRETTY_FUNCTION__, __LINE__);
@ -708,18 +701,43 @@ asynStatus pmacv3Axis::doMove(double position, int relative, double minVelocity,
// 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);
// Perform handshake, Set target position and start the move command
// 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__);
}
// Prepend the new motor speed, if the user is allowed to set the speed.
// Mind the " " (space) before the closing "", as the command created here
// is prepended to the one down below.
if (motorCanSetSpeed != 0) {
snprintf(command, sizeof(command), "Q%2.2d04=%lf ", axisNo_,
motorVelocity);
writeOffset = strlen(command);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
"%s => line %d:\nSetting speed of axis %d to %lf.\n",
__PRETTY_FUNCTION__, __LINE__, axisNo_, motorVelocity);
}
// Perform handshake, Set target position (and speed, if allowed) 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_);
snprintf(&command[writeOffset], sizeof(command) - writeOffset,
"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_);
snprintf(&command[writeOffset], sizeof(command) - writeOffset,
"P%2.2d23=0 Q%2.2d01=%lf M%2.2d=1", axisNo_, axisNo_,
motorCoordinatesPosition, axisNo_);
}
// We don't expect an answer
@ -950,7 +968,7 @@ asynStatus pmacv3Axis::rereadEncoder() {
// Check if the axis is disabled. If not, inform the user that this
// is necessary
int enabled = 0;
pl_status = pC_->getIntegerParam(axisNo_, pC_->enableMotorRBV_, &enabled);
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorEnableRBV_, &enabled);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "enableMotorRBV_",
__PRETTY_FUNCTION__, __LINE__);
@ -1031,13 +1049,6 @@ asynStatus pmacv3Axis::enable(bool on) {
__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;
}

35
src/pmacv3Controller.cpp
View File

@ -54,10 +54,9 @@ pmacv3Controller::pmacv3Controller(const char *portName,
- ENCODER_TYPE
- REREAD_ENCODER_POSITION
- READ_CONFIG
- MOTOR_VELOCITY_FROM_DRIVER
- MOTOR_ACCEL_FROM_DRIVER
- ACCEL_FROM_DRIVER
*/
5)
4)
{
@ -114,26 +113,6 @@ pmacv3Controller::pmacv3Controller(const char *portName,
exit(-1);
}
status = createParam("MOTOR_VELOCITY_FROM_DRIVER", asynParamFloat64,
&motorVelocityRBV_);
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_ACCEL_FROM_DRIVER", asynParamFloat64,
&motorAccelRBV_);
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
@ -426,6 +405,16 @@ asynStatus pmacv3Controller::writeRead(int axisNo, const char *command,
return asynSuccess;
}
asynStatus pmacv3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) {
if (pasynUser->reason == motorCanDisable_) {
// By default, motors cannot be disabled
*value = 1;
return asynSuccess;
} else {
return sinqController::readInt32(pasynUser, value);
}
}
asynStatus pmacv3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) {
int function = pasynUser->reason;

23
src/pmacv3Controller.h
View File

@ -51,10 +51,20 @@ class pmacv3Controller : public sinqController {
pmacv3Axis *getAxis(int axisNo);
/**
* @brief Overloaded function of asynMotorController
* @brief Overloaded function of sinqController
*
* The function is overloaded to allow enabling / disabling the motor and
* rereading the encoder.
* The function is overloaded to read the canDisable property.
*
* @param pasynUser Specify the axis via the asynUser
* @param value Read-out value
* @return asynStatus
*/
asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
/**
* @brief Overloaded function of sinqController
*
* The function is overloaded to allow rereading the encoder and config.
*
* @param pasynUser Specify the axis via the asynUser
* @param value New value
@ -132,13 +142,6 @@ class pmacv3Controller : public sinqController {
int readConfig_;
int encoderType_;
/*
Same strategy as with the limits in sinqController -> Use additional PVs to
write speed and acceleration from the driver to the record.
*/
int motorVelocityRBV_;
int motorAccelRBV_;
friend class pmacv3Axis;
};