lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity
Files
eea6efa880ceeea67ad5fbd3090be10ab100bd8f
motorBase/motorApp/NewportSrc/HXPDriver.cpp
T

571 lines
20 KiB
C++
Raw Blame History

/*
FILENAME... HXPDriver.cpp
USAGE... Motor driver support for the Newport Hexapod controller.
Note: This driver was tested with the v1.3.x of the firmware
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <iocsh.h>
#include <epicsThread.h>
#include <epicsString.h>
#include <asynOctetSyncIO.h>
#include "HXPDriver.h"
#include "hxp_drivers.h"
#include <epicsExport.h>
#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5)
#define NUM_AXES 6
#define GROUP "HEXAPOD"
#define MRES 0.00001
static const char *driverName = "HXPDriver";
/** Creates a new HXPController object.
* \param[in] portName The name of the asyn port that will be created for this driver
* \param[in] IPAddress The Newport hexapod controller's ip address
* \param[in] IPPort TCP/IP port used to communicate with the hexapod controller
* \param[in] movingPollPeriod The time between polls when any axis is moving
* \param[in] idlePollPeriod The time between polls when no axis is moving
*/
HXPController::HXPController(const char *portName, const char *IPAddress, int IPPort,
double movingPollPeriod, double idlePollPeriod)
: asynMotorController(portName, NUM_AXES, NUM_HXP_PARAMS,
0, // No additional interfaces beyond those in base class
0, // No additional callback interfaces beyond those in base class
ASYN_CANBLOCK | ASYN_MULTIDEVICE,
1, // autoconnect
0, 0) // Default priority and stack size
{
int axis;
HXPAxis *pAxis;
static const char *functionName = "HXPController::HXPController";
axisNames_ = epicsStrDup("XYZUVW");
IPAddress_ = epicsStrDup(IPAddress);
IPPort_ = IPPort;
createParam(HXPMoveCoordSysString, asynParamInt32, &HXPMoveCoordSys_);
createParam(HXPStatusString, asynParamInt32, &HXPStatus_);
createParam(HXPErrorString, asynParamInt32, &HXPError_);
createParam(HXPErrorDescString, asynParamOctet, &HXPErrorDesc_);
createParam(HXPMoveAllString, asynParamInt32, &HXPMoveAll_);
createParam(HXPMoveAllTargetXString, asynParamFloat64, &HXPMoveAllTargetX_);
createParam(HXPMoveAllTargetYString, asynParamFloat64, &HXPMoveAllTargetY_);
createParam(HXPMoveAllTargetZString, asynParamFloat64, &HXPMoveAllTargetZ_);
createParam(HXPMoveAllTargetUString, asynParamFloat64, &HXPMoveAllTargetU_);
createParam(HXPMoveAllTargetVString, asynParamFloat64, &HXPMoveAllTargetV_);
createParam(HXPMoveAllTargetWString, asynParamFloat64, &HXPMoveAllTargetW_);
// This socket is used for polling by the controller and all axes
pollSocket_ = HXPTCP_ConnectToServer((char *)IPAddress, IPPort, HXP_POLL_TIMEOUT);
if (pollSocket_ < 0) {
printf("%s:%s: error calling TCP_ConnectToServer for pollSocket\n",
driverName, functionName);
}
HXPFirmwareVersionGet(pollSocket_, firmwareVersion_);
for (axis=0; axis<NUM_AXES; axis++) {
pAxis = new HXPAxis(this, axis);
}
startPoller(movingPollPeriod, idlePollPeriod, 2);
}
/** Creates a new HXPController object.
* Configuration command, called directly or from iocsh
* \param[in] portName The name of the asyn port that will be created for this driver
* \param[in] HXPPortName The name of the drvAsynIPPPort that was created previously to connect to the Newport hexapod controller
* \param[in] numAxes The number of axes that this controller supports
* \param[in] movingPollPeriod The time in ms between polls when any axis is moving
* \param[in] idlePollPeriod The time in ms between polls when no axis is moving
*/
extern "C" int HXPCreateController(const char *portName, const char *IPAddress, int IPPort,
int movingPollPeriod, int idlePollPeriod)
{
HXPController *pHXPController
= new HXPController(portName, IPAddress, IPPort, movingPollPeriod/1000., idlePollPeriod/1000.);
pHXPController = NULL;
return(asynSuccess);
}
/** Reports on status of the driver
* \param[in] fp The file pointer on which report information will be written
* \param[in] level The level of report detail desired
*
* If details > 0 then information is printed about each axis.
* After printing controller-specific information it calls asynMotorController::report()
*/
void HXPController::report(FILE *fp, int level)
{
int coordSys;
getIntegerParam(HXPMoveCoordSys_, &coordSys);
fprintf(fp, "Newport hexapod motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f, coordSys=%d\n",
this->portName, NUM_AXES, movingPollPeriod_, idlePollPeriod_, coordSys);
// Call the base class method
asynMotorController::report(fp, level);
}
/** Returns a pointer to an HXPAxis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
HXPAxis* HXPController::getAxis(asynUser *pasynUser)
{
return static_cast<HXPAxis*>(asynMotorController::getAxis(pasynUser));
}
/** Returns a pointer to an HXPAxis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] axisNo Axis index number. */
HXPAxis* HXPController::getAxis(int axisNo)
{
return static_cast<HXPAxis*>(asynMotorController::getAxis(axisNo));
}
/** Called when asyn clients call pasynInt32->write().
* Extracts the function and axis number from pasynUser.
* Sets the value in the parameter library.
* If the function is HXPMoveAll_ then it calls moves all motors with a single command
* For all other functions it calls asynMotorController::writeInt32.
* Calls any registered callbacks for this pasynUser->reason and address.
* \param[in] pasynUser asynUser structure that encodes the reason and address.
* \param[in] value Value to write. */
asynStatus HXPController::writeInt32(asynUser *pasynUser, epicsInt32 value)
{
int function = pasynUser->reason;
asynStatus status = asynSuccess;
HXPAxis *pAxis = getAxis(pasynUser);
static const char *functionName = "writeInt32";
/* Set the parameter and readback in the parameter library. This may be overwritten when we read back the
* status at the end, but that's OK */
status = setIntegerParam(pAxis->axisNo_, function, value);
if (function == HXPMoveAll_)
{
/* if value == 1: motors are moved
if value == 0: nothing is done and parameter is simply reset */
if (value == 1)
{
moveAll(pAxis);
}
}
else
{
/* Call base class method */
status = asynMotorController::writeInt32(pasynUser, value);
}
/* Do callbacks so higher layers see any changes */
callParamCallbacks(pAxis->axisNo_);
if (status)
asynPrint(pasynUser, ASYN_TRACE_ERROR,
"%s:%s: error, status=%d function=%d, value=%d\n",
driverName, functionName, status, function, value);
else
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
"%s:%s: function=%d, value=%d\n",
driverName, functionName, function, value);
return status;
}
/**
* Moves all hexpod axes to new target positions
*/
int HXPController::moveAll(HXPAxis *pAxis)
{
int status;
double x, y, z, u, v, w;
getDoubleParam(0, HXPMoveAllTargetX_, &x);
getDoubleParam(0, HXPMoveAllTargetY_, &y);
getDoubleParam(0, HXPMoveAllTargetZ_, &z);
getDoubleParam(0, HXPMoveAllTargetU_, &u);
getDoubleParam(0, HXPMoveAllTargetV_, &v);
getDoubleParam(0, HXPMoveAllTargetW_, &w);
status = HXPHexapodMoveAbsolute(pAxis->moveSocket_, GROUP, "Work", x, y, z, u, v, w);
/* This is similar to what is done in HXPAxis::move() */
if (status < 0)
{
/* Set the error */
setIntegerParam(HXPError_, status);
/* Get the error string */
HXPErrorStringGet(pAxis->moveSocket_, status, pAxis->errorDescFull_);
/* Trim the error string */
strncpy(pAxis->errorDesc_, pAxis->errorDescFull_, 39);
pAxis->errorDesc_[39] = 0;
/* Set the error description */
setStringParam(HXPErrorDesc_, pAxis->errorDesc_);
}
else
{
/* Clear the error */
setIntegerParam(HXPError_, 0);
setStringParam(HXPErrorDesc_, "");
/* If there was a way to force the motor record to switch to the move poll rate,
this would be the place to do it. */
}
callParamCallbacks();
return status;
}
// These are the HXPAxis methods
/** Creates a new HXPAxis object.
* \param[in] pC Pointer to the HXPController to which this axis belongs.
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
*
* Initializes register numbers, etc.
*/
HXPAxis::HXPAxis(HXPController *pC, int axisNo)
: asynMotorAxis(pC, axisNo),
pC_(pC)
{
axisName_ = pC_->axisNames_[axisNo];
sprintf(positionerName_, "%s.%c", GROUP, (char) axisName_);
// Couldn't a negative timeout be used here instead?
moveSocket_ = HXPTCP_ConnectToServer(pC_->IPAddress_, pC->IPPort_, HXP_POLL_TIMEOUT);
/* Set the poll rate on the moveSocket to a negative number, which means that SendAndReceive should do only a write, no read */
HXPTCP_SetTimeout(moveSocket_, -0.1);
pollSocket_ = pC_->pollSocket_;
/* Enable gain support so that the CNEN field to enable/disable the hexapod */
setIntegerParam(pC_->motorStatusGainSupport_, 1);
// does the hexapod read encoders from the stages? leave in for now to test relative moves
setIntegerParam(pC_->motorStatusHasEncoder_, 1);
}
/** Reports on status of the axis
* \param[in] fp The file pointer on which report information will be written
* \param[in] level The level of report detail desired
*
* After printing device-specific information calls asynMotorAxis::report()
*/
void HXPAxis::report(FILE *fp, int level)
{
if (level > 0) {
fprintf(fp, " axis %d\n", axisNo_);
fprintf(fp, " axisName %c\n", axisName_);
}
// Call the base class method
asynMotorAxis::report(fp, level);
}
asynStatus HXPAxis::move(double position, int relative, double baseVelocity, double slewVelocity, double acceleration)
{
int status;
double start_pos;
double end_pos;
double diff_pos;
double cur_pos[NUM_AXES];
double rel_pos[NUM_AXES] = {};
double *pos;
int coordSys; // 0 = work, 1 = tool
static const char *functionName = "HXPAxis::move";
asynStatus retval = asynSuccess;
pC_->getIntegerParam(pC_->HXPMoveCoordSys_, &coordSys);
if (relative) {
rel_pos[axisNo_] = position * MRES;
pos = rel_pos;
if (coordSys == 0)
{
status = HXPHexapodMoveIncremental(moveSocket_, GROUP, "Work", pos[0], pos[1], pos[2], pos[3], pos[4], pos[5]);
}
else
{
status = HXPHexapodMoveIncremental(moveSocket_, GROUP, "Tool", pos[0], pos[1], pos[2], pos[3], pos[4], pos[5]);
}
if (status != 0 && status != -27) {
asynPrint(pasynUser_, ASYN_TRACE_ERROR,
"%s:%s: Error performing HexapodMoveIncremental[%s,%d] %d\n",
driverName, functionName, pC_->portName, axisNo_, status);
/* Error -27 is caused when the motor record changes dir i.e. when it aborts a move! */
retval = asynError;
}
} else {
// get current positions before moving (could an error overflow the array?)
status = HXPGroupPositionCurrentGet(pollSocket_, GROUP, 6, (double *) &cur_pos);
// Capture current position for relative move calc
start_pos = cur_pos[axisNo_];
end_pos = position * MRES;
if (coordSys == 0)
{
// Update position of axis to be moved
cur_pos[axisNo_] = end_pos;
pos = cur_pos;
status = HXPHexapodMoveAbsolute(moveSocket_, GROUP, "Work", pos[0], pos[1], pos[2], pos[3], pos[4], pos[5]);
}
else
{
// Calculate relative move amount (needed for Tool coord-sys moves)
diff_pos = end_pos - start_pos;
// Update position of axis to be moved
rel_pos[axisNo_] = diff_pos;
pos = rel_pos;
status = HXPHexapodMoveIncremental(moveSocket_, GROUP, "Tool", pos[0], pos[1], pos[2], pos[3], pos[4], pos[5]);
}
if (status != 0 && status != -27) {
asynPrint(pasynUser_, ASYN_TRACE_ERROR,
"%s:%s: Error performing HexapodMoveAbsolute[%s,%d] %d\n",
driverName, functionName, pC_->portName, axisNo_, status);
/* Error -27 is caused when the motor record changes dir i.e. when it aborts a move! */
retval = asynError;
}
}
if (status < 0)
{
/* Set the error */
pC_->setIntegerParam(pC_->HXPError_, status);
/* Get the error string */
HXPErrorStringGet(moveSocket_, status, errorDescFull_);
/* Trim the error string */
strncpy(errorDesc_, errorDescFull_, 39);
errorDesc_[39] = 0;
/* Set the error description */
pC_->setStringParam(pC_->HXPErrorDesc_, errorDesc_);
}
else
{
/* Clear the error */
pC_->setIntegerParam(pC_->HXPError_, 0);
pC_->setStringParam(pC_->HXPErrorDesc_, "");
}
callParamCallbacks();
return retval;
}
asynStatus HXPAxis::home(double baseVelocity, double slewVelocity, double acceleration, int forwards)
{
// static const char *functionName = "HXPAxis::home";
// kill all
HXPGroupKill(moveSocket_, GROUP);
// initialize
HXPGroupInitialize(moveSocket_, GROUP);
// home
HXPGroupHomeSearch(moveSocket_, GROUP);
return asynSuccess;
}
asynStatus HXPAxis::stop(double acceleration )
{
int status;
//static const char *functionName = "HXPAxis::stop";
status = HXPGroupMoveAbort(moveSocket_, GROUP);
return asynSuccess;
}
asynStatus HXPAxis::setClosedLoop(bool closedLoop)
{
int status;
static const char *functionName = "HXPAxis::setClosedLoop";
if (closedLoop) {
status = HXPGroupMotionEnable(pollSocket_, GROUP);
if (status) {
asynPrint(pasynUser_, ASYN_TRACE_ERROR,
"%s:%s: [%s,%d]: error calling GroupMotionEnable status=%d\n",
driverName, functionName, pC_->portName, axisNo_, status);
} else {
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
"%s:%s: set XPS %s, axis %d closed loop enable\n",
driverName, functionName, pC_->portName, axisNo_);
}
} else {
status = HXPGroupMotionDisable(pollSocket_, GROUP);
if (status) {
asynPrint(pasynUser_, ASYN_TRACE_ERROR,
"%s:%s: [%s,%d]: error calling GroupMotionDisable status=%d\n",
driverName, functionName, pC_->portName, axisNo_, status);
} else {
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
"%s:%s: motorAxisSetInteger set XPS %s, axis %d closed loop disable\n",
driverName, functionName, pC_->portName, axisNo_);
}
}
return (asynStatus)status;
}
/** Polls the axis.
* This function reads the motor position, the limit status, the home status, the moving status,
* and the drive power-on status.
* It calls setIntegerParam() and setDoubleParam() for each item that it polls,
* and then calls callParamCallbacks() at the end.
* \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */
asynStatus HXPAxis::poll(bool *moving)
{
int status;
//char readResponse[25];
static const char *functionName = "HXPAxis::poll";
status = HXPGroupStatusGet(pollSocket_,
GROUP,
&axisStatus_);
if (status) {
asynPrint(pasynUser_, ASYN_TRACE_ERROR,
"%s:%s: [%s,%d]: error calling GroupStatusGet status=%d; pollSocket=%d\n",
driverName, functionName, pC_->portName, axisNo_, status, pollSocket_);
goto done;
}
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
"%s:%s: [%s,%d]: %s axisStatus=%d\n",
driverName, functionName, pC_->portName, axisNo_, positionerName_, axisStatus_);
/* Set the status */
/* Note: there is only one status PV for the controller. Currently it reflects the status of axis 0.
It might be better to call pC_->setIntegerParam(pC_->HXPStatus_, axisStatus_); */
setIntegerParam(pC_->HXPStatus_, axisStatus_);
/* If the group is not moving then the axis is not moving */
if ((axisStatus_ < 43) || (axisStatus_ > 48))
moving_ = false;
else
moving_ = true;
/* Set the axis done parameter */
*moving = moving_;
setIntegerParam(pC_->motorStatusDone_, *moving?0:1);
/*Test for states that mean we cannot move an axis (disabled, uninitialised, etc.)
and set problem bit in MSTA.*/
if ((axisStatus_ < 10) || ((axisStatus_ >= 20) && (axisStatus_ <= 42)) ||
(axisStatus_ == 50) || (axisStatus_ == 64))
{
/* Don't consider a normal disabled status to be a problem */
if ( axisStatus_==20 )
{
setIntegerParam(pC_->motorStatusProblem_, 0);
}
else
{
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
"%s:%s: [%s,%d]: in unintialised/disabled/not referenced. XPS State Code: %d\n",
driverName, functionName, pC_->portName, axisNo_, axisStatus_);
setIntegerParam(pC_->motorStatusProblem_, 1);
}
/* Group status indicates power is off */
setIntegerParam(pC_->motorStatusPowerOn_, 0);
}
else
{
setIntegerParam(pC_->motorStatusProblem_, 0);
setIntegerParam(pC_->motorStatusPowerOn_, 1);
}
status = HXPGroupPositionCurrentGet(pollSocket_,
positionerName_,
1,
&encoderPosition_);
if (status) {
asynPrint(pasynUser_, ASYN_TRACE_ERROR,
"%s:%s: [%s,%d]: error calling GroupPositionCurrentGet status=%d\n",
driverName, functionName, pC_->portName, axisNo_, status);
goto done;
}
//setDoubleParam(pC_->motorEncoderPosition_, (encoderPosition_/stepSize_));
setDoubleParam(pC_->motorEncoderPosition_, encoderPosition_ / MRES);
status = HXPGroupPositionSetpointGet(pollSocket_,
positionerName_,
1,
&setpointPosition_);
if (status) {
asynPrint(pasynUser_, ASYN_TRACE_ERROR,
"%s:%s: [%s,%d]: error calling GroupPositionSetpointGet status=%d\n",
driverName, functionName, pC_->portName, axisNo_, status);
goto done;
}
//setDoubleParam(pC_->motorPosition_, (setpointPosition_/stepSize_));
setDoubleParam(pC_->motorPosition_, setpointPosition_ / MRES);
// limit check?
// dir flag?
done:
setIntegerParam(pC_->motorStatusProblem_, status ? 1:0);
callParamCallbacks();
return status ? asynError : asynSuccess;
}
/** Code for iocsh registration */
static const iocshArg HXPCreateControllerArg0 = {"Port name", iocshArgString};
static const iocshArg HXPCreateControllerArg1 = {"IP address", iocshArgString};
static const iocshArg HXPCreateControllerArg2 = {"Port", iocshArgInt};
static const iocshArg HXPCreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt};
static const iocshArg HXPCreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt};
static const iocshArg * const HXPCreateControllerArgs[] = {&HXPCreateControllerArg0,
&HXPCreateControllerArg1,
&HXPCreateControllerArg2,
&HXPCreateControllerArg3,
&HXPCreateControllerArg4};
static const iocshFuncDef HXPCreateControllerDef = {"HXPCreateController", 5, HXPCreateControllerArgs};
static void HXPCreateControllerCallFunc(const iocshArgBuf *args)
{
HXPCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival);
}
static void HXPRegister(void)
{
iocshRegister(&HXPCreateControllerDef, HXPCreateControllerCallFunc);
}
extern "C" {
epicsExportRegistrar(HXPRegister);
}
View Git Blame Copy Permalink