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

WIP version of the MasterMACS driver

Browse Source
This commit is contained in:
mathis_s
2025-01-28 13:15:53 +01:00
parent 2a74b6a478
commit ea8c34ab84
5 changed files with 399 additions and 359 deletions
Download Patch File Download Diff File Expand all files Collapse all files

285
src/masterMacsAxis.cpp
View File

@@ -40,9 +40,8 @@ masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo)
axisStatus_ = std::bitset<16>(0);
axisError_ = std::bitset<16>(0);
// Default values for the watchdog timeout mechanism
offsetMovTimeout_ = 30; // seconds
scaleMovTimeout_ = 2.0;
// Initial value for the motor speed, is overwritten in atFirstPoll.
lastSetSpeed_ = 0.0;
// masterMacs motors can always be disabled
status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable_, 1);
@@ -84,7 +83,7 @@ asynStatus masterMacsAxis::atFirstPoll() {
// Local variable declaration
asynStatus pl_status = asynSuccess;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char response[pC_->MAXBUF_];
int nvals = 0;
double motorRecResolution = 0.0;
double motorPosition = 0.0;
@@ -104,56 +103,54 @@ asynStatus masterMacsAxis::atFirstPoll() {
}
// Read out the current position
snprintf(command, sizeof(command), "%dR12", axisNo_);
pl_status = pC_->writeRead(axisNo_, command, response, true);
pl_status = pC_->read(axisNo_, 12, response);
if (pl_status != asynSuccess) {
return pl_status;
}
nvals = sscanf(response, "%lf", &motorPosition);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
return pC_->errMsgCouldNotParseResponse("R12", response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Read out the current velocity
snprintf(command, sizeof(command), "%dR05", axisNo_);
pl_status = pC_->writeRead(axisNo_, command, response, true);
pl_status = pC_->read(axisNo_, 05, response);
if (pl_status != asynSuccess) {
return pl_status;
}
nvals = sscanf(response, "%lf", &motorVelocity);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
return pC_->errMsgCouldNotParseResponse("R05", 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;
pl_status = pC_->read(axisNo_, 26, response);
if (pl_status != asynSuccess) {
return pl_status;
}
nvals = sscanf(response, "%lf", &motorVmax);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse("R26", response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Read out the acceleration
snprintf(command, sizeof(command), "%dR06", axisNo_);
pl_status = pC_->writeRead(axisNo_, command, response, true);
pl_status = pC_->read(axisNo_, 06, response);
if (pl_status != asynSuccess) {
return pl_status;
}
nvals = sscanf(response, "%lf", &motorAccel);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
return pC_->errMsgCouldNotParseResponse("R06", response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Cache the motor speed. If this value differs from the one in the motor
// record at the start of a movement, the motor record value is sent to
// MasterMACS.
lastSetSpeed_ = motorVelocity;
// Transform from motor to parameter library coordinates
motorPosition = motorPosition / motorRecResolution;
@@ -208,7 +205,7 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
// Status of parameter library operations
asynStatus pl_status = asynSuccess;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char response[pC_->MAXBUF_];
int nvals = 0;
int direction = 0;
@@ -219,7 +216,6 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
double highLimit = 0.0;
double lowLimit = 0.0;
double limitsOffset = 0.0;
int wasMoving = 0;
// =========================================================================
@@ -249,86 +245,113 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
return rw_status;
}
if (faultConditionSet()) {
rw_status = readAxisError();
// TODO: Handling
// 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
// }
}
// Update the movement status
*moving = isMoving();
if (*moving) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s => line %d:\nMOVMOVMOV\n", __PRETTY_FUNCTION__, __LINE__);
}
if (enabled()) {
}
// Read the current position
snprintf(command, sizeof(command), "%dR12", axisNo_);
rw_status = pC_->writeRead(axisNo_, command, response, true);
rw_status = pC_->read(axisNo_, 12, response);
if (rw_status != asynSuccess) {
return rw_status;
}
nvals = sscanf(response, "%lf", &currentPosition);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
return pC_->errMsgCouldNotParseResponse("R12", 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();
// Read out the limits, if the motor is not moving
if (!isMoving()) {
rw_status = pC_->read(axisNo_, 34, response);
if (rw_status != asynSuccess) {
return rw_status;
}
nvals = sscanf(response, "%lf", &lowLimit);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(
"R34", response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
}
// TODO> Error handling
rw_status = pC_->read(axisNo_, 33, response);
if (rw_status != asynSuccess) {
return rw_status;
}
nvals = sscanf(response, "%lf", &highLimit);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(
"R33", 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.
*/
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;
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__);
}
}
/*
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
// Update the enable PV
pl_status = setIntegerParam(pC_->motorEnableRBV_, enabled());
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorEnableRBV_",
@@ -379,20 +402,6 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
__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;
@@ -416,7 +425,7 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
// Status of parameter library operations
asynStatus pl_status = asynSuccess;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char value[pC_->MAXBUF_];
double motorCoordinatesPosition = 0.0;
double motorRecResolution = 0.0;
double motorVelocity = 0.0;
@@ -461,10 +470,14 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
__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);
// Set the new motor speed, if the user is allowed to do so and if the motor
// speed changed since the last move command.
if (motorCanSetSpeed != 0 && lastSetSpeed_ != motorVelocity) {
lastSetSpeed_ = motorVelocity;
snprintf(value, sizeof(value), "%lf", motorVelocity);
rw_status = pC_->write(axisNo_, 05, value);
if (rw_status != asynSuccess) {
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
if (pl_status != asynSuccess) {
@@ -481,9 +494,8 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
}
// Set the target position
snprintf(command, sizeof(command), "%dS02=%lf", axisNo_,
motorCoordinatesPosition);
rw_status = pC_->writeRead(axisNo_, command, response, false);
snprintf(value, sizeof(value), "%lf", motorCoordinatesPosition);
rw_status = pC_->write(axisNo_, 02, value);
if (rw_status != asynSuccess) {
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
if (pl_status != asynSuccess) {
@@ -495,11 +507,10 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
// Start the move
if (relative) {
snprintf(command, sizeof(command), "%dS00=2", axisNo_);
rw_status = pC_->write(axisNo_, 00, "2");
} else {
snprintf(command, sizeof(command), "%dS00=1", axisNo_);
rw_status = pC_->write(axisNo_, 00, "1");
}
rw_status = pC_->writeRead(axisNo_, command, response, false);
if (rw_status != asynSuccess) {
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
if (pl_status != asynSuccess) {
@@ -526,12 +537,9 @@ asynStatus masterMacsAxis::stop(double acceleration) {
// 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);
rw_status = pC_->write(axisNo_, 00, "8");
if (rw_status != asynSuccess) {
pl_status = setIntegerParam(pC_->motorStatusProblem_, true);
if (pl_status != asynSuccess) {
@@ -555,7 +563,7 @@ asynStatus masterMacsAxis::doHome(double min_velocity, double max_velocity,
// Status of parameter library operations
asynStatus pl_status = asynSuccess;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char response[pC_->MAXBUF_];
// =========================================================================
@@ -569,8 +577,7 @@ asynStatus masterMacsAxis::doHome(double min_velocity, double max_velocity,
// 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);
rw_status = pC_->write(axisNo_, 00, "9");
if (rw_status != asynSuccess) {
return rw_status;
}
@@ -616,8 +623,7 @@ asynStatus masterMacsAxis::readEncoderType() {
// =========================================================================
// Check if this is an absolute encoder
snprintf(command, sizeof(command), "%dR60", axisNo_);
rw_status = pC_->writeRead(axisNo_, command, response, true);
rw_status = pC_->read(axisNo_, 60, response);
if (rw_status != asynSuccess) {
return rw_status;
}
@@ -650,7 +656,7 @@ asynStatus masterMacsAxis::readEncoderType() {
asynStatus masterMacsAxis::enable(bool on) {
int timeout_enable_disable = 2;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char value[pC_->MAXBUF_];
// Status of read-write-operations of ASCII commands to the controller
asynStatus rw_status = asynSuccess;
@@ -659,10 +665,11 @@ asynStatus masterMacsAxis::enable(bool on) {
asynStatus pl_status = asynSuccess;
bool moving = false;
doPoll(&moving);
// =========================================================================
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
@@ -686,7 +693,7 @@ asynStatus masterMacsAxis::enable(bool on) {
// Axis is already enabled / disabled and a new enable / disable command
// was sent => Do nothing
if ((axisStatus_ != -3) == on) {
if (enabled() == 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,
@@ -695,7 +702,7 @@ asynStatus masterMacsAxis::enable(bool on) {
}
// Enable / disable the axis if it is not moving
snprintf(command, sizeof(command), "%dS04=%d", axisNo_, on);
snprintf(value, sizeof(value), "%d", 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_,
@@ -709,7 +716,7 @@ asynStatus masterMacsAxis::enable(bool on) {
return pC_->paramLibAccessFailed(pl_status, "motorMessageText_",
__PRETTY_FUNCTION__, __LINE__);
}
rw_status = pC_->writeRead(axisNo_, command, response, false);
rw_status = pC_->write(axisNo_, 04, value);
if (rw_status != asynSuccess) {
return rw_status;
}
@@ -736,14 +743,14 @@ asynStatus masterMacsAxis::enable(bool on) {
// Failed to change axis status within timeout_enable_disable => Send a
// corresponding message
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%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",
snprintf(value, sizeof(value), "Failed to %s within %d seconds",
on ? "enable" : "disable", timeout_enable_disable);
pl_status = setStringParam(pC_->motorMessageText_, "Enabling ...");
if (pl_status != asynSuccess) {
@@ -754,19 +761,18 @@ asynStatus masterMacsAxis::enable(bool on) {
}
asynStatus masterMacsAxis::readAxisStatus() {
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char response[pC_->MAXBUF_];
// =========================================================================
snprintf(command, sizeof(command), "%dR10", axisNo_);
asynStatus rw_status = pC_->writeRead(axisNo_, command, response, true);
asynStatus rw_status = pC_->read(axisNo_, 10, response);
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__);
"R10", response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
}
axisStatus_ = std::bitset<16>(axisStatus);
}
@@ -774,21 +780,20 @@ asynStatus masterMacsAxis::readAxisStatus() {
}
asynStatus masterMacsAxis::readAxisError() {
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char response[pC_->MAXBUF_];
// =========================================================================
snprintf(command, sizeof(command), "%dR11", axisNo_);
asynStatus rw_status = pC_->writeRead(axisNo_, command, response, true);
asynStatus rw_status = pC_->read(axisNo_, 11, response);
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__);
"R11", response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
}
axisError_ = std::bitset<16>(axisError);
}
return rw_status;
}
}