|
|
|
@@ -0,0 +1,349 @@
|
|
|
|
|
/*
|
|
|
|
|
FILENAME... 874xMotorDriver.cpp
|
|
|
|
|
USAGE... Motor driver support for the NewFocus 874x series motor controller
|
|
|
|
|
|
|
|
|
|
Based on ACRMotorDriver.cpp by:
|
|
|
|
|
Mark Rivers
|
|
|
|
|
March 4, 2011
|
|
|
|
|
|
|
|
|
|
== Modifications ==
|
|
|
|
|
2015-12-01 - Wayne Lewis - Modify for NewFocus 874x series
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* TODO: Extend for 8743 closed loop functionality
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
|
#include <string.h>
|
|
|
|
|
#include <stdlib.h>
|
|
|
|
|
|
|
|
|
|
#include <iocsh.h>
|
|
|
|
|
#include <epicsThread.h>
|
|
|
|
|
|
|
|
|
|
#include <asynOctetSyncIO.h>
|
|
|
|
|
|
|
|
|
|
#include "asynMotorController.h"
|
|
|
|
|
#include "asynMotorAxis.h"
|
|
|
|
|
|
|
|
|
|
#include <epicsExport.h>
|
|
|
|
|
#include "874xMotorDriver.h"
|
|
|
|
|
|
|
|
|
|
static const char *driverName = "nf874xMotorDriver";
|
|
|
|
|
|
|
|
|
|
/** Creates a new nf874xController object.
|
|
|
|
|
* \param[in] portName The name of the asyn port that will be created for this driver
|
|
|
|
|
* \param[in] nf874xPortName The name of the drvAsynIPPPort that was created previously to connect to the nf874x controller
|
|
|
|
|
* \param[in] numAxes The number of axes that this controller supports. Create one extra axis to allow for base 1 indexing of NewFocus controllers.
|
|
|
|
|
* \param[in] movingPollPeriod The time between polls when any axis is moving
|
|
|
|
|
* \param[in] idlePollPeriod The time between polls when no axis is moving
|
|
|
|
|
*/
|
|
|
|
|
nf874xController::nf874xController(const char *portName, const char *nf874xPortName, int numAxes,
|
|
|
|
|
double movingPollPeriod, double idlePollPeriod)
|
|
|
|
|
//: asynMotorController(portName, numAxes, NUM_nf874x_PARAMS,
|
|
|
|
|
: asynMotorController(portName, numAxes+1, 0,
|
|
|
|
|
asynUInt32DigitalMask,
|
|
|
|
|
asynUInt32DigitalMask,
|
|
|
|
|
ASYN_CANBLOCK | ASYN_MULTIDEVICE,
|
|
|
|
|
1, // autoconnect
|
|
|
|
|
0, 0) // Default priority and stack size
|
|
|
|
|
{
|
|
|
|
|
int axis;
|
|
|
|
|
asynStatus status;
|
|
|
|
|
static const char *functionName = "nf874xController";
|
|
|
|
|
|
|
|
|
|
/* Connect to nf874x controller */
|
|
|
|
|
status = pasynOctetSyncIO->connect(nf874xPortName, 0, &pasynUserController_, NULL);
|
|
|
|
|
if (status) {
|
|
|
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
|
|
|
"%s:%s: cannot connect to nf874x controller\n",
|
|
|
|
|
driverName, functionName);
|
|
|
|
|
}
|
|
|
|
|
epicsThreadSleep(0.5);
|
|
|
|
|
// Create the axis objects
|
|
|
|
|
// Axis 0 will remain unused. This allows consistent axis numbering with
|
|
|
|
|
// NewFocus convention.
|
|
|
|
|
for (axis=1; axis<=numAxes; axis++) {
|
|
|
|
|
new nf874xAxis(this, axis);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
startPoller(movingPollPeriod, idlePollPeriod, 2);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/** Creates a new nf874xController 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] nf874xPortName The name of the drvAsynIPPPort that was created previously to connect to the nf874x 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 nf874xCreateController(const char *portName, const char *nf874xPortName, int numAxes,
|
|
|
|
|
int movingPollPeriod, int idlePollPeriod)
|
|
|
|
|
{
|
|
|
|
|
new nf874xController(portName, nf874xPortName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
|
|
|
|
|
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 calls asynMotorController::report()
|
|
|
|
|
*/
|
|
|
|
|
void nf874xController::report(FILE *fp, int level)
|
|
|
|
|
{
|
|
|
|
|
fprintf(fp, "nf874x motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n",
|
|
|
|
|
this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_);
|
|
|
|
|
|
|
|
|
|
// Call the base class method
|
|
|
|
|
asynMotorController::report(fp, level);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** Returns a pointer to an nf874xMotorAxis object.
|
|
|
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
|
|
|
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
|
|
|
|
|
nf874xAxis* nf874xController::getAxis(asynUser *pasynUser)
|
|
|
|
|
{
|
|
|
|
|
return static_cast<nf874xAxis*>(asynMotorController::getAxis(pasynUser));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** Returns a pointer to an nf874xMotorAxis object.
|
|
|
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
|
|
|
* \param[in] axisNo Axis index number. */
|
|
|
|
|
nf874xAxis* nf874xController::getAxis(int axisNo)
|
|
|
|
|
{
|
|
|
|
|
return static_cast<nf874xAxis*>(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.
|
|
|
|
|
* 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 nf874xController::writeInt32(asynUser *pasynUser, epicsInt32 value)
|
|
|
|
|
{
|
|
|
|
|
int function = pasynUser->reason;
|
|
|
|
|
asynStatus status = asynSuccess;
|
|
|
|
|
nf874xAxis *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);
|
|
|
|
|
|
|
|
|
|
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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** Called when asyn clients call pasynFloat64->write().
|
|
|
|
|
* Extracts the function and axis number from pasynUser.
|
|
|
|
|
* Sets the value in the parameter library.
|
|
|
|
|
* Calls any registered callbacks for this pasynUser->reason and address.
|
|
|
|
|
* For all other functions it calls asynMotorController::writeFloat64.
|
|
|
|
|
* \param[in] pasynUser asynUser structure that encodes the reason and address.
|
|
|
|
|
* \param[in] value Value to write. */
|
|
|
|
|
asynStatus nf874xController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
|
|
|
|
|
{
|
|
|
|
|
int function = pasynUser->reason;
|
|
|
|
|
asynStatus status = asynSuccess;
|
|
|
|
|
nf874xAxis *pAxis = getAxis(pasynUser);
|
|
|
|
|
static const char *functionName = "writeFloat64";
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* Set the parameter and readback in the parameter library. */
|
|
|
|
|
status = setDoubleParam(pAxis->axisNo_, function, value);
|
|
|
|
|
|
|
|
|
|
status = asynMotorController::writeFloat64(pasynUser, value);
|
|
|
|
|
|
|
|
|
|
/* Do callbacks so higher layers see any changes */
|
|
|
|
|
pAxis->callParamCallbacks();
|
|
|
|
|
if (status)
|
|
|
|
|
asynPrint(pasynUser, ASYN_TRACE_ERROR,
|
|
|
|
|
"%s:%s: error, status=%d function=%d, value=%f\n",
|
|
|
|
|
driverName, functionName, status, function, value);
|
|
|
|
|
else
|
|
|
|
|
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
|
|
|
|
|
"%s:%s: function=%d, value=%f\n",
|
|
|
|
|
driverName, functionName, function, value);
|
|
|
|
|
return status;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// These are the nf874xAxis methods
|
|
|
|
|
|
|
|
|
|
/** Creates a new nf874xAxis object.
|
|
|
|
|
* \param[in] pC Pointer to the nf874xController to which this axis belongs.
|
|
|
|
|
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
|
|
|
|
|
*
|
|
|
|
|
* Initializes register numbers, etc.
|
|
|
|
|
*/
|
|
|
|
|
nf874xAxis::nf874xAxis(nf874xController *pC, int axisNo)
|
|
|
|
|
: asynMotorAxis(pC, axisNo),
|
|
|
|
|
pC_(pC)
|
|
|
|
|
{
|
|
|
|
|
sprintf(axisName_, "%d", axisNo);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** 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 calls asynMotorController::report()
|
|
|
|
|
*/
|
|
|
|
|
void nf874xAxis::report(FILE *fp, int level)
|
|
|
|
|
{
|
|
|
|
|
if (level > 0) {
|
|
|
|
|
fprintf(fp, " axis %d\n", axisNo_ );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Call the base class method
|
|
|
|
|
asynMotorAxis::report(fp, level);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
asynStatus nf874xAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
|
|
|
|
|
{
|
|
|
|
|
asynStatus status;
|
|
|
|
|
|
|
|
|
|
sprintf(pC_->outString_, "%s AC %f", axisName_, acceleration);
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
sprintf(pC_->outString_, "%s VA %f", axisName_, maxVelocity);
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
if (relative) {
|
|
|
|
|
sprintf(pC_->outString_, "%s PR %f", axisName_, position);
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
} else {
|
|
|
|
|
sprintf(pC_->outString_, "%s PA %f", axisName_, position);
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
}
|
|
|
|
|
return status;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
asynStatus nf874xAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
|
|
|
|
|
{
|
|
|
|
|
asynStatus status;
|
|
|
|
|
|
|
|
|
|
sprintf(pC_->outString_, "%s AC %f", axisName_, acceleration);
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
sprintf(pC_->outString_, "%s VA %f", axisName_, acceleration);
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
sprintf(pC_->outString_, "%s DH", axisName_);
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
return status;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
asynStatus nf874xAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
|
|
|
|
|
{
|
|
|
|
|
asynStatus status;
|
|
|
|
|
double speed=maxVelocity;
|
|
|
|
|
int forwards=1;
|
|
|
|
|
|
|
|
|
|
if (speed < 0) {
|
|
|
|
|
speed = -speed;
|
|
|
|
|
forwards = 0;
|
|
|
|
|
}
|
|
|
|
|
sprintf(pC_->outString_, "%s AC %f", axisName_, acceleration);
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
sprintf(pC_->outString_, "%s VA %f", axisName_, speed);
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
sprintf(pC_->outString_, "%s MV %s", axisName_, forwards ? "+" : "-");
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
return status;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
asynStatus nf874xAxis::stop(double acceleration )
|
|
|
|
|
{
|
|
|
|
|
asynStatus status;
|
|
|
|
|
|
|
|
|
|
sprintf(pC_->outString_, "%s ST", axisName_);
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
return status;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
asynStatus nf874xAxis::setPosition(double position)
|
|
|
|
|
{
|
|
|
|
|
asynStatus status;
|
|
|
|
|
|
|
|
|
|
sprintf(pC_->outString_, "%s DH %f", axisName_, position);
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
return status;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** Polls the axis.
|
|
|
|
|
* This function reads the controller position, encoder position, the limit status, the moving status,
|
|
|
|
|
* and the drive power-on status. It does not current detect following error, etc. but this could be
|
|
|
|
|
* added.
|
|
|
|
|
* 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 (1) or done (0). */
|
|
|
|
|
asynStatus nf874xAxis::poll(bool *moving)
|
|
|
|
|
{
|
|
|
|
|
int done;
|
|
|
|
|
asynStatus comStatus;
|
|
|
|
|
|
|
|
|
|
// Read the current encoder position
|
|
|
|
|
sprintf(pC_->outString_, "%s TP?", axisName_);
|
|
|
|
|
comStatus = pC_->writeReadController();
|
|
|
|
|
if (comStatus) goto skip;
|
|
|
|
|
encoderPosition_ = atof(pC_->inString_);
|
|
|
|
|
setDoubleParam(pC_->motorEncoderPosition_,encoderPosition_);
|
|
|
|
|
|
|
|
|
|
// Read the current theoretical position
|
|
|
|
|
setDoubleParam(pC_->motorPosition_, encoderPosition_);
|
|
|
|
|
|
|
|
|
|
// Read the current moving status
|
|
|
|
|
sprintf(pC_->outString_, "%s MD?", axisName_);
|
|
|
|
|
comStatus = pC_->writeReadController();
|
|
|
|
|
if (comStatus) goto skip;
|
|
|
|
|
done = atoi(pC_->inString_);
|
|
|
|
|
setIntegerParam(pC_->motorStatusDone_, done);
|
|
|
|
|
*moving = done ? false:true;
|
|
|
|
|
|
|
|
|
|
skip:
|
|
|
|
|
setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0);
|
|
|
|
|
callParamCallbacks();
|
|
|
|
|
return comStatus ? asynError : asynSuccess;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** Code for iocsh registration */
|
|
|
|
|
static const iocshArg nf874xCreateControllerArg0 = {"Port name", iocshArgString};
|
|
|
|
|
static const iocshArg nf874xCreateControllerArg1 = {"nf874x port name", iocshArgString};
|
|
|
|
|
static const iocshArg nf874xCreateControllerArg2 = {"Number of axes", iocshArgInt};
|
|
|
|
|
static const iocshArg nf874xCreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt};
|
|
|
|
|
static const iocshArg nf874xCreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt};
|
|
|
|
|
static const iocshArg * const nf874xCreateControllerArgs[] = {&nf874xCreateControllerArg0,
|
|
|
|
|
&nf874xCreateControllerArg1,
|
|
|
|
|
&nf874xCreateControllerArg2,
|
|
|
|
|
&nf874xCreateControllerArg3,
|
|
|
|
|
&nf874xCreateControllerArg4};
|
|
|
|
|
static const iocshFuncDef nf874xCreateControllerDef = {"nf874xCreateController", 5, nf874xCreateControllerArgs};
|
|
|
|
|
static void nf874xCreateContollerCallFunc(const iocshArgBuf *args)
|
|
|
|
|
{
|
|
|
|
|
nf874xCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static void nf874xMotorRegister(void)
|
|
|
|
|
{
|
|
|
|
|
iocshRegister(&nf874xCreateControllerDef, nf874xCreateContollerCallFunc);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
extern "C" {
|
|
|
|
|
epicsExportRegistrar(nf874xMotorRegister);
|
|
|
|
|
}
|
rsluiter