lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity

Added asyn, model-3 driver for the MVP2001.

Browse Source
This commit is contained in:
kmpeters
2013-08-14 22:14:00 +00:00
parent 9b3af6ab55
commit 9b2e2b346a
4 changed files with 585 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/MicroMoSrc/MVP2001Driver.cpp
+515
View File
@@ -0,0 +1,515 @@
/*
FILENAME... MVP2001Driver.cpp
USAGE... Motor driver support for the MicroMo MVP 2001 controller.
Kevin Peterson
August 14, 2013
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <iocsh.h>
#include <epicsThread.h>
#include <asynOctetSyncIO.h>
#include "MVP2001Driver.h"
#include <epicsExport.h>
#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5)
/** Creates a new MVP2001Controller object.
* \param[in] portName The name of the asyn port that will be created for this driver
* \param[in] MVP2001PortName The name of the drvAsynSerialPort that was created previously to connect to the MVP2001 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
*/
MVP2001Controller::MVP2001Controller(const char *portName, const char *MVP2001PortName, int numAxes,
double movingPollPeriod,double idlePollPeriod)
: asynMotorController(portName, numAxes, NUM_MVP2001_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
{
asynStatus status;
static const char *functionName = "MVP2001Controller::MVP2001Controller";
/* Connect to MVP2001 controller */
status = pasynOctetSyncIO->connect(MVP2001PortName, 0, &pasynUserController_, NULL);
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: cannot connect to MVP 2001 controller\n",
functionName);
}
// Don't create axes here; make the user configure them at boot time since additional information about the axes needs to be specified
startPoller(movingPollPeriod, idlePollPeriod, 2);
}
/** Creates a new MVP2001Controller 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] MVP2001PortName The name of the drvAsynIPPPort that was created previously to connect to the MVP2001 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 MVP2001CreateController(const char *portName, const char *MVP2001PortName, int numAxes,
int movingPollPeriod, int idlePollPeriod)
{
MVP2001Controller *pMVP2001Controller
= new MVP2001Controller(portName, MVP2001PortName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
pMVP2001Controller = 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 MVP2001Controller::report(FILE *fp, int level)
{
fprintf(fp, "MVP 2001 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 MVP2001Axis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
MVP2001Axis* MVP2001Controller::getAxis(asynUser *pasynUser)
{
return static_cast<MVP2001Axis*>(asynMotorController::getAxis(pasynUser));
}
/** Returns a pointer to an MVP2001Axis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] axisNo Axis index number. */
MVP2001Axis* MVP2001Controller::getAxis(int axisNo)
{
return static_cast<MVP2001Axis*>(asynMotorController::getAxis(axisNo));
}
/** Writes a string to the controller and reads the response.
* Calls writeRead2xController() with default locations of the input and output strings
* and default timeout. */
asynStatus MVP2001Controller::writeRead2xController()
{
size_t nread;
return writeRead2xController(outString_, inString_, sizeof(inString_), &nread, DEFAULT_CONTROLLER_TIMEOUT);
}
/* A custom read method is needed to handle the prepended terminator for commands
MVP2001 commands that generate a response. */
asynStatus MVP2001Controller::writeRead2xController(const char *output, char *input, size_t maxChars, size_t *nread, double timeout)
{
size_t nwrite;
asynStatus status;
int eomReason;
// const char *functionName="writeRead2xController";
// Write the command; read the prepended terminator
status = pasynOctetSyncIO->writeRead(pasynUserController_, output, strlen(output), input, maxChars, timeout, &nwrite, nread, &eomReason);
if (status) goto skip;
// A small delay is needed to avoid timeouts in the following read
epicsThreadSleep(0.033);
// Read the response
status = pasynOctetSyncIO->read(pasynUserController_, input, maxChars, timeout, nread, &eomReason);
skip:
return status;
}
void MVP2001Controller::parseReply(char *inString, int *val, int nchars)
{
buff_[0] = '\0';
// Controller responses are of the form "0001 FFFF"
strncat(buff_, &inString[5], nchars);
sscanf(buff_, "%x", val);
//*val = strtoul(buff_, NULL, 16);
}
// These are the MVP2001Axis methods
/** Creates a new MVP2001Axis object.
* \param[in] pC Pointer to the MVP2001Controller to which this axis belongs.
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
*
* Initializes register numbers, etc.
*/
MVP2001Axis::MVP2001Axis(MVP2001Controller *pC, int axisNo, int encLPR, int maxCurr)
: asynMotorAxis(pC, axisNo),
pC_(pC)
{
asynStatus status;
axisIndex_ = axisNo + 1;
encoderLinesPerRev_ = encLPR;
// The MVP2001 manual implies that the range is 0.1-2.3A is ok
if ( (maxCurr < 100) || maxCurr > 2300 )
maxCurrent_ = 100;
else
maxCurrent_ = maxCurr;
// Home the motor (this zeroes the position, which will be restored by autosave at iocInit)
sprintf(pC_->outString_, "%d HO", axisIndex_);
status = pC_->writeController();
/* The old driver enabled the motor here. Doing so causes problems.
Specifically, if enabling the motor causes the readback position to be
non-zero, then the autosaved position is NOT restored at iocInit */
//sprintf(pC_->outString_, "%d EN", axisIndex_);
//status = pC_->writeController();
// Query the MVP Loop sample period
sprintf(pC_->outString_, "%d SR", axisIndex_);
status = pC_->writeRead2xController();
if (status == asynSuccess)
pC_->parseReply(pC_->inString_, &samplePeriod_, 4);
else
samplePeriod_ = 500;
// Allow CNEN to turn motor power on/off
setIntegerParam(pC->motorStatusGainSupport_, 1);
setIntegerParam(pC->motorStatusHasEncoder_, 1);
}
extern "C" int MVP2001CreateAxis(const char *MVP2001Name, int axisNo, int encLPR, int maxCurr)
{
MVP2001Controller *pC;
pC = (MVP2001Controller*) findAsynPortDriver(MVP2001Name);
if (!pC)
{
printf("Error port %s not found\n", MVP2001Name);
return asynError;
}
pC->lock();
new MVP2001Axis(pC, axisNo, encLPR, maxCurr);
pC->unlock();
return asynSuccess;
}
/** 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 MVP2001Axis::report(FILE *fp, int level)
{
if (level > 0) {
fprintf(fp, " axis %d\n", axisNo_);
fprintf(fp, " axis index %d\n", axisIndex_);
fprintf(fp, " encoderLinesPerRev %d\n", encoderLinesPerRev_);
fprintf(fp, " maxCurrent %d (mA)\n", maxCurrent_);
fprintf(fp, " samplePeriod %d (us)\n", samplePeriod_);
}
// Call the base class method
asynMotorAxis::report(fp, level);
}
asynStatus MVP2001Axis::sendAccelAndVelocity(double acceleration, double velocity)
{
asynStatus status;
int ano;
int sp;
int ac;
// static const char *functionName = "MVP2001::sendAccelAndVelocity";
// Send the maximum current (equation determined from data in a graph in the MVP manual)
ano = NINT(maxCurrent_ * 0.865909 + 2103.431);
sprintf(pC_->outString_, "%d ANO %d", axisIndex_, ano);
status = pC_->writeController();
// Send the velocity
// TODO: explain the velocity calc
sp = NINT(samplePeriod_ * 6e-5 * velocity);
sprintf(pC_->outString_, "%d SP %d", axisIndex_, sp);
status = pC_->writeController();
// Send the acceleration
// TODO: explain thie acceleration calc
ac = NINT(7.5e-12 * samplePeriod_ * samplePeriod_ * encoderLinesPerRev_ * acceleration);
if (ac < sp)
{
// Don't allow negative accelerations
if (ac <= 0)
ac = 1;
}
else
{
// Don't let the acceleration exceed the speed
ac = sp;
}
sprintf(pC_->outString_, "%d AC %d", axisIndex_, ac);
status = pC_->writeController();
// Maybe put a goto earlier to go to here if not asynSuccess, but then need to do the same in methods that call this one
return status;
}
asynStatus MVP2001Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
{
asynStatus status;
// static const char *functionName = "MVP2001Axis::move";
status = sendAccelAndVelocity(acceleration, maxVelocity);
// Set the target position
if (relative) {
sprintf(pC_->outString_, "%d LR %d", axisIndex_, NINT(position));
} else {
sprintf(pC_->outString_, "%d LA %d", axisIndex_, NINT(position));
}
status = pC_->writeController();
// Send the 'move' command
sprintf(pC_->outString_, "%d M", axisIndex_);
status = pC_->writeController();
return status;
}
asynStatus MVP2001Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
{
// static const char *functionName = "MVP2001Axis::home";
// Homing wasn't implemented in the original driver.
// It could be implemented using the HA & HP commands.
return asynSuccess;
}
asynStatus MVP2001Axis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
{
asynStatus status;
int sp;
static const char *functionName = "MVP2001Axis::moveVelocity";
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
"%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n",
functionName, minVelocity, maxVelocity, acceleration);
// Call this to set the max current and acceleration
status = sendAccelAndVelocity(acceleration, maxVelocity);
// Calculate velocity
sp = NINT(samplePeriod_ * 6e-5 * maxVelocity);
sprintf(pC_->outString_, "%d V %d", axisIndex_, sp);
status = pC_->writeController();
return status;
}
asynStatus MVP2001Axis::stop(double acceleration )
{
asynStatus status;
//static const char *functionName = "MVP2001Axis::stop";
sprintf(pC_->outString_, "%d AB", axisIndex_);
status = pC_->writeController();
return status;
}
asynStatus MVP2001Axis::setPosition(double position)
{
asynStatus status;
//static const char *functionName = "MVP2001Axis::setPosition";
sprintf(pC_->outString_, "%d HO %d", axisIndex_, NINT(position));
status = pC_->writeController();
return status;
}
/** Set the proportional gain of the motor.
* \param[in] pGain The new proportional gain. */
asynStatus MVP2001Axis::setPGain(double pGain)
{
int ival;
asynStatus status;
ival = NINT(pGain * 28000 + 4000);
sprintf(pC_->outString_, "%d POR %d", axisIndex_, ival);
status = pC_->writeController();
return status;
}
/** Set the integral gain of the motor.
* \param[in] iGain The new integral gain. */
asynStatus MVP2001Axis::setIGain(double iGain)
{
int ival;
asynStatus status;
ival = NINT(iGain * 31999 + 1);
sprintf(pC_->outString_, "%d I %d", axisIndex_, ival);
status = pC_->writeController();
return status;
}
/** Set the derivative gain of the motor.
* \param[in] dGain The new derivative gain. */
asynStatus MVP2001Axis::setDGain(double dGain)
{
int ival;
asynStatus status;
ival = NINT(dGain * 31000 + 1000);
sprintf(pC_->outString_, "%d DER %d", axisIndex_, ival);
status = pC_->writeController();
return status;
}
asynStatus MVP2001Axis::setClosedLoop(bool closedLoop)
{
asynStatus status;
//static const char *functionName = "MVP2001Axis::setClosedLoop";
if (closedLoop)
{
// Send an AB here for EN to work (EN fails if status ends in 8, rather than E)
sprintf(pC_->outString_, "%d AB", axisIndex_);
status = pC_->writeController();
epicsThreadSleep(0.033);
sprintf(pC_->outString_, "%d EN", axisIndex_);
}
else
{
sprintf(pC_->outString_, "%d DI", axisIndex_);
}
status = pC_->writeController();
return 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 MVP2001Axis::poll(bool *moving)
{
int done;
int driveOn;
int limit;
int position;
int status;
asynStatus comStatus;
// Read the current motor position
sprintf(pC_->outString_, "%d POS", axisIndex_);
comStatus = pC_->writeRead2xController();
if (comStatus)
goto skip;
// The response string is of the form "0001 000001F4"
pC_->parseReply(pC_->inString_, &position, 8);
setDoubleParam(pC_->motorPosition_, position);
// Read the moving status of this motor
sprintf(pC_->outString_, "%d ST", axisIndex_);
comStatus = pC_->writeRead2xController();
if (comStatus)
goto skip;
// The response string is of the form "0001 0008"
pC_->parseReply(pC_->inString_, &status, 4);
// Set the direction bit in the move method instead of here since there isn't a direction bit, requires private readback position var
// Or set the direction bit here, requires a private target position var
done = (status & 0x1) ? 0 : 1;
setIntegerParam(pC_->motorStatusDone_, done);
*moving = done ? false:true;
// Read the limit status
limit = (status & 0x2000) ? 1 : 0;
setIntegerParam(pC_->motorStatusHighLimit_, limit);
limit = (status & 0x8000) ? 1 : 0;
setIntegerParam(pC_->motorStatusLowLimit_, limit);
// MVP2001 doesn't have a home status bit
// Read the drive power on status
driveOn = (status & 0x100) ? 0 : 1;
setIntegerParam(pC_->motorStatusPowerOn_, driveOn);
setIntegerParam(pC_->motorStatusProblem_, 0);
skip:
setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0);
callParamCallbacks();
return comStatus ? asynError : asynSuccess;
}
/** Code for iocsh registration */
static const iocshArg MVP2001CreateControllerArg0 = {"Port name", iocshArgString};
static const iocshArg MVP2001CreateControllerArg1 = {"MVP 2001 port name", iocshArgString};
static const iocshArg MVP2001CreateControllerArg2 = {"Number of axes", iocshArgInt};
static const iocshArg MVP2001CreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt};
static const iocshArg MVP2001CreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt};
static const iocshArg * const MVP2001CreateControllerArgs[] = {&MVP2001CreateControllerArg0,
&MVP2001CreateControllerArg1,
&MVP2001CreateControllerArg2,
&MVP2001CreateControllerArg3,
&MVP2001CreateControllerArg4};
static const iocshFuncDef MVP2001CreateControllerDef = {"MVP2001CreateController", 5, MVP2001CreateControllerArgs};
static void MVP2001CreateContollerCallFunc(const iocshArgBuf *args)
{
MVP2001CreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival);
}
/* MVP2001CreateAxis */
static const iocshArg MVP2001CreateAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg MVP2001CreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg MVP2001CreateAxisArg2 = {"Encoder lines per rev", iocshArgInt};
static const iocshArg MVP2001CreateAxisArg3 = {"Max current (ma)", iocshArgInt};
static const iocshArg * const MVP2001CreateAxisArgs[] = {&MVP2001CreateAxisArg0,
&MVP2001CreateAxisArg1,
&MVP2001CreateAxisArg2,
&MVP2001CreateAxisArg3};
static const iocshFuncDef MVP2001CreateAxisDef = {"MVP2001CreateAxis", 4, MVP2001CreateAxisArgs};
static void MVP2001CreateAxisCallFunc(const iocshArgBuf *args)
{
MVP2001CreateAxis(args[0].sval, args[1].ival, args[2].ival, args[3].ival);
}
static void MVP2001Register(void)
{
iocshRegister(&MVP2001CreateControllerDef, MVP2001CreateContollerCallFunc);
iocshRegister(&MVP2001CreateAxisDef, MVP2001CreateAxisCallFunc);
}
extern "C" {
epicsExportRegistrar(MVP2001Register);
}
motorApp/MicroMoSrc/MVP2001Driver.h
+64
View File
@@ -0,0 +1,64 @@
/*
FILENAME... MVP2001Driver.h
USAGE... Motor driver support for the MicroMo MVP2001 controller.
Kevin Peterson
August 14, 2013
*/
#include "asynMotorController.h"
#include "asynMotorAxis.h"
#define MAX_MVP2001_AXES 32 /* motor.h sets the maximum number of axes */
#define BUFF_SIZE 20 /* Maximum length of string to/from MVP2001 */
// No controller-specific parameters yet
#define NUM_MVP2001_PARAMS 0
class MVP2001Axis : public asynMotorAxis
{
public:
/* These are the methods we override from the base class */
MVP2001Axis(class MVP2001Controller *pC, int axisNo, int encLPR, int maxCurr);
void report(FILE *fp, int level);
asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration);
asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration);
asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards);
asynStatus stop(double acceleration);
asynStatus poll(bool *moving);
asynStatus setPosition(double position);
asynStatus setPGain(double pGain);
asynStatus setIGain(double iGain);
asynStatus setDGain(double dGain);
asynStatus setClosedLoop(bool closedLoop);
private:
MVP2001Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs.
* Abbreviated because it is used very frequently */
int axisIndex_;
double stepsPerRev_;
int encoderLinesPerRev_;
int maxCurrent_;
int samplePeriod_;
asynStatus sendAccelAndVelocity(double accel, double velocity);
friend class MVP2001Controller;
};
class MVP2001Controller : public asynMotorController {
public:
MVP2001Controller(const char *portName, const char *MVP2001PortName, int numAxes, double movingPollPeriod, double idlePollPeriod);
void report(FILE *fp, int level);
MVP2001Axis* getAxis(asynUser *pasynUser);
MVP2001Axis* getAxis(int axisNo);
private:
char buff_[BUFF_SIZE];
asynStatus writeRead2xController();
asynStatus writeRead2xController(const char *output, char *response, size_t maxResponseLen, size_t *responseLen, double timeout);
void parseReply(char *inString, int *val, int nchars);
friend class MVP2001Axis;
};
motorApp/MicroMoSrc/Makefile
+3 -1
View File
@@ -10,9 +10,11 @@ DBD += devMicroMo.dbd
LIBRARY_IOC = MicroMo
# Intelligent Motion Systems driver support.
# MVP2001 driver
SRCS += MicroMoRegister.cc
SRCS += devMVP2001.cc drvMVP2001.cc
# MVP2001 asyn model 3 driver
SRCS += MVP2001Driver.cpp
MicroMo_LIBS += motor asyn
MicroMo_LIBS += $(EPICS_BASE_IOC_LIBS)
motorApp/MicroMoSrc/devMicroMo.dbd
+3
View File
@@ -3,3 +3,6 @@ device(motor,VME_IO,devMVP2001,"MVP2001")
driver(drvMVP2001)
#variable(drvMVP2001debug)
# Model 3 driver
registrar(MVP2001Register)