478 lines
18 KiB
C++
478 lines
18 KiB
C++
#include "C804Axis.h"
|
|
#include "C804Controller.h"
|
|
#include <cmath>
|
|
#include <errlog.h>
|
|
#include <limits>
|
|
#include <math.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
|
|
C804Axis::C804Axis(C804Controller *pC, int axisNo)
|
|
: SINQAxis(pC, axisNo), pC_(pC) {
|
|
/*
|
|
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. See C804Controller.cpp
|
|
for further explanation. 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_) {
|
|
exit(-1);
|
|
}
|
|
last_position_steps_ = 0;
|
|
|
|
last_poll_ = 0.0;
|
|
}
|
|
|
|
C804Axis::~C804Axis(void) {
|
|
// Since the controller memory is managed somewhere else, we don't need to
|
|
// clean up the pointer pC here.
|
|
}
|
|
|
|
/*
|
|
The polling function informs us about the state of the axis, in particular if it
|
|
is currently moving. It is called periodically, with the period defined by
|
|
the controller constructor arguments idlePollPeriod and movingPollPeriod
|
|
depending on the current axis state.
|
|
*/
|
|
asynStatus C804Axis::poll(bool *moving) {
|
|
// Local variable declaration
|
|
static const char *functionName = "C804Axis::poll";
|
|
|
|
// The poll function is just a wrapper around poll_no_param_lib_update and
|
|
// handles mainly the callParamCallbacks() function
|
|
asynStatus status_poll = C804Axis::poll_no_param_lib_update(moving);
|
|
|
|
// According to the function documentation of asynMotorAxis::poll, this
|
|
// function should be called at the end of a poll implementation.
|
|
asynStatus status_callback = callParamCallbacks();
|
|
if (status_callback != asynSuccess) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: Updating the parameter library failed for axis %d\n",
|
|
functionName, axisNo_);
|
|
return status_callback;
|
|
} else {
|
|
return status_poll;
|
|
}
|
|
}
|
|
|
|
// Perform the actual poll
|
|
asynStatus C804Axis::poll_no_param_lib_update(bool *moving) {
|
|
// Local variable declaration
|
|
static const char *functionName = "C804Axis::poll";
|
|
asynStatus status;
|
|
int axis_status = 0;
|
|
|
|
// The controller returns the position and velocity in encoder steps.
|
|
// This value needs to be converted in user units (engineering units EGU)
|
|
// via the record field MRES of the motor record. This field has already
|
|
// been read by the constructor into the member variable
|
|
// motorRecResolution_. To go from steps to user units, multiply with
|
|
// motorRecResolution_ Example: If 10 steps correspond to 1 mm, MRES should
|
|
// be 0.1.
|
|
int position_error_steps = 0;
|
|
int motor_position_steps = 0;
|
|
int motor_velocity_steps = 0;
|
|
int programmed_motor_velocity_steps = 0;
|
|
double position_error = .0;
|
|
double motor_position = .0;
|
|
double motor_velocity = .0;
|
|
double programmed_motor_velocity = .0;
|
|
|
|
// The buffer sizes for command and response are defined in the controller
|
|
// (see the corresponding source code files)
|
|
char command[pC_->C804_MAXBUF_], response[pC_->C804_MAXBUF_];
|
|
|
|
/*
|
|
Cancel the poll if the last poll has "just" happened.
|
|
*/
|
|
if (time(NULL) < last_poll_ + 0.5 * pC_->movingPollPeriod_) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING,
|
|
"%s: Aborted poll since the last poll for axis %d happened a "
|
|
"short time ago\n",
|
|
functionName, axisNo_);
|
|
return asynSuccess;
|
|
} else {
|
|
last_poll_ = time(NULL);
|
|
}
|
|
|
|
/*
|
|
The parameter motorRecResolution_ is coupled to the field MRES of the motor
|
|
record in the following manner:
|
|
- In sinq_asyn_motor.db, the PV (motor_record_pv_name)MOTOR_REC_RESOLUTION
|
|
is defined as a copy of the field (motor_record_pv_name).MRES:
|
|
|
|
record(ao,"$(P)$(M):Resolution") {
|
|
field(DESC, "$(M) resolution")
|
|
field(DOL, "$(P)$(M).MRES CP MS")
|
|
field(OMSL, "closed_loop")
|
|
field(DTYP, "asynFloat64")
|
|
field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_REC_RESOLUTION")
|
|
field(PREC, "$(PREC)")
|
|
}
|
|
|
|
- The PV name MOTOR_REC_RESOLUTION is coupled in asynMotorController.h to
|
|
the constant motorRecResolutionString
|
|
- ... which in turn is assigned to motorRecResolution_ in
|
|
asynMotorController.cpp This way of making the field visible to the driver
|
|
is described here: https://epics.anl.gov/tech-talk/2020/msg00378.php This is
|
|
a one-way coupling, changes to the parameter library via setDoubleParam are
|
|
NOT transferred to (motor_record_pv_name).MRES or to
|
|
(motor_record_pv_name):Resolution.
|
|
|
|
NOTE: This function must not be called in the constructor (e.g. in order to
|
|
save the read result to the member variable earlier), since the parameter
|
|
library is updated at a later stage!
|
|
*/
|
|
pC_->getDoubleParam(axisNo_, pC_->motorRecResolution_,
|
|
&motorRecResolution_);
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW, "Poll axis %d\n", axisNo_);
|
|
|
|
/*
|
|
We know that the motor resolution must not be zero. During the startup of
|
|
the IOC, polls can happen before the record is fully initialized. In that
|
|
case, all values are zero.
|
|
*/
|
|
if (motorRecResolution_ == 0) {
|
|
return asynError;
|
|
}
|
|
|
|
/*
|
|
Assume that the axis does not have a status problem. If it does have a
|
|
problem, this value will be overwritten further below. Setting this value
|
|
in itself does not trigger a callback immediately, any callbacks
|
|
(such as e.g. updating camonitor) are done in callParamCallbacks() at the
|
|
end of this function.
|
|
*/
|
|
setIntegerParam(pC_->motorStatusProblem_, false);
|
|
|
|
// Read out the position error of the axis (delta of target position to
|
|
// actual position)
|
|
snprintf(command, pC_->C804_MAXBUF_ - 1, "%dTE", axisNo_);
|
|
status = pC_->lowLevelWriteRead(axisNo_, command, response, true);
|
|
if (status == asynSuccess) {
|
|
int parsed_axis;
|
|
sscanf(response, "%2dE%10d", &parsed_axis, &position_error_steps);
|
|
|
|
// Scale from the encoder resultion to user units
|
|
position_error = double(position_error_steps) * motorRecResolution_;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s: Axis %d, response %s, position error %f\n", functionName,
|
|
axisNo_, response, position_error);
|
|
} else {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: Reading the position error failed for axis %d\n",
|
|
functionName, axisNo_);
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
|
|
// Stop the evaluation prematurely
|
|
return status;
|
|
}
|
|
|
|
// Read the current position.
|
|
snprintf(command, this->pC_->C804_MAXBUF_ - 1, "%dTP", this->axisNo_);
|
|
status =
|
|
this->pC_->lowLevelWriteRead(this->axisNo_, command, response, true);
|
|
if (status == asynSuccess) {
|
|
int parsed_axis;
|
|
sscanf(response, "%2dP%10d", &parsed_axis, &motor_position_steps);
|
|
|
|
// Scale from the encoder resultion to user units
|
|
motor_position = double(motor_position_steps) * motorRecResolution_;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s: Axis %d, response %s, position %f\n", functionName,
|
|
axisNo_, response, motor_position);
|
|
setDoubleParam(pC_->motorPosition_, motor_position);
|
|
setDoubleParam(pC_->motorEncoderPosition_, motor_position);
|
|
} else {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: Reading the position failed for axis %d\n", functionName,
|
|
axisNo_);
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
|
|
return status;
|
|
}
|
|
|
|
// Read the current velocity
|
|
snprintf(command, this->pC_->C804_MAXBUF_ - 1, "%dTV", this->axisNo_);
|
|
status =
|
|
this->pC_->lowLevelWriteRead(this->axisNo_, command, response, true);
|
|
if (status == asynSuccess) {
|
|
int parsed_axis;
|
|
sscanf(response, "%2dV%10d", &parsed_axis, &motor_velocity_steps);
|
|
|
|
// Scale from the encoder resultion to user units
|
|
motor_velocity = double(motor_velocity_steps) * motorRecResolution_;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s: Axis %d, response %s, velocity %f\n", functionName,
|
|
axisNo_, response, motor_velocity);
|
|
} else {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: Reading the velocity failed for axis %d\n", functionName,
|
|
axisNo_);
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
|
|
return status;
|
|
}
|
|
|
|
// Read the programmed velocity
|
|
snprintf(command, this->pC_->C804_MAXBUF_ - 1, "%dTY", this->axisNo_);
|
|
status =
|
|
this->pC_->lowLevelWriteRead(this->axisNo_, command, response, true);
|
|
if (status == asynSuccess) {
|
|
|
|
int parsed_axis;
|
|
sscanf(response, "%2dY%10d", &parsed_axis,
|
|
&programmed_motor_velocity_steps);
|
|
|
|
// Scale from the encoder resultion to user units
|
|
programmed_motor_velocity =
|
|
double(programmed_motor_velocity_steps) * motorRecResolution_;
|
|
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s: Axis %d, response %s, programmed velocity %f\n",
|
|
functionName, axisNo_, response, programmed_motor_velocity);
|
|
} else {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: Reading the programmed velocity failed for axis %d\n",
|
|
functionName, axisNo_);
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
|
|
return status;
|
|
}
|
|
|
|
// Read the motor status
|
|
snprintf(command, pC_->C804_MAXBUF_ - 1, "%dTS", axisNo_);
|
|
status = pC_->lowLevelWriteRead(this->axisNo_, command, response, true);
|
|
if (status == asynSuccess) {
|
|
int parsed_axis;
|
|
sscanf(response, "%2dS%10d", &parsed_axis, &axis_status);
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
|
|
"%s: Axis %d, response %s, status %d\n", functionName,
|
|
axisNo_, response, axis_status);
|
|
} else {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: Reading the motor status %d\n", functionName, axisNo_);
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
|
|
// Stop prematurely
|
|
return status;
|
|
}
|
|
|
|
// Check if the axis is enabled by reading out bit 2 (see
|
|
// https://stackoverflow.com/questions/2249731/how-do-i-get-bit-by-bit-data-from-an-integer-value-in-c)
|
|
int mask = 1 << 2;
|
|
int masked_n = axis_status & mask;
|
|
// Is 1 if the axis is disabled
|
|
int disabled = masked_n >> 2;
|
|
if (disabled) {
|
|
enabled_ = false;
|
|
} else {
|
|
enabled_ = true;
|
|
}
|
|
|
|
/*
|
|
Determine if the motor is moving. This is determined by the following
|
|
criteria: 1) The motor position changes from poll to poll 2) The motor is
|
|
enabled
|
|
*/
|
|
*moving = enabled_ && motor_position_steps != this->last_position_steps_;
|
|
|
|
// Update the cached_position
|
|
this->last_position_steps_ = motor_position_steps;
|
|
|
|
/*
|
|
Calculate the time the motor should need to reach its target, based on the
|
|
programmed velocity and compare this to the actual time the motor has spent
|
|
moving. If it has spent too much time in a moving state without reaching
|
|
the target, stop the motor and return an error.
|
|
*/
|
|
if (*moving) {
|
|
|
|
int motorStatusMoving = 0;
|
|
pC_->getIntegerParam(axisNo_, pC_->motorStatusMoving_,
|
|
&motorStatusMoving);
|
|
|
|
// motor is moving, but didn't move in the last poll
|
|
if (motorStatusMoving == 0) {
|
|
time_t current_time = time(NULL);
|
|
|
|
// Factor 2 of the calculated moving time
|
|
estimatedArrivalTime_ =
|
|
current_time + std::ceil(2 * std::fabs(position_error) /
|
|
programmed_motor_velocity);
|
|
} else {
|
|
// /*
|
|
// Motor is moving for a longer time than it should: Stop it
|
|
// */
|
|
// if (time(NULL) > estimatedArrivalTime_)
|
|
// {
|
|
// snprintf(command, pC_->C804_MAXBUF_ - 1, "%dST", axisNo_);
|
|
// status = pC_->lowLevelWriteRead(axisNo_, command, response);
|
|
// asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Stopped
|
|
// axis %d since it moved for double the time it should to reach
|
|
// its target\n", functionName, axisNo_);
|
|
// }
|
|
}
|
|
}
|
|
|
|
/*
|
|
One of these parameters (or both) are used to set (PV-name).DMOV.
|
|
This PV tells EPICS whether the axis / motor is currently moving or not.
|
|
*/
|
|
setIntegerParam(pC_->motorStatusMoving_, *moving);
|
|
setIntegerParam(pC_->motorStatusDone_, !(*moving));
|
|
callParamCallbacks();
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus C804Axis::move(double position, int relative, double minVelocity,
|
|
double maxVelocity, double acceleration) {
|
|
asynStatus status;
|
|
static const char *functionName = "C804Axis::move";
|
|
char command[pC_->C804_MAXBUF_], response[pC_->C804_MAXBUF_];
|
|
double position_c_units = 0.0; // Controller units
|
|
int position_steps = 0;
|
|
|
|
// Convert from user coordinates (EGU) to controller coordinates (steps).
|
|
// Check for overflow
|
|
if (motorRecResolution_ == 0.0) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: MRES must not be zero. Movement is aborted",
|
|
functionName);
|
|
return asynError;
|
|
}
|
|
position_c_units = position / motorRecResolution_;
|
|
|
|
// Check for overflow during the division
|
|
if (position_c_units * motorRecResolution_ != position) {
|
|
asynPrint(
|
|
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: could not convert from user units (%f) to controller units "
|
|
"(user units divided by resolution MRES %f) due to overflow.",
|
|
functionName, position, motorRecResolution_);
|
|
return asynError;
|
|
}
|
|
|
|
// Steps can only be integer values => cast to integer while checking for
|
|
// overflow
|
|
if (std::numeric_limits<int>::max() < position_c_units ||
|
|
std::numeric_limits<int>::min() > position_c_units) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: target position %f cannot be converted to int "
|
|
"(overflow). Check target value %f and MRES %f",
|
|
functionName, position_c_units, position_c_units,
|
|
motorRecResolution_);
|
|
return asynError;
|
|
}
|
|
position_steps = static_cast<int>(position_c_units);
|
|
|
|
// Convert from relative to absolute values
|
|
if (relative) {
|
|
position_steps += last_position_steps_;
|
|
}
|
|
|
|
// If the axis is currently disabled, enable it
|
|
if (!enabled_) {
|
|
snprintf(command, pC_->C804_MAXBUF_ - 1, "%dGO", axisNo_);
|
|
status =
|
|
pC_->lowLevelWriteRead(this->axisNo_, command, response, false);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: Enabling axis %d\n failed", functionName, axisNo_);
|
|
return status;
|
|
}
|
|
}
|
|
|
|
// Start movement
|
|
snprintf(command, pC_->C804_MAXBUF_ - 1, "%dMA%d", axisNo_, position_steps);
|
|
status = pC_->lowLevelWriteRead(this->axisNo_, command, response, false);
|
|
if (status != asynSuccess) {
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: Setting the target position %d failed for axis %d\n",
|
|
functionName, position_steps, axisNo_);
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
return status;
|
|
}
|
|
setIntegerParam(pC_->motorStatusProblem_, false);
|
|
|
|
// Reset the error flag
|
|
errorReported_ = 0;
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus C804Axis::moveVelocity(double min_velocity, double max_velocity,
|
|
double acceleration) {
|
|
static const char *functionName = "C804Axis::moveVelocity";
|
|
return asynError;
|
|
}
|
|
|
|
asynStatus C804Axis::stop(double acceleration) {
|
|
asynStatus status = asynSuccess;
|
|
static const char *functionName = "C804Axis::stop";
|
|
char command[pC_->C804_MAXBUF_], response[pC_->C804_MAXBUF_];
|
|
|
|
bool moving = false;
|
|
|
|
poll(&moving);
|
|
if (moving) {
|
|
// ST = Stop
|
|
snprintf(command, pC_->C804_MAXBUF_ - 1, "%dST", axisNo_);
|
|
status = pC_->lowLevelWriteRead(axisNo_, command, response, false);
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE, "%s: Stop axis %d\n",
|
|
functionName, axisNo_);
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus C804Axis::home(double minVelocity, double maxVelocity,
|
|
double acceleration, int forwards) {
|
|
asynStatus status = asynSuccess;
|
|
static const char *functionName = "C804Axis::home";
|
|
char command[pC_->C804_MAXBUF_], response[pC_->C804_MAXBUF_];
|
|
|
|
snprintf(command, pC_->C804_MAXBUF_ - 1, "%dFE0",
|
|
axisNo_); // Home to the upper limit of the axis (25 mm)
|
|
status = pC_->lowLevelWriteRead(axisNo_, command, response, false);
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE, "%s: Homing axis %d\n",
|
|
functionName, axisNo_);
|
|
|
|
return status;
|
|
}
|
|
|
|
/**
|
|
If on is 0, disable the motor, otherwise enable it.
|
|
*/
|
|
asynStatus C804Axis::enable(int on) {
|
|
asynStatus status = asynSuccess;
|
|
static const char *functionName = "C804Axis::enable";
|
|
char command[pC_->C804_MAXBUF_], response[pC_->C804_MAXBUF_];
|
|
|
|
if (on == 0) {
|
|
snprintf(command, pC_->C804_MAXBUF_ - 1, "%dMF", axisNo_);
|
|
status = pC_->lowLevelWriteRead(axisNo_, command, response, false);
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE,
|
|
"%s: Disable axis %d\n", functionName, axisNo_);
|
|
} else {
|
|
snprintf(command, pC_->C804_MAXBUF_ - 1, "%dMN", axisNo_);
|
|
status = pC_->lowLevelWriteRead(axisNo_, command, response, false);
|
|
asynPrint(pC_->pasynUserSelf, ASYN_TRACEIO_DEVICE,
|
|
"%s: Enable axis %d\n", functionName, axisNo_);
|
|
}
|
|
return status;
|
|
}
|