diff --git a/motorApp/MicroMoSrc/MVP2001Driver.cpp b/motorApp/MicroMoSrc/MVP2001Driver.cpp new file mode 100644 index 00000000..07203879 --- /dev/null +++ b/motorApp/MicroMoSrc/MVP2001Driver.cpp @@ -0,0 +1,515 @@ +/* +FILENAME... MVP2001Driver.cpp +USAGE... Motor driver support for the MicroMo MVP 2001 controller. + +Kevin Peterson +August 14, 2013 + +*/ + +#include +#include +#include +#include + +#include +#include + +#include + +#include "MVP2001Driver.h" +#include + +#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(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(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); +} diff --git a/motorApp/MicroMoSrc/MVP2001Driver.h b/motorApp/MicroMoSrc/MVP2001Driver.h new file mode 100644 index 00000000..72920838 --- /dev/null +++ b/motorApp/MicroMoSrc/MVP2001Driver.h @@ -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; +}; diff --git a/motorApp/MicroMoSrc/Makefile b/motorApp/MicroMoSrc/Makefile index ca537673..a113f3c0 100644 --- a/motorApp/MicroMoSrc/Makefile +++ b/motorApp/MicroMoSrc/Makefile @@ -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) diff --git a/motorApp/MicroMoSrc/devMicroMo.dbd b/motorApp/MicroMoSrc/devMicroMo.dbd index b8f1f078..645e30e3 100644 --- a/motorApp/MicroMoSrc/devMicroMo.dbd +++ b/motorApp/MicroMoSrc/devMicroMo.dbd @@ -3,3 +3,6 @@ device(motor,VME_IO,devMVP2001,"MVP2001") driver(drvMVP2001) #variable(drvMVP2001debug) +# Model 3 driver +registrar(MVP2001Register) +