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

Added functions to get/set motorPosition.

Browse Source 
Changed to functions motorPosition and setMotorPosition in order to
retrieve / set motor positions from / to the paramLib.
This commit is contained in:
mathis_s
2025-03-28 14:57:20 +01:00
parent 83a74ce8d0
commit a990da4245
6 changed files with 36 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
src/turboPmacAxis.cpp
View File

@@ -122,7 +122,7 @@ asynStatus turboPmacAxis::init() {
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
int nvals = 0;
double motorRecResolution = 0.0;
double motorPosition = 0.0;
double motorPos = 0.0;
double motorVelocity = 0.0;
double motorVmax = 0.0;
double motorAccel = 0.0;
@@ -166,7 +166,7 @@ asynStatus turboPmacAxis::init() {
if (status != asynSuccess) {
return status;
}
nvals = sscanf(response, "%d %lf %lf %lf %lf %d", &axStatus, &motorPosition,
nvals = sscanf(response, "%d %lf %lf %lf %lf %d", &axStatus, &motorPos,
&motorVmax, &motorVelocity, &motorAccel, &acoDelay);
// The acoDelay is given in milliseconds -> Convert to seconds, rounded up
@@ -178,18 +178,14 @@ asynStatus turboPmacAxis::init() {
motorAccel = motorAccel * 1000;
if (nvals != 6) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
return pC_->couldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Transform from motor to parameter library coordinates
motorPosition = motorPosition / motorRecResolution;
// Store these values in the parameter library
status = pC_->setDoubleParam(axisNo_, pC_->motorPosition(), motorPosition);
status = setMotorPosition(motorPos);
if (status != asynSuccess) {
return pC_->paramLibAccessFailed(status, "motorPosition_", axisNo_,
__PRETTY_FUNCTION__, __LINE__);
return status;
}
// Initial motor status is idle
@@ -297,8 +293,8 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
nvals = sscanf(response, "%d %d", &handshakePerformed, &error);
if (nvals != 2) {
return pC_->errMsgCouldNotParseResponse(
command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
return pC_->couldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
if (error != 0) {
@@ -333,7 +329,6 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
}
}
// Motor resolution from parameter library
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
&motorRecResolution);
if (pl_status != asynSuccess) {
@@ -343,14 +338,11 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
}
// Read the previous motor position
pl_status =
pC_->getDoubleParam(axisNo_, pC_->motorPosition(), &previousPosition);
pl_status = motorPosition(&previousPosition);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorPosition_", axisNo_,
__PRETTY_FUNCTION__, __LINE__);
return pl_status;
}
// Read the previous motor position
pl_status = pC_->getIntegerParam(axisNo_, pC_->motorStatusDone(),
&previousStatusDone);
if (pl_status != asynSuccess) {
@@ -358,10 +350,6 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
__PRETTY_FUNCTION__, __LINE__);
}
// Transform from EPICS to motor coordinates (see comment in
// turboPmacAxis::atFirstPoll)
previousPosition = previousPosition * motorRecResolution;
// Query the axis status
snprintf(command, sizeof(command),
"P%2.2d00 Q%2.2d10 P%2.2d01 Q%2.2d13 Q%2.2d14", axisNo_, axisNo_,
@@ -374,8 +362,8 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
nvals = sscanf(response, "%d %lf %d %lf %lf", &axStatus, &currentPosition,
&error, &highLimit, &lowLimit);
if (nvals != 5) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
return pC_->couldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
/*
@@ -876,16 +864,10 @@ asynStatus turboPmacAxis::doPoll(bool *moving) {
__PRETTY_FUNCTION__, __LINE__);
}
// Transform from motor to EPICS coordinates (see comment in
// turboPmacAxis::init())
currentPosition = currentPosition / motorRecResolution;
pl_status = setDoubleParam(pC_->motorPosition(), currentPosition);
pl_status = setMotorPosition(currentPosition);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorPosition_", axisNo_,
__PRETTY_FUNCTION__, __LINE__);
return pl_status;
}
return poll_status;
}
@@ -1042,7 +1024,7 @@ asynStatus turboPmacAxis::stop(double acceleration) {
return rw_status;
}
asynStatus turboPmacAxis::reset() {
asynStatus turboPmacAxis::doReset() {
// Status of read-write-operations of ASCII commands to the controller
asynStatus rw_status = asynSuccess;
@@ -1160,16 +1142,16 @@ asynStatus turboPmacAxis::readEncoderType() {
int reponse_length = strlen(response);
if (reponse_length < 3) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
return pC_->couldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// We are only interested in the last two digits and the last value in
// the string before the terminator is \r
nvals = sscanf(response + (reponse_length - 3), "%2X", &encoder_id);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
return pC_->couldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
snprintf(command, sizeof(command), "P46");
@@ -1380,8 +1362,8 @@ asynStatus turboPmacAxis::enable(bool on) {
}
nvals = sscanf(response, "%d", &axisStatus_);
if (nvals != 1) {
return pC_->errMsgCouldNotParseResponse(
command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__);
return pC_->couldNotParseResponse(command, response, axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
if ((axisStatus_ != -3) == on) {