|
|
|
@@ -1,361 +0,0 @@
|
|
|
|
|
/*
|
|
|
|
|
FILENAME... C300MotorDriver.cpp
|
|
|
|
|
USAGE... Motor driver support for the nPoint C300 series of controllers
|
|
|
|
|
|
|
|
|
|
Kevin Peterson
|
|
|
|
|
March 27, 2012
|
|
|
|
|
|
|
|
|
|
KNOWN PROBLEMS:
|
|
|
|
|
|
|
|
|
|
1. The C300 behaves more like a temperature controller than a motor controller.
|
|
|
|
|
The position is the set point. There is no command to change the speed or
|
|
|
|
|
acceleration. There is no done-moving indicator. Attempts to change the speed
|
|
|
|
|
or acceleration from EPICS are ignored. The done-moving indicator is simulated.
|
|
|
|
|
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
|
#include <string.h>
|
|
|
|
|
#include <stdlib.h>
|
|
|
|
|
#include <math.h>
|
|
|
|
|
|
|
|
|
|
#include <iocsh.h>
|
|
|
|
|
#include <epicsThread.h>
|
|
|
|
|
|
|
|
|
|
#include <asynOctetSyncIO.h>
|
|
|
|
|
|
|
|
|
|
#include "C300MotorDriver.h"
|
|
|
|
|
#include <epicsExport.h>
|
|
|
|
|
|
|
|
|
|
#define C300_TIMEOUT 1.0
|
|
|
|
|
#define C300_DATA_OFFSET 112
|
|
|
|
|
#define C300_CORR_OFFSET 28
|
|
|
|
|
|
|
|
|
|
static const char *driverName = "C300MotorDriver";
|
|
|
|
|
|
|
|
|
|
int C300Tolerance = 256;
|
|
|
|
|
extern "C" {epicsExportAddress(int, C300Tolerance);}
|
|
|
|
|
|
|
|
|
|
/** Creates a new C300Controller object.
|
|
|
|
|
* \param[in] portName The name of the asyn port that will be created for this driver
|
|
|
|
|
* \param[in] C300PortName The name of the drvAsynIPPPort that was created previously to connect to the C300 controller
|
|
|
|
|
* \param[in] numAxes The number of axes that this controller supports
|
|
|
|
|
* \param[in] movingPollPeriod The time between polls when any axis is moving
|
|
|
|
|
* \param[in] idlePollPeriod The time between polls when no axis is moving
|
|
|
|
|
*/
|
|
|
|
|
C300Controller::C300Controller(const char *portName, const char *C300PortName, int numAxes,
|
|
|
|
|
double movingPollPeriod, double idlePollPeriod)
|
|
|
|
|
: asynMotorController(portName, numAxes, NUM_C300_PARAMS,
|
|
|
|
|
asynUInt32DigitalMask,
|
|
|
|
|
asynUInt32DigitalMask,
|
|
|
|
|
ASYN_CANBLOCK | ASYN_MULTIDEVICE,
|
|
|
|
|
1, // autoconnect
|
|
|
|
|
0, 0) // Default priority and stack size
|
|
|
|
|
{
|
|
|
|
|
int axis;
|
|
|
|
|
asynStatus status;
|
|
|
|
|
static const char *functionName = "C300Controller";
|
|
|
|
|
|
|
|
|
|
/* Connect to C300 controller */
|
|
|
|
|
status = pasynOctetSyncIO->connect(C300PortName, 0, &pasynUserC300_, NULL);
|
|
|
|
|
if (status) {
|
|
|
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
|
|
|
"%s:%s: cannot connect to C300 controller\n",
|
|
|
|
|
driverName, functionName);
|
|
|
|
|
}
|
|
|
|
|
// Unlock the controller
|
|
|
|
|
sprintf(outString_, "SYS:PASS:CEN \"nPoint\"");
|
|
|
|
|
writeController();
|
|
|
|
|
|
|
|
|
|
// get controller id, firmware version and range?
|
|
|
|
|
|
|
|
|
|
// Wait a short while so that any responses to the above commands have time to arrive so we can flush
|
|
|
|
|
// them in the next writeReadController()
|
|
|
|
|
//epicsThreadSleep(0.5);
|
|
|
|
|
|
|
|
|
|
// Create the axis objects
|
|
|
|
|
for (axis=0; axis<numAxes; axis++) {
|
|
|
|
|
new C300Axis(this, axis);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
startPoller(movingPollPeriod, idlePollPeriod, 2);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/** Creates a new C300Controller 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] C300PortName The name of the drvAsynIPPPort that was created previously to connect to the C300 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 C300CreateController(const char *portName, const char *C300PortName, int numAxes,
|
|
|
|
|
int movingPollPeriod, int idlePollPeriod)
|
|
|
|
|
{
|
|
|
|
|
new C300Controller(portName, C300PortName, 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 C300Controller::report(FILE *fp, int level)
|
|
|
|
|
{
|
|
|
|
|
int axis;
|
|
|
|
|
C300Axis *pAxis;
|
|
|
|
|
|
|
|
|
|
fprintf(fp, "C300 motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n",
|
|
|
|
|
this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_);
|
|
|
|
|
|
|
|
|
|
if (level > 0) {
|
|
|
|
|
for (axis=0; axis<numAxes_; axis++) {
|
|
|
|
|
pAxis = getAxis(axis);
|
|
|
|
|
fprintf(fp, " axis %d\n"
|
|
|
|
|
" bitsPerUnit_ = %f\n"
|
|
|
|
|
" encoder position=%f\n"
|
|
|
|
|
" theory position=%f\n"
|
|
|
|
|
" limits=0x%x\n",
|
|
|
|
|
pAxis->axisNo_, pAxis->bitsPerUnit_,
|
|
|
|
|
pAxis->encoderPosition_, pAxis->theoryPosition_,
|
|
|
|
|
pAxis->currentLimits_);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Call the base class method
|
|
|
|
|
asynMotorController::report(fp, level);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/** Returns a pointer to an C300MotorAxis object.
|
|
|
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
|
|
|
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
|
|
|
|
|
C300Axis* C300Controller::getAxis(asynUser *pasynUser)
|
|
|
|
|
{
|
|
|
|
|
return static_cast<C300Axis*>(asynMotorController::getAxis(pasynUser));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** Returns a pointer to an C300MotorAxis object.
|
|
|
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
|
|
|
* \param[in] axisNo Axis index number. */
|
|
|
|
|
C300Axis* C300Controller::getAxis(int axisNo)
|
|
|
|
|
{
|
|
|
|
|
return static_cast<C300Axis*>(asynMotorController::getAxis(axisNo));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/** Writes a string to the C300 controller.
|
|
|
|
|
* Calls writeController() with a default location of the string to write and a default timeout. */
|
|
|
|
|
asynStatus C300Controller::writeController()
|
|
|
|
|
{
|
|
|
|
|
return writeController(outString_, C300_TIMEOUT);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** Writes a string to the C300 controller.
|
|
|
|
|
* \param[in] output The string to be written.
|
|
|
|
|
* \param[in] timeout Timeout before returning an error.*/
|
|
|
|
|
asynStatus C300Controller::writeController(const char *output, double timeout)
|
|
|
|
|
{
|
|
|
|
|
size_t nwrite;
|
|
|
|
|
asynStatus status;
|
|
|
|
|
const char *functionName="writeController";
|
|
|
|
|
|
|
|
|
|
status = pasynOctetSyncIO->write(pasynUserC300_, output,
|
|
|
|
|
strlen(output), timeout, &nwrite);
|
|
|
|
|
|
|
|
|
|
asynPrint(pasynUserC300_, ASYN_TRACEIO_DRIVER,
|
|
|
|
|
"%s:%s: %s\n", driverName, functionName, output);
|
|
|
|
|
|
|
|
|
|
return status ;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** Writes a string to the C300 controller and reads the response.
|
|
|
|
|
* Calls writeReadController() with default locations of the input and output strings
|
|
|
|
|
* and default timeout. */
|
|
|
|
|
asynStatus C300Controller::writeReadController()
|
|
|
|
|
{
|
|
|
|
|
size_t nread;
|
|
|
|
|
return writeReadController(outString_, inString_, sizeof(inString_), &nread, C300_TIMEOUT);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** Writes a string to the C300 controller and reads a response.
|
|
|
|
|
* \param[in] output Pointer to the output string.
|
|
|
|
|
* \param[out] input Pointer to the input string location.
|
|
|
|
|
* \param[in] maxChars Size of the input buffer.
|
|
|
|
|
* \param[out] nread Number of characters read.
|
|
|
|
|
* \param[out] timeout Timeout before returning an error.*/
|
|
|
|
|
asynStatus C300Controller::writeReadController(const char *output, char *input, size_t maxChars, size_t *nread, double timeout)
|
|
|
|
|
{
|
|
|
|
|
size_t nwrite;
|
|
|
|
|
asynStatus status;
|
|
|
|
|
int eomReason;
|
|
|
|
|
const char *functionName="writeReadController";
|
|
|
|
|
|
|
|
|
|
status = pasynOctetSyncIO->writeRead(pasynUserC300_, output,
|
|
|
|
|
strlen(output), input, maxChars, timeout,
|
|
|
|
|
&nwrite, nread, &eomReason);
|
|
|
|
|
|
|
|
|
|
asynPrint(pasynUserC300_, ASYN_TRACEIO_DRIVER,
|
|
|
|
|
"%s:%s: output: %s input: %s\n", driverName, functionName, output, input);
|
|
|
|
|
|
|
|
|
|
return status;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// These are the C300Axis methods
|
|
|
|
|
|
|
|
|
|
/** Creates a new C300Axis object.
|
|
|
|
|
* \param[in] pC Pointer to the C300Controller to which this axis belongs.
|
|
|
|
|
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
|
|
|
|
|
*
|
|
|
|
|
* Initializes register numbers, etc.
|
|
|
|
|
*/
|
|
|
|
|
C300Axis::C300Axis(C300Controller *pC, int axisNo)
|
|
|
|
|
: asynMotorAxis(pC, axisNo),
|
|
|
|
|
pC_(pC)
|
|
|
|
|
{
|
|
|
|
|
asynStatus status;
|
|
|
|
|
|
|
|
|
|
sprintf(axisName_, "CHAN%d", axisNo+1);
|
|
|
|
|
|
|
|
|
|
// Get the range for this axis
|
|
|
|
|
sprintf(pC->outString_, "SYST:%s:STAG:RANG?", axisName_);
|
|
|
|
|
status = pC->writeReadController();
|
|
|
|
|
if (status) {
|
|
|
|
|
setIntegerParam(pC->motorStatusProblem_, 1);
|
|
|
|
|
} else {
|
|
|
|
|
// 2^20 bits per total range
|
|
|
|
|
bitsPerUnit_ = (1048576) / atof(pC->inString_);
|
|
|
|
|
// We assume servo motor with encoder for now
|
|
|
|
|
setIntegerParam(pC->motorStatusGainSupport_, 1);
|
|
|
|
|
setIntegerParam(pC->motorStatusHasEncoder_, 1);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Get the DI Factor for this axis
|
|
|
|
|
sprintf(pC->outString_, "SYST:%s:DI:FACT?", axisName_);
|
|
|
|
|
status = pC->writeReadController();
|
|
|
|
|
if (status) {
|
|
|
|
|
setIntegerParam(pC->motorStatusProblem_, 1);
|
|
|
|
|
} else {
|
|
|
|
|
//
|
|
|
|
|
diScaleFactor_ = atof(pC->inString_);
|
|
|
|
|
diScaleFactorInv_ = 1.0 / diScaleFactor_;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
callParamCallbacks();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
asynStatus C300Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
|
|
|
|
|
{
|
|
|
|
|
asynStatus status;
|
|
|
|
|
// static const char *functionName = "moveAxis";
|
|
|
|
|
|
|
|
|
|
if (relative) {
|
|
|
|
|
/*sprintf(pC_->outString_, "%s:POS %f", axisName_, (theoryPosition_ + position)/bitsPerUnit_);*/
|
|
|
|
|
sprintf(pC_->outString_, "%s:POS %f", axisName_, (theoryPosition_ + position));
|
|
|
|
|
status = pC_->writeController();
|
|
|
|
|
} else {
|
|
|
|
|
/*sprintf(pC_->outString_, "%s:POS %f", axisName_, position/bitsPerUnit_);*/
|
|
|
|
|
sprintf(pC_->outString_, "%s:POS %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 C300Axis::poll(bool *moving)
|
|
|
|
|
{
|
|
|
|
|
int done;
|
|
|
|
|
asynStatus comStatus;
|
|
|
|
|
|
|
|
|
|
// Read the current encoder position
|
|
|
|
|
sprintf(pC_->outString_, "%s:DATA?", axisName_);
|
|
|
|
|
comStatus = pC_->writeReadController();
|
|
|
|
|
if (comStatus) goto skip;
|
|
|
|
|
// encoder position is the 9th floating point value in a comma separated list
|
|
|
|
|
|
|
|
|
|
//printf("\naxisName_ = %s\n", axisName_);
|
|
|
|
|
//printf("data string = %s\n", pC_->inString_);
|
|
|
|
|
|
|
|
|
|
sscanf(&pC_->inString_[C300_DATA_OFFSET], "%lG", &posMonCorrectedVal_);
|
|
|
|
|
//printf("data string = %s\n", pC_->inString_);
|
|
|
|
|
sscanf(&pC_->inString_[C300_CORR_OFFSET], "%lG", &analogInScaledVal_);
|
|
|
|
|
//printf("data string = %s\n", pC_->inString_);
|
|
|
|
|
|
|
|
|
|
//printf("posMonCorrectedVal_ = %lG\n", posMonCorrectedVal_);
|
|
|
|
|
//printf("analogInScaledVal_ = %lG\n", analogInScaledVal_);
|
|
|
|
|
//printf("diScaleFactor_ = %f\n", diScaleFactor_);
|
|
|
|
|
//printf("diScaleFactorInv_ = %f\n", diScaleFactorInv_);
|
|
|
|
|
//printf("bitsPerUnit_ = %f\n", bitsPerUnit_);
|
|
|
|
|
/*encoderPosition_ = (posMonCorrectedVal_ - analogInScaledVal_) * -1.0 * diScaleFactorInv_;*/
|
|
|
|
|
encoderPosition_ = (posMonCorrectedVal_ - analogInScaledVal_) * diScaleFactorInv_;
|
|
|
|
|
//printf("encoderPosition_ = %f\n", encoderPosition_);
|
|
|
|
|
|
|
|
|
|
setDoubleParam(pC_->motorEncoderPosition_,encoderPosition_);
|
|
|
|
|
|
|
|
|
|
// Read the current theoretical position
|
|
|
|
|
sprintf(pC_->outString_, "%s:POS?", axisName_);
|
|
|
|
|
comStatus = pC_->writeReadController();
|
|
|
|
|
if (comStatus) goto skip;
|
|
|
|
|
theoryPosition_ = atof(pC_->inString_);
|
|
|
|
|
setDoubleParam(pC_->motorPosition_, theoryPosition_);
|
|
|
|
|
//printf("theoryPosition_ = %f\n", theoryPosition_);
|
|
|
|
|
|
|
|
|
|
// Read error status
|
|
|
|
|
|
|
|
|
|
// Simulate dmov functionality here
|
|
|
|
|
done = (fabs(theoryPosition_ - encoderPosition_) > C300Tolerance) ? 0 : 1;
|
|
|
|
|
setIntegerParam(pC_->motorStatusDone_, done);
|
|
|
|
|
*moving = done ? false:true;
|
|
|
|
|
|
|
|
|
|
// The limits will be fixed.
|
|
|
|
|
|
|
|
|
|
// Motor doesn't have limits or home switch (maybe move this to a constructor)
|
|
|
|
|
setIntegerParam(pC_->motorStatusHighLimit_, 0);
|
|
|
|
|
setIntegerParam(pC_->motorStatusLowLimit_, 0);
|
|
|
|
|
setIntegerParam(pC_->motorStatusAtHome_, 0);
|
|
|
|
|
|
|
|
|
|
skip:
|
|
|
|
|
setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0);
|
|
|
|
|
callParamCallbacks();
|
|
|
|
|
return comStatus ? asynError : asynSuccess;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/** Code for iocsh registration */
|
|
|
|
|
static const iocshArg C300CreateControllerArg0 = {"Port name", iocshArgString};
|
|
|
|
|
static const iocshArg C300CreateControllerArg1 = {"C300 port name", iocshArgString};
|
|
|
|
|
static const iocshArg C300CreateControllerArg2 = {"Number of axes", iocshArgInt};
|
|
|
|
|
static const iocshArg C300CreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt};
|
|
|
|
|
static const iocshArg C300CreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt};
|
|
|
|
|
static const iocshArg * const C300CreateControllerArgs[] = {&C300CreateControllerArg0,
|
|
|
|
|
&C300CreateControllerArg1,
|
|
|
|
|
&C300CreateControllerArg2,
|
|
|
|
|
&C300CreateControllerArg3,
|
|
|
|
|
&C300CreateControllerArg4};
|
|
|
|
|
static const iocshFuncDef C300CreateControllerDef = {"C300CreateController", 5, C300CreateControllerArgs};
|
|
|
|
|
static void C300CreateContollerCallFunc(const iocshArgBuf *args)
|
|
|
|
|
{
|
|
|
|
|
C300CreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static void C300MotorRegister(void)
|
|
|
|
|
{
|
|
|
|
|
iocshRegister(&C300CreateControllerDef, C300CreateContollerCallFunc);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
extern "C" {
|
|
|
|
|
epicsExportRegistrar(C300MotorRegister);
|
|
|
|
|
}
|
kpetersn