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

Removed MicroMoSrc; Added motorMicroMo submodule

Browse Source
This commit is contained in:
kpetersn
2019-04-05 15:42:41 -05:00
parent 2481133ed3
commit ab0a01efb7
16 changed files with 5 additions and 2078 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitmodules
+3
View File
@@ -25,3 +25,6 @@
[submodule "modules/motorMclennan"]
path = modules/motorMclennan
url = https://github.com/epics-motor/motorMclennan.git
[submodule "modules/motorMicroMo"]
path = modules/motorMicroMo
url = https://github.com/epics-motor/motorMicroMo.git
iocBoot/iocWithAsyn/st.cmd.Vx
-15
View File
@@ -57,21 +57,6 @@ dbLoadRecords("$(MOTOR)/db/motorUtil.db", "P=IOC:")
#!asynOctetSetOutputEos("a-Serial[0]",0,"\r")
#!drvMCB4BDebug=4
# MicroMo MVP2001 driver setup parameters:
#
# NOTE: The 1st controller on each chain should have it's address = 1.
# The rest of the controllers on a chain should follow sequentially.
#
#
# int MVP2001Setup(int num_cards, /* number of CHAINS of controllers */
# int scan_rate) /* polling rate (Min=1Hz, max=60Hz) */
#!MVP2001Setup(1, 10)
# int MVP2001Config(int card, /* CHAIN being configured */
# (2) ASYN port name
#!MVP2001Config(0, "a-Serial[0]")
#!drvMVP2001debug=4
# PI C-844 driver setup parameters:
# (1) maximum number of controllers in system
# (2) motor task polling rate (min=1Hz,max=60Hz)
iocBoot/iocWithAsyn/st.cmd.unix
-15
View File
@@ -50,21 +50,6 @@ asynSetOption("L0", -1, "crtscts", "N")
#!asynOctetSetOutputEos("L0",0,"\r")
#!var drvMCB4BDebug 4
# MicroMo MVP2001 driver setup parameters:
#
# NOTE: The 1st controller on each chain should have it's address = 1.
# The rest of the controllers on a chain should follow sequentially.
#
#
# int MVP2001Setup(int num_cards, /* number of CHAINS of controllers */
# int scan_rate) /* polling rate (Min=1Hz, max=60Hz) */
#!MVP2001Setup(1, 10)
# int MVP2001Config(int card, /* CHAIN being configured */
# (2) ASYN port name
#!MVP2001Config(0, "L0")
#!drvMVP2001debug=4
# PI C-844 driver setup parameters:
# (1) maximum number of controllers in system
# (2) motor task polling rate (min=1Hz,max=60Hz)
iocBoot/iocWithAsyn/st.cmd.win32
-15
View File
@@ -39,21 +39,6 @@ dbLoadRecords("$(MOTOR)/db/motorUtil.db", "P=IOC:")
#!asynOctetSetOutputEos("L0",0,"\r")
#!var drvMCB4BDebug 4
# MicroMo MVP2001 driver setup parameters:
#
# NOTE: The 1st controller on each chain should have it's address = 1.
# The rest of the controllers on a chain should follow sequentially.
#
#
# int MVP2001Setup(int num_cards, /* number of CHAINS of controllers */
# int scan_rate) /* polling rate (Min=1Hz, max=60Hz) */
#!MVP2001Setup(1, 10)
# int MVP2001Config(int card, /* CHAIN being configured */
# (2) ASYN port name
#!MVP2001Config(0, "L0")
#!drvMVP2001debug=4
# PI C-844 driver setup parameters:
# (1) maximum number of controllers in system
# (2) motor task polling rate (min=1Hz,max=60Hz)
modules/Makefile
+1
View File
@@ -14,6 +14,7 @@ SUBMODULES += motorHytec
endif
SUBMODULES += motorKohzu
SUBMODULES += motorMclennan
SUBMODULES += motorMicroMo
# Allow sites to add extra submodules
-include Makefile.local
modules/motorMicroMo
Submodule
+1
Submodule modules/motorMicroMo added at de19d95f67
motorApp/Makefile
-3
View File
@@ -33,9 +33,6 @@ PiSrc_DEPEND_DIRS = MotorSrc
DIRS += PIGCS2Src
PIGCS2Src_DEPEND_DIRS = MotorSrc
DIRS += MicroMoSrc
MicroMoSrc_DEPEND_DIRS = MotorSrc
DIRS += MicosSrc
MicosSrc_DEPEND_DIRS = MotorSrc
motorApp/MicroMoSrc/MVP2001Driver.cpp
-531
View File
@@ -1,531 +0,0 @@
/*
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 "asynMotorController.h"
#include "asynMotorAxis.h"
#include <epicsExport.h>
#include "MVP2001Driver.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, int limPol)
: 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;
// Default to NO if a bad value is given (1=NO, 0=NC)
if ( limPol != 0 )
limitPolarity_ = 1;
else
limitPolarity_ = 0;
// Set the limit & estop polarity
sprintf(pC_->outString_, "%d LP %d", axisIndex_, limitPolarity_);
status = pC_->writeController();
// 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, int limPol)
{
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, limPol);
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_);
fprintf(fp, " limitPolarity %d\n", limitPolarity_);
}
// 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 MVP2001CreateAxisArg4 = {"Limit polarity", iocshArgInt};
static const iocshArg * const MVP2001CreateAxisArgs[] = {&MVP2001CreateAxisArg0,
&MVP2001CreateAxisArg1,
&MVP2001CreateAxisArg2,
&MVP2001CreateAxisArg3,
&MVP2001CreateAxisArg4};
static const iocshFuncDef MVP2001CreateAxisDef = {"MVP2001CreateAxis", 5, MVP2001CreateAxisArgs};
static void MVP2001CreateAxisCallFunc(const iocshArgBuf *args)
{
MVP2001CreateAxis(args[0].sval, args[1].ival, args[2].ival, args[3].ival, args[4].ival);
}
static void MVP2001Register(void)
{
iocshRegister(&MVP2001CreateControllerDef, MVP2001CreateContollerCallFunc);
iocshRegister(&MVP2001CreateAxisDef, MVP2001CreateAxisCallFunc);
}
extern "C" {
epicsExportRegistrar(MVP2001Register);
}
motorApp/MicroMoSrc/MVP2001Driver.h
-65
View File
@@ -1,65 +0,0 @@
/*
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 epicsShareClass MVP2001Axis : public asynMotorAxis
{
public:
/* These are the methods we override from the base class */
MVP2001Axis(class MVP2001Controller *pC, int axisNo, int encLPR, int maxCurr, int limPol);
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_;
int limitPolarity_;
asynStatus sendAccelAndVelocity(double accel, double velocity);
friend class MVP2001Controller;
};
class epicsShareClass 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
-23
View File
@@ -1,23 +0,0 @@
# Makefile
TOP = ../..
include $(TOP)/configure/CONFIG
# Both the following line, and a line in the *.dbd file,
# must be uncommented to use diagnostic debugging messages.
#!USR_CXXFLAGS += -DDEBUG
DBD += devMicroMo.dbd
LIBRARY_IOC = MicroMo
# 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)
include $(TOP)/configure/RULES
motorApp/MicroMoSrc/MicroMoRegister.cc
-64
View File
@@ -1,64 +0,0 @@
/*
FILENAME... MicroMoRegister.cc
USAGE... Register MicroMo MVP 2001 B02 motor controller device driver shell
commands.
*/
/*****************************************************************
COPYRIGHT NOTIFICATION
*****************************************************************
(C) COPYRIGHT 1993 UNIVERSITY OF CHICAGO
This software was developed under a United States Government license
described on the COPYRIGHT_UniversityOfChicago file included as part
of this distribution.
**********************************************************************/
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
#include <iocsh.h>
#include "motor.h"
#include "drvMVP2001.h"
#include "epicsExport.h"
extern "C"
{
// MicroMo Setup arguments
static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt};
static const iocshArg setupArg1 = {"Polling rate", iocshArgInt};
// MicroMo Config arguments
static const iocshArg configArg0 = {"Card being configured", iocshArgInt};
static const iocshArg configArg1 = {"asyn port name", iocshArgString};
static const iocshArg * const MicroMoSetupArgs[2] = {&setupArg0, &setupArg1};
static const iocshArg * const MicroMoConfigArgs[3] = {&configArg0, &configArg1};
static const iocshFuncDef setupMicroMo = {"MicroMoSetup", 2, MicroMoSetupArgs};
static const iocshFuncDef configMicroMo = {"MicroMoConfig", 2, MicroMoConfigArgs};
static void setupMicroMoCallFunc(const iocshArgBuf *args)
{
MVP2001Setup(args[0].ival, args[1].ival);
}
static void configMicroMoCallFunc(const iocshArgBuf *args)
{
MVP2001Config(args[0].ival, args[1].sval);
}
static void MicroMomotorRegister(void)
{
iocshRegister(&setupMicroMo, setupMicroMoCallFunc);
}
epicsExportRegistrar(MicroMomotorRegister);
} // extern "C"
motorApp/MicroMoSrc/devMVP2001.cc
-474
View File
@@ -1,474 +0,0 @@
/*
FILENAME... devMVP2001.cc
USAGE... Motor record device level support for MicroMo
MVP 2001 B02 (Linear, RS-485).
*/
/*
* Original Author: Kevin Peterson
* Date: 08/27/2002
*
*
* Illinois Open Source License
* University of Illinois
* Open Source License
*
*
* Copyright (c) 2004, UNICAT. All rights reserved.
*
*
* Developed by:
*
* UNICAT, Advanced Photon Source, Argonne National Laboratory
*
* Frederick Seitz Materials Research Laboratory,
* University of Illinois at Urbana-Champaign
*
* http://www.uni.aps.anl.gov
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the
* "Software"), to deal with the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
*
* Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimers.
*
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimers in the
* documentation and/or other materials provided with the distribution.
*
*
* Neither the names of UNICAT, Frederick Seitz Materials Research
* Laboratory, University of Illinois at Urbana-Champaign,
* nor the names of its contributors may be used to endorse or promote
* products derived from this Software without specific prior written
* permission.
*
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL THE CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR
* ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
* SOFTWARE OR THE USE OR OTHER DEALINGS WITH THE SOFTWARE.
*
*
* Modification Log:
* -----------------
* .01 08/27/02 kmp copied from devIM483PL.c (rev 1.9, mod .04) and
* customized for the MVP2001.
* .02 08/27/02 kmp changed message construction to allow for addresses
* larger than 9.
* .03 08/29/02 kmp fixed the sending of the ANO command. Previously had
* attempted to send it during "case GO:" but now properly
* handled using the mr->prem. Also implemented JOG.
* .04 08/30/02 kmp fixed problem with HOMF & HOMR. The MVP2001 does not
* home commands. The correct way to disable the HOMF &
* HOMR is to simply break from the switch statement.
* Doing anything else (i.e. "send = OFF;" or
* "trans->state = IDLE_STATE;") causes the MEDM window
* to get stuck in the "Moving" state even though the motor
* is not moving. Use a current version of MEDM.
* .05 10/02/02 kmp changed default current limit (in case where desired
* current limit is out of range) from 500mA to 100mA.
* .06 02/13/04 rls port to R3.14.x
* .07 06/06/08 rls Bug fix initializing driver twice.
* .08 03/18/11 kmp Corrected the MVP2001_table structure; only the "GO"
* command causes the motor to move.
*
*/
#include <string.h>
#include <math.h>
#include <ctype.h>
#include <stdlib.h>
#include <errlog.h>
#include "motorRecord.h"
#include "motor.h"
#include "motordevCom.h"
#include "drvMVP2001.h"
#include "epicsExport.h"
extern struct driver_table MVP2001_access;
#define BUFF_SIZE 20 /* Maximum length of string to/from MVP2001 */
/* ----------------Create the dsets for devMVP2001----------------- */
static struct driver_table *drvtabptr;
static long MVP2001_init(int);
static long MVP2001_init_record(void *);
static long MVP2001_start_trans(struct motorRecord *);
static RTN_STATUS MVP2001_build_trans(motor_cmnd, double *, struct motorRecord *);
static RTN_STATUS MVP2001_end_trans(struct motorRecord *);
struct motor_dset devMVP2001 =
{
{8, NULL, (DEVSUPFUN) MVP2001_init, (DEVSUPFUN) MVP2001_init_record, NULL},
motor_update_values,
MVP2001_start_trans,
MVP2001_build_trans,
MVP2001_end_trans
};
extern "C"{epicsExportAddress(dset, devMVP2001);}
/* --------------------------- program data --------------------- */
/* This table is used to define the command types */
/* WARNING! this must match "motor_cmnd" in motor.h */
static msg_types MVP2001_table[] = {
IMMEDIATE, /* MOVE_ABS */
IMMEDIATE, /* MOVE_REL */
IMMEDIATE, /* HOME_FOR */
IMMEDIATE, /* HOME_REV */
IMMEDIATE, /* LOAD_POS */
IMMEDIATE, /* SET_VEL_BASE */
IMMEDIATE, /* SET_VELOCITY */
IMMEDIATE, /* SET_ACCEL */
MOTION, /* GO */
IMMEDIATE, /* SET_ENC_RATIO */
INFO, /* GET_INFO */
MOVE_TERM, /* STOP_AXIS */
VELOCITY, /* JOG */
IMMEDIATE, /* SET_PGAIN */
IMMEDIATE, /* SET_IGAIN */
IMMEDIATE, /* SET_DGAIN */
IMMEDIATE, /* ENABLE_TORQUE */
IMMEDIATE, /* DISABL_TORQUE */
IMMEDIATE, /* PRIMITIVE */
IMMEDIATE, /* SET_HIGH_LIMIT */
IMMEDIATE, /* SET_LOW_LIMIT */
VELOCITY /* JOG_VELOCITY */
};
static struct board_stat **MVP2001_cards;
/* --------------------------- program data --------------------- */
/* initialize device support for MVP2001 DC motor */
static long MVP2001_init(int after)
{
long rtnval;
drvtabptr = &MVP2001_access;
rtnval = motor_init_com(after, *drvtabptr->cardcnt_ptr, drvtabptr, &MVP2001_cards);
return(rtnval);
}
/* initialize a record instance */
static long MVP2001_init_record(void *arg)
{
struct motorRecord *mr = (struct motorRecord *) arg;
return(motor_init_record_com(mr, *drvtabptr->cardcnt_ptr, drvtabptr, MVP2001_cards));
}
/* start building a transaction */
static long MVP2001_start_trans(struct motorRecord *mr)
{
return(OK);
}
/* end building a transaction */
static RTN_STATUS MVP2001_end_trans(struct motorRecord *mr)
{
return(OK);
}
/* add a part to the transaction */
static RTN_STATUS MVP2001_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr)
{
struct motor_trans *trans = (struct motor_trans *) mr->dpvt;
struct mess_node *motor_call;
struct controller *brdptr;
struct mess_info *motor_info;
struct MVPcontroller *cntrl;
char buff[BUFF_SIZE], enc_cpr[10], max_mA[6], parm[31];
char prem[3] = {'A', 'N', 'O'};
int card, axis;
unsigned int size;
int sp, ac, i, j, ano;
RTN_STATUS rtnval;
epicsInt32 cntrl_units = 0;
double dval;
bool send;
send = true; /* Default to send motor command. */
rtnval = OK;
buff[0] = '\0';
dval = *parms;
motor_start_trans_com(mr, MVP2001_cards);
motor_call = &(trans->motor_call);
card = motor_call->card;
axis = motor_call->signal + 1;
brdptr = (*trans->tabptr->card_array)[card];
if (brdptr == NULL)
return(rtnval = ERROR);
cntrl = (struct MVPcontroller *) brdptr->DevicePrivate;
cntrl_units = (epicsInt32) NINT(dval);
if (MVP2001_table[command] > motor_call->type)
motor_call->type = MVP2001_table[command];
if (trans->state != BUILD_STATE)
return(rtnval = ERROR);
if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0)
{
sprintf(buff, "%d %s", axis, mr->init);
strcpy(motor_call->message, buff);
buff[0] = '\0';
rtnval = motor_end_trans_com(mr, drvtabptr);
rtnval = (RTN_STATUS) motor_start_trans_com(mr, MVP2001_cards);
motor_call->type = MVP2001_table[command];
}
switch (command)
{
case MOVE_ABS:
case MOVE_REL:
case JOG:
if (strlen(mr->prem) != 0)
{
if (strncmp(mr->prem, prem, 3) == 0)
{
/*
* If the current limit has not already been calculated, the
* calculation is performed using the current limit specified
* in the parm string of the out field (in mA). The parm string
* has the form: "ENC_CPR,MAX_CURRENT_in_mA"
*/
if (cntrl->maxCurrent[axis - 1] == 0)
{
i = 0;
strcpy(parm, mr->out.value.vmeio.parm);
/* get the max current from the parm string */
while (parm[i] != ',')
{
i++;
}
i++;
j = i;
while (isdigit(parm[i]))
{
max_mA[i - j] = parm[i];
i++;
}
max_mA[i - j] = '\0';
cntrl->maxCurrent[axis - 1] = atoi(max_mA);
}
/* The MVP2001 manual implies that the range 0.1-2.3 Amps is ok */
if (cntrl->maxCurrent[axis - 1] < 100 || cntrl->maxCurrent[axis - 1] > 2300)
cntrl->maxCurrent[axis - 1] = 100;
/*
* The values in the ano calc are determined from data in the
* MVP manual
*/
ano = NINT(cntrl->maxCurrent[axis - 1] * 0.865909 + 2103.431);
sprintf(buff, "%d ANO %d", axis, ano);
}
else
{
sprintf(buff, "%d %s", axis, mr->prem);
}
strcpy(motor_call->message, buff);
buff[0] = '\0';
/* end ANO command trans because MVP can't handle multiple commands */
rtnval = motor_end_trans_com(mr, drvtabptr);
/* begin the original transaction again (command) */
rtnval = (RTN_STATUS) motor_start_trans_com(mr, MVP2001_cards);
motor_call->type = MVP2001_table[command];
}
/*
* The following probably will not work for the MVP2001 as the mr->post
* field is not ready to be sent in it's primitive form
*/
if (strlen(mr->post) != 0)
motor_call->postmsgptr = (char *) &mr->post;
break;
case HOME_FOR:
case HOME_REV:
default:
break;
}
switch (command)
{
case MOVE_ABS:
sprintf(buff, "%d LA %d", axis, cntrl_units);
break;
case MOVE_REL:
sprintf(buff, "%d LR %d", axis, cntrl_units);
break;
case HOME_FOR:
case HOME_REV:
break;
case LOAD_POS:
sprintf(buff, "%d HO %d", axis, cntrl_units);
break;
case SET_VEL_BASE:
send = false;
break;
case SET_VELOCITY:
cntrl_units = NINT(dval * 0.03);
sprintf(buff, "%d SP %d", axis, cntrl_units);
break;
/*
* The calculation of the acceleration requires the number of encoder
* counts per revolution. This value is passed in through the parm
* string of the out field. the parm string has the form:
* "ENC_CPR,MAX_CURRENT_in_mA"
*/
case SET_ACCEL:
/*
* If the Encoder counts per revolution has already been calculated and
* stored into the device private structure, then it will not be
* recalculated.
*/
if (cntrl->encoderCpr[axis - 1] == 0)
{
i = 0;
strcpy(parm, mr->out.value.vmeio.parm);
/* get the encoder cpr from the parm string */
while (isdigit(parm[i]))
{
enc_cpr[i] = parm[i];
i++;
}
enc_cpr[i] = '\0';
cntrl->encoderCpr[axis - 1] = atoi(enc_cpr);
}
sp = NINT(mr->velo / fabs(mr->mres) * 0.03);
ac = NINT(dval * 0.03 * cntrl->encoderCpr[axis - 1] * 0.0000625);
if (ac < sp)
{
if (ac > 0)
cntrl_units = ac;
else
cntrl_units = 1;
}
else
{
cntrl_units = sp;
}
sprintf(buff, "%d AC %d", axis, cntrl_units);
break;
case GO:
sprintf(buff, "%d M", axis);
break;
case PRIMITIVE:
case GET_INFO:
/*
* These commands are not actually done by sending a message, but
* rather they will indirectly cause the driver to read the status of
* all motors
*/
break;
case STOP_AXIS:
sprintf(buff, "%d AB", axis);
break;
case JOG_VELOCITY:
case JOG:
cntrl_units = NINT(dval * 0.03);
sprintf(buff, "%d V %d", axis, cntrl_units);
break;
case SET_PGAIN:
sprintf(buff, "%d POR %ld", axis, (NINT(dval * 28000 + 4000)));
break;
case SET_IGAIN:
sprintf(buff, "%d I %ld", axis, (NINT(dval * 31999 + 1)));
break;
case SET_DGAIN:
sprintf(buff, "%d DER %ld", axis, (NINT(dval * 31000 + 1000)));
break;
case ENABLE_TORQUE:
sprintf(buff, "%d EN", axis);
break;
case DISABL_TORQUE:
sprintf(buff, "%d DI", axis);
break;
case SET_HIGH_LIMIT:
motor_info = &(*trans->tabptr->card_array)[card]->motor_info[axis - 1];
trans->state = IDLE_STATE; /* No command sent to the controller. */
if (cntrl_units > motor_info->high_limit)
{
mr->dhlm = motor_info->high_limit * fabs(mr->mres);
rtnval = ERROR;
}
break;
case SET_LOW_LIMIT:
motor_info = &(*trans->tabptr->card_array)[card]->motor_info[axis - 1];
trans->state = IDLE_STATE; /* No command sent to the controller. */
if (cntrl_units < motor_info->low_limit)
{
mr->dllm = motor_info->low_limit * fabs(mr->mres);
rtnval = ERROR;
}
break;
case SET_ENC_RATIO:
trans->state = IDLE_STATE; /* No command sent to the controller. */
send = false;
break;
default:
send = false;
rtnval = ERROR;
}
size = strlen(buff);
if (send == false)
return(rtnval);
else if (size > sizeof(buff) || (strlen(motor_call->message) + size) > MAX_MSG_SIZE)
errlogMessage("MVP2001_build_trans(): buffer overflow.\n");
else
{
strcat(motor_call->message, buff);
motor_end_trans_com(mr, drvtabptr);
}
return(rtnval);
}
motorApp/MicroMoSrc/devMicroMo.dbd
-8
View File
@@ -1,8 +0,0 @@
# Micro Mo MVP 2001 Driver support.
device(motor,VME_IO,devMVP2001,"MVP2001")
driver(drvMVP2001)
#variable(drvMVP2001debug)
# Model 3 driver
registrar(MVP2001Register)
motorApp/MicroMoSrc/drvMVP2001.cc
-741
View File
@@ -1,741 +0,0 @@
/*
FILENAME... drvMVP2001.cc
USAGE... Motor record driver level support for MicroMo
MVP 2001 B02 (Linear, RS-485).
*/
/*
* Original Author: Kevin Peterson
* Date: 08/27/2002
*
*
* Illinois Open Source License
* University of Illinois
* Open Source License
*
*
* Copyright (c) 2004, UNICAT. All rights reserved.
*
*
* Developed by:
*
* UNICAT, Advanced Photon Source, Argonne National Laboratory
*
* Frederick Seitz Materials Research Laboratory,
* University of Illinois at Urbana-Champaign
*
* http://www.uni.aps.anl.gov
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the
* "Software"), to deal with the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
*
* Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimers.
*
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimers in the
* documentation and/or other materials provided with the distribution.
*
*
* Neither the names of UNICAT, Frederick Seitz Materials Research
* Laboratory, University of Illinois at Urbana-Champaign,
* nor the names of its contributors may be used to endorse or promote
* products derived from this Software without specific prior written
* permission.
*
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL THE CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR
* ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
* SOFTWARE OR THE USE OR OTHER DEALINGS WITH THE SOFTWARE.
*
*
* Modification Log:
* -----------------
* .01 08/27/02 kmp copied from drvIM483PL.c (rev 1.7, mod .03) and
* customized for the MVP2001.
* .02 08/27/02 kmp changed message construction to allow for addresses
* larger than 9.
* .03 09/06/02 kmp added an extra loop to motor_init() that sends the HO
* command a second time to ensure the position is set to
* zero. Previously, saved positions would not be loaded
* if the controller was power-cycled.
* .04 02/06/04 rls Eliminate erroneous "Motor motion timeout ERROR".
* .05 02/13/04 rls port to R3.14.x
* .06 07/12/04 rls Converted from MPF to asyn.
* .07 12/16/04 rls - asyn R4.0 support.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
* - retry on initial communication.
* .08 10/02/06 rls - Bug in recv_mess(); always returned nread=0.
* .09 06/06/08 rls - Bug fix setting RA_DONE based on inMotion.
* .10 06/09/08 rls - Controller workaround for comm. errors; delay after
* each messages received for 0.1 second.
* .11 03/18/11 kmp Set status.Bits.EA_POSITION properly in set_status() so
* the torque enable/disable button is accurate.
*/
/*
DESIGN LIMITATIONS...
1 - Like all controllers, the MVP2001 must be powered-on when EPICS is first
booted up.
2 - The MVP2001 cannot be power cycled while EPICS is up and running. The
consequences are permanent communication loss with the MVP2001 until
EPICS is rebooted.
3 - Translation between the MVP2001 and the ACCL/BACC fields is not obvious.
*/
/*
MORE DESIGN LIMITATIONS
1 - For the most part the standard terminology (card-signal-axis) has been
used here so that this code resembles other drivers. Unfortunately,
the terminology is not the best for this controller. The MVP2001 is a
single-axis, RS-485-daisy-chainable, DC controller. The following
equations succinctly illustrate the relationship between the physical
setup and the standard terminology:
card = chain of MVP2001 controllers
signal = axis = one of the MVP2001 controllers on a chain
2 - Strtol and strtoul have been switched in the vxWorks that KMP used
at the time that this was being written. If your vxWorks functions
behave correctly, then they will have to be switched in the code.
3 - Factors that currently limit the number of controllers on one chain:
A. MVP2001 addresses
The MVP2001 can have an address of 1-64 for serial communication.
B. RS-485 communication degradation
There is a practical limit to how many controllers can be on one chain
C. The motor_info array of the controller structure in motordrvCom.h
For a chain to work correctly, there needs to be one element in the
motor_info array for every controller on the chain. The limit is
set by the constant MAX_AXIS, which is defined in motor.h. The end
result is that the number of controllers is limited by the motor
record. The current maximum number of controllers is 10.
*/
#include <string.h>
#include <epicsThread.h>
#include <drvSup.h>
#include <stdlib.h>
#include <errlog.h>
#include "motor.h"
#include "drvMVP2001.h"
#include "epicsExport.h"
#define MVP2001_NUM_CARDS 8
#define BUFF_SIZE 20 /* Maximum length of string to/from MVP2001 */
/*----------------debugging-----------------*/
volatile int drvMVP2001debug = 0;
extern "C" {epicsExportAddress(int, drvMVP2001debug);}
static inline void Debug(int level, const char *format, ...) {
#ifdef DEBUG
if (level < drvMVP2001debug) {
va_list pVar;
va_start(pVar, format);
vprintf(format, pVar);
va_end(pVar);
}
#endif
}
/* --- Local data. --- */
int MVP2001_num_cards = 0;
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
/*----------------functions-----------------*/
static int recv_mess(int, char *, int);
static RTN_STATUS send_mess(int, char const *, char *);
static int set_status(int, int);
static long report(int);
static long init();
static int motor_init();
static void query_done(int, int, struct mess_node *);
/*----------------functions-----------------*/
struct driver_table MVP2001_access =
{
motor_init,
motor_send,
motor_free,
motor_card_info,
motor_axis_info,
&mess_queue,
&queue_lock,
&free_list,
&freelist_lock,
&motor_sem,
&motor_state,
&total_cards,
&any_motor_in_motion,
send_mess,
recv_mess,
set_status,
query_done,
NULL,
&initialized,
NULL
};
struct drvMVP2001_drvet
{
long number;
long (*report) (int);
long (*init) (void);
} drvMVP2001 = {2, report, init};
extern "C" {epicsExportAddress(drvet, drvMVP2001);}
static struct thread_args targs = {SCAN_RATE, &MVP2001_access, 0.0};
/*********************************************************
* Print out driver status report
*********************************************************/
static long report(int level)
{
int card;
if (MVP2001_num_cards <= 0)
printf(" No MVP2001 CHAINS configured.\n");
else
{
for (card = 0; card < MVP2001_num_cards; card++)
{
struct controller *brdptr = motor_state[card];
if (brdptr == NULL)
printf(" MVP2001 controller chain #%d connection failed.\n", card);
else
{
struct MVPcontroller *cntrl;
cntrl = (struct MVPcontroller *) brdptr->DevicePrivate;
printf(" MVP2001 controller chain #%d, port=%s, id: %s \n", card,
cntrl->asyn_port, brdptr->ident);
}
}
}
return(0);
}
static long init()
{
/* initialize all hardware and software */
motor_init();
/* Check for setup */
if (MVP2001_num_cards <= 0)
{
Debug(1, "init(): MVP2001 driver disabled. MVP2001Setup() missing from startup script.\n");
}
return((long) 0);
}
static void query_done(int card, int axis, struct mess_node *nodeptr)
{
}
/********************************************************************************
* *
* FUNCTION NAME: set_status *
* *
* LOGIC: *
* Initialize. *
* Send "Moving Status" query. *
* Read response. *
* IF normal response to query. *
* Set communication status to NORMAL. *
* ELSE *
* IF communication status is NORMAL. *
* Set communication status to RETRY. *
* NORMAL EXIT. *
* ELSE *
* Set communication status error. *
* ERROR EXIT. *
* ENDIF *
* ENDIF *
* *
* IF "Moving Status" indicates any motion (i.e. status != 0). *
* Clear "Done Moving" status bit. *
* ELSE *
* Set "Done Moving" status bit. *
* ENDIF *
* *
* *
********************************************************************************/
/*
* When the motor record calls set_status it passes it card and signal values
* that are found in the mvpMotors template (or the OUT field of the M.R.)
*/
static int set_status(int card, int signal)
{
struct MVPcontroller *cntrl;
struct mess_node *nodeptr;
register struct mess_info *motor_info;
/* Message parsing variables */
char buff[BUFF_SIZE];
char statusStr[BUFF_SIZE], positionStr[BUFF_SIZE];
int rtn_state;
epicsInt32 motorData;
MOTOR_STATUS mstat;
bool plusdir, ls_active = false;
msta_field status;
cntrl = (struct MVPcontroller *) motor_state[card]->DevicePrivate;
motor_info = &(motor_state[card]->motor_info[signal]);
nodeptr = motor_info->motor_motion;
status.All = motor_info->status.All;
statusStr[0] = positionStr[0] = buff[0] = '\0';
sprintf(buff, "%d ST", (signal + 1));
send_mess(card, buff, (char*) NULL);
rtn_state = recv_mess(card, buff, 1);
if (rtn_state > 0)
{
cntrl->status = NORMAL;
status.Bits.CNTRL_COMM_ERR = 0;
}
else
{
if (cntrl->status == NORMAL)
{
cntrl->status = RETRY;
rtn_state = 0;
goto exit;
}
else
{
cntrl->status = COMM_ERR;
status.Bits.CNTRL_COMM_ERR = 1;
status.Bits.RA_PROBLEM = 1;
rtn_state = 1;
goto exit;
}
}
/*
* Parse status string Status string format: 0001 FFFF Skip to status
* substring for this motor, convert from hex to int
*/
strncat(statusStr, &buff[5], 4);
mstat.All = strtoul(statusStr, NULL, 16);
buff[0] = '\0';
status.Bits.RA_DONE = !mstat.Bits.inMotion;
sprintf(buff, "%d POS", (signal + 1));
send_mess(card, buff, (char*) NULL);
recv_mess(card, buff, 1);
/*
* Parse motor position Position string format: 0001 FFFFFFFF Skip to
* position substring for this motor, convert from hex to int
*/
strncat(positionStr, &buff[5], 8);
motorData = (epicsInt32) strtoul(positionStr, NULL, 16);
buff[0] = '\0';
/*
* Set direction by comparing positions since the MVP2001 does not have a
* direction bit.
*/
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
epicsInt32 newposition;
newposition = NINT(motorData);
status.Bits.RA_DIRECTION = (newposition >= motor_info->position) ? 1 : 0;
motor_info->position = newposition;
motor_info->no_motion_count = 0;
}
plusdir = (status.Bits.RA_DIRECTION) ? true : false;
/* Set limit switch error indicators. */
if (mstat.Bits.plusLS == false)
status.Bits.RA_PLUS_LS = 0;
else
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
if (mstat.Bits.minusLS == false)
status.Bits.RA_MINUS_LS = 0;
else
{
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
}
/* The MVP2001 doesn't have a home feature */
status.Bits.RA_HOME = 0;
/* Set the enable-torque PV properly */
status.Bits.EA_POSITION = (mstat.Bits.NOT_power == false) ? 1 : 0;
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
if (motor_state[card]->motor_info[signal].encoder_present == NO)
motor_info->encoder_position = 0;
else
{
/*
* There is not a seperate call for "encoder_position" as every call
* for the position of the DC motor reads the encoder.
*/
motor_info->encoder_position = motorData;
}
status.Bits.RA_PROBLEM = 0;
/* Parse motor velocity? */
/* NEEDS WORK */
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
/* Test for post-move string. */
if ((status.Bits.RA_DONE || ls_active == true) && nodeptr != 0 &&
nodeptr->postmsgptr != 0)
{
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, (char*) NULL);
nodeptr->postmsgptr = NULL;
}
exit:
motor_info->status.All = status.All;
return(rtn_state);
}
/*****************************************************/
/* send a message to the MVP2001 board */
/* send_mess() */
/*****************************************************/
static RTN_STATUS send_mess(int card, char const *com, char *name)
{
struct MVPcontroller *cntrl;
int size;
size_t nwrite;
size = strlen(com);
if (size > MAX_MSG_SIZE)
{
errlogMessage("drvMVP2001.c:send_mess(); message size violation.\n");
return(ERROR);
}
else if (size == 0) /* Normal exit on empty input message. */
return(OK);
if (!motor_state[card])
{
errlogPrintf("drvMVP2001.c:send_mess() - invalid card #%d\n", card);
return(ERROR);
}
Debug(2, "send_mess(): message = %s\n", com);
cntrl = (struct MVPcontroller *) motor_state[card]->DevicePrivate;
pasynOctetSyncIO->write(cntrl->pasynUser, com, size, COMM_TIMEOUT, &nwrite);
return(OK);
}
/*****************************************************/
/* receive a message from the MVP2001 board */
/* recv_mess() */
/*****************************************************/
static int recv_mess(int card, char *com, int flag)
{
struct MVPcontroller *cntrl;
char temp[BUFF_SIZE];
size_t nread = 0, lenTemp = 0;
asynStatus status = asynError;
int timeout;
int eomReason;
/* Check that card exists */
if (!motor_state[card])
return(-1);
cntrl = (struct MVPcontroller *) motor_state[card]->DevicePrivate;
if (flag == FLUSH)
timeout = 0;
else
timeout = COMM_TIMEOUT;
status = pasynOctetSyncIO->read(cntrl->pasynUser, temp, BUFF_SIZE,
COMM_TIMEOUT, &lenTemp, &eomReason);
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
COMM_TIMEOUT, &nread, &eomReason);
Debug(5, "bytes: 1st call: %d\t2nd call: %d\n", lenTemp, nread);
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
}
else
epicsThreadSleep(0.1); /* Fix for communication timeout. */
Debug(2, "recv_mess(): message = \"%s\"\n", com);
return(nread);
}
/*****************************************************/
/* Setup system configuration */
/* MVP2001Setup() */
/*****************************************************/
RTN_STATUS
MVP2001Setup(int num_cards, /* number of CHAINS of controllers */
int scan_rate) /* polling rate (Min=1Hz, max=60Hz) */
{
if (num_cards < 1 || num_cards > MVP2001_NUM_CARDS)
MVP2001_num_cards = MVP2001_NUM_CARDS;
else
MVP2001_num_cards = num_cards;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
else
targs.motor_scan_rate = SCAN_RATE;
/*
* Allocate space for motor_state structures. Note this must be done
* before MVP2001Config is called, so it cannot be done in motor_init()
* This means that we must allocate space for a card without knowing if it
* really exists, which is not a serious problem
*/
motor_state = (struct controller **) calloc(MVP2001_num_cards,
sizeof(struct controller *));
return(OK);
}
/*******************************************************
* Configure a CHAIN of controllers *
* *
* Note: Addresses of controllers on a chain must *
* begin at 1 and follow sequentially. *
* *
* MVP2001Config() *
********************************************************/
RTN_STATUS
MVP2001Config(int card, /* CHAIN being configured */
const char *name) /* asyn server task name */
{
struct MVPcontroller *cntrl;
if (card < 0 || card >= MVP2001_num_cards)
return(ERROR);
motor_state[card] = (struct controller *) calloc(1, sizeof(struct controller));
motor_state[card]->DevicePrivate = calloc(1, sizeof(struct MVPcontroller));
cntrl = (struct MVPcontroller *) motor_state[card]->DevicePrivate;
strcpy(cntrl->asyn_port, name);
return(OK);
}
/*****************************************************/
/* initialize all software and hardware */
/* motor_init() */
/*****************************************************/
static int motor_init()
{
struct controller *brdptr;
struct MVPcontroller *cntrl;
int card_index, motor_index;
char buff[BUFF_SIZE], limitStr[BUFF_SIZE];
int total_axis = 0;
int status;
asynStatus success_rtn;
static const char output_terminator[] = "\r";
static const char input_terminator[] = "\n";
buff[0] = limitStr[0] = '\0';
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (MVP2001_num_cards <= 0)
return(ERROR);
for (card_index = 0; card_index < MVP2001_num_cards; card_index++)
{
if (!motor_state[card_index])
continue;
brdptr = motor_state[card_index];
brdptr->ident[0] = (char) NULL; /* No controller identification
* message. */
brdptr->cmnd_response = false; /* The MVP doesn't respond to every
* command */
total_cards = card_index + 1;
cntrl = (struct MVPcontroller *) brdptr->DevicePrivate;
/* Initialize communications channel */
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0,
&cntrl->pasynUser, NULL);
if (success_rtn == asynSuccess)
{
pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator,
strlen(output_terminator));
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator,
strlen(input_terminator));
/* Send a message to the board, see if it exists */
for (total_axis = 0; total_axis < MAX_AXIS; total_axis++)
{
int retry = 0;
/*
* flush any junk at input port - should not be any data
* available
*/
do
recv_mess(card_index, buff, FLUSH);
while (strlen(buff) != 0);
do
{
sprintf(buff, "%d ST", (total_axis + 1));
send_mess(card_index, buff, (char*) NULL);
status = recv_mess(card_index, buff, 1);
retry++;
} while (status <= 0 && retry < 3);
if (status <= 0)
break;
}
brdptr->total_axis = total_axis;
Debug(5, "brdptr->total_axis (number of controllers on chain %d) = %d\n", card_index, brdptr->total_axis);
}
if (success_rtn == asynSuccess && total_axis > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
/* stop and initialize the controller */
sprintf(buff, "%d V 0", (motor_index + 1));
send_mess(card_index, buff, (char*) NULL);
sprintf(buff, "%d HO", (motor_index + 1));
send_mess(card_index, buff, (char*) NULL);
sprintf(buff, "%d EN", (motor_index + 1));
send_mess(card_index, buff, (char*) NULL);
motor_info->status.All = 0;
motor_info->no_motion_count = 0;
motor_info->encoder_position = 0;
motor_info->position = 0;
brdptr->motor_info[motor_index].motor_motion = NULL;
/* no encoder support for correct DC controller interaction */
motor_info->encoder_present = NO;
motor_info->status.Bits.EA_PRESENT = 0;
/* MVP2001 has PID capabilities */
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
limitStr[0] = '\0';
/* Determine low limit */
sprintf(buff, "%d LL -", (motor_index + 1));
send_mess(card_index, buff, (char*) NULL);
recv_mess(card_index, buff, 1);
strncat(limitStr, &buff[5], 8);
motor_info->low_limit = (epicsInt32) strtoul(limitStr, NULL, 16);
limitStr[0] = '\0';
/* Determine high limit */
sprintf(buff, "%d LL", (motor_index + 1));
send_mess(card_index, buff, (char*) NULL);
recv_mess(card_index, buff, 1);
strncat(limitStr, &buff[5], 8);
motor_info->high_limit = (epicsInt32) strtoul(limitStr, NULL, 16);
}
/*
* Ensure that the position is correctly set to zero so that
* auto_sr loads the saved positions. The task delay is necessary
* because sending the HO command too soon after the EN command
* results in reading back a position within ten encoder pulses
* away from zero.
*/
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
epicsThreadSleep(0.2);
sprintf(buff, "%d HO", (motor_index + 1));
send_mess(card_index, buff, (char*) NULL);
set_status(card_index, motor_index); /* Read status of each
* motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
}
any_motor_in_motion = 0;
mess_queue.head = (struct mess_node *) NULL;
mess_queue.tail = (struct mess_node *) NULL;
free_list.head = (struct mess_node *) NULL;
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "MVP2001_motor", epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
return(0);
}
motorApp/MicroMoSrc/drvMVP2001.h
-122
View File
@@ -1,122 +0,0 @@
/*
FILENAME... drvMVP2001.h
USAGE... This file contains driver "include" information that is specific to
the MicroMo MVP 2001 B02 (Linear, RS-485).
*/
/*
* Original Author: Kevin Peterson
* Date: 08/27/2002
*
* Illinois Open Source License
* University of Illinois
* Open Source License
*
*
* Copyright (c) 2004, UNICAT. All rights reserved.
*
*
* Developed by:
*
* UNICAT, Advanced Photon Source, Argonne National Laboratory
*
* Frederick Seitz Materials Research Laboratory,
* University of Illinois at Urbana-Champaign
*
* http://www.uni.aps.anl.gov
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the
* "Software"), to deal with the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
*
* Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimers.
*
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimers in the
* documentation and/or other materials provided with the distribution.
*
*
* Neither the names of UNICAT, Frederick Seitz Materials Research
* Laboratory, University of Illinois at Urbana-Champaign,
* nor the names of its contributors may be used to endorse or promote
* products derived from this Software without specific prior written
* permission.
*
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL THE CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR
* ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
* SOFTWARE OR THE USE OR OTHER DEALINGS WITH THE SOFTWARE.
*
*
* Modification Log:
* -----------------
* .01 08/27/2002 kmp copied from drvIM483.h (rev 1.1, mod .01) and
* customized for the MVP2001.
* .02 07/12/2004 rls Converted from MPF to asyn.
*
*/
#ifndef INCdrvMVP2001h
#define INCdrvMVP2001h 1
#include "motor.h"
#include "motordrvCom.h"
#include "asynDriver.h"
#include "asynOctetSyncIO.h"
#define COMM_TIMEOUT 2 /* Command timeout in seconds. */
/* MVP2001 (chain) specific data is stored in this structure. */
struct MVPcontroller
{
asynUser *pasynUser; /* For RS-232 */
char asyn_port[80]; /* asyn port name */
int maxCurrent[MAX_AXIS]; /* Maximum current (in mA) */
int encoderCpr[MAX_AXIS]; /* Encoder counts per revolution */
CommStatus status; /* Controller communication status. */
};
/* Motor status response for MVP2001. */
typedef union
{
epicsUInt16 All;
struct
{
bool minusLS :1; /* negative limit switch hit */
bool extEvent2 :1; /* external event #2 */
bool plusLS :1; /* positive limit switch hit */
bool extEvent1 :1; /* external event #1 */
bool emergencyStop :1; /* emergency stop flag is active */
bool localMode :1; /* 1:local mode - 0:remote mode */
bool softLimit :1; /* soft-limit has been reached */
bool NOT_power :1; /* motor power 0 - ON; 1 - OFF. */
bool offTraj :1; /* off traj (more than FD com. value) */
bool devicenetErr :1; /* error in devicenet message packets */
bool devicenet :1; /* devicenet connection active */
bool trajComplete :1; /* 1=traj (set by T com.) is complete */
bool aiState :1; /* ANM mode not used--always 1 */
bool operatingMode :1; /* 1:Velocity - 0:Position */
bool inPosition :1; /* in-position indicator */
bool inMotion :1; /* in-motion indicator */
} Bits;
} MOTOR_STATUS;
/* Function prototypes. */
extern RTN_STATUS MVP2001Setup(int, int);
extern RTN_STATUS MVP2001Config(int, const char *);
#endif /* INCdrvMVP2001h */
motorExApp/WithAsyn/Makefile
-2
View File
@@ -15,7 +15,6 @@ COMMONDBDS += motorSupport.dbd
COMMONDBDS += devAcsMotor.dbd
COMMONDBDS += devImsMotor.dbd
COMMONDBDS += devMicos.dbd
COMMONDBDS += devMicroMo.dbd
ifdef SNCSEQ
COMMONDBDS += devAerotechSeq.dbd
endif
@@ -65,7 +64,6 @@ WithAsyn_OBJS_vxWorks += $(EPICS_BASE_BIN)/vxComLibrary
COMMONLIBS += Aerotech
COMMONLIBS += Micos
COMMONLIBS += MicroMo
COMMONLIBS += PI
COMMONLIBS += Acs
COMMONLIBS += Ims