From f7d9f5f8ddcb76e85dd08e67d76946fe4f65d5ae Mon Sep 17 00:00:00 2001 From: MarkRivers Date: Fri, 2 Mar 2012 00:12:33 +0000 Subject: [PATCH] New file, Model 3 driver --- acsApp/src/MCB4BDriver.cpp | 334 +++++++++++++++++++++++++++++++++++++ acsApp/src/MCB4BDriver.h | 49 ++++++ 2 files changed, 383 insertions(+) create mode 100644 acsApp/src/MCB4BDriver.cpp create mode 100644 acsApp/src/MCB4BDriver.h diff --git a/acsApp/src/MCB4BDriver.cpp b/acsApp/src/MCB4BDriver.cpp new file mode 100644 index 0000000..70dcbad --- /dev/null +++ b/acsApp/src/MCB4BDriver.cpp @@ -0,0 +1,334 @@ +/* +FILENAME... MCB4BDriver.cpp +USAGE... Motor driver support for the ACS MCB-4B controller. + +Mark Rivers +March 1, 2012 + +*/ + + +#include +#include +#include +#include + +#include +#include + +#include + +#include "MCB4BDriver.h" +#include + +#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5) + +/** Creates a new MCB4BController object. + * \param[in] portName The name of the asyn port that will be created for this driver + * \param[in] MCB4BPortName The name of the drvAsynSerialPort that was created previously to connect to the MCB4B 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 + */ +MCB4BController::MCB4BController(const char *portName, const char *MCB4BPortName, int numAxes, + double movingPollPeriod, double idlePollPeriod) + : asynMotorController(portName, numAxes, NUM_MCB4B_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 +{ + int axis; + asynStatus status; + MCB4BAxis *pAxis; + static const char *functionName = "MCB4BController::MCB4BController"; + + /* Connect to MCB4B controller */ + status = pasynOctetSyncIO->connect(MCB4BPortName, 0, &pasynUserController_, NULL); + if (status) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: cannot connect to MCB-4B controller\n", + functionName); + } + for (axis=0; axis 0 then information is printed about each axis. + * After printing controller-specific information it calls asynMotorController::report() + */ +void MCB4BController::report(FILE *fp, int level) +{ + fprintf(fp, "MCB-4B 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 MCB4BAxis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] pasynUser asynUser structure that encodes the axis index number. */ +MCB4BAxis* MCB4BController::getAxis(asynUser *pasynUser) +{ + return static_cast(asynMotorController::getAxis(pasynUser)); +} + +/** Returns a pointer to an MCB4BAxis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] axisNo Axis index number. */ +MCB4BAxis* MCB4BController::getAxis(int axisNo) +{ + return static_cast(asynMotorController::getAxis(axisNo)); +} + + +// These are the MCB4BAxis methods + +/** Creates a new MCB4BAxis object. + * \param[in] pC Pointer to the MCB4BController to which this axis belongs. + * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. + * + * Initializes register numbers, etc. + */ +MCB4BAxis::MCB4BAxis(MCB4BController *pC, int axisNo) + : asynMotorAxis(pC, axisNo), + pC_(pC) +{ +} + +/** Reports on status of the driver + * \param[in] fp The file pointer on which report information will be written + * \param[in] level The level of report detail desired + * + * If details > 0 then information is printed about each axis. + * After printing controller-specific information calls asynMotorController::report() + */ +void MCB4BAxis::report(FILE *fp, int level) +{ + if (level > 0) { + fprintf(fp, " axis %d\n", + axisNo_); + } + + // Call the base class method + asynMotorAxis::report(fp, level); +} + +asynStatus MCB4BAxis::sendAccelAndVelocity(double acceleration, double velocity) +{ + asynStatus status; + int ival; + // static const char *functionName = "MCB4B::sendAccelAndVelocity"; + + // Send the velocity + ival = NINT(fabs(115200./velocity)); + if (ival < 2) ival=2; + if (ival > 255) ival = 255; + sprintf(pC_->outString_, "#%02dV=%d", axisNo_, ival); + status = pC_->writeController(); + + // Send the acceleration + // acceleration is in steps/sec/sec + // MCB is programmed with Ramp Index (R) where: + // dval (steps/sec/sec) = 720,000/(256-R) */ + // or R=256-(720,000/dval) */ + ival = NINT(256-(720000./acceleration)); + if (ival < 1) ival=1; + if (ival > 255) ival=255; + sprintf(pC_->outString_, "#%02dR=%d", axisNo_, ival); + status = pC_->writeController(); + return status; +} + + +asynStatus MCB4BAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) +{ + asynStatus status; + // static const char *functionName = "MCB4BAxis::move"; + + status = sendAccelAndVelocity(acceleration, maxVelocity); + + if (relative) { + sprintf(pC_->outString_, "#%02dI%+d", axisNo_, NINT(position)); + } else { + sprintf(pC_->outString_, "#%02dG%+d", axisNo_, NINT(position)); + } + status = pC_->writeController(); + return status; +} + +asynStatus MCB4BAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) +{ + asynStatus status; + // static const char *functionName = "MCB4BAxis::home"; + + status = sendAccelAndVelocity(acceleration, maxVelocity); + + if (forwards) { + sprintf(pC_->outString_, "#%02dH+", axisNo_); + } else { + sprintf(pC_->outString_, "#%02dH-", axisNo_); + } + status = pC_->writeController(); + return status; +} + +asynStatus MCB4BAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration) +{ + asynStatus status; + // static const char *functionName = "MCB4BAxis::moveVelocity"; + + status = sendAccelAndVelocity(acceleration, maxVelocity); + + /* MCB-4B does not have jog command. Move 1 million steps */ + if (maxVelocity > 0.) { + /* This is a positive move in MCB4B coordinates */ + sprintf(pC_->outString_, "#%02dM+1000000", axisNo_); + } else { + /* This is a negative move in MCB4B coordinates */ + sprintf(pC_->outString_, "#%02dM-1000000", axisNo_); + } + status = pC_->writeController(); + return status; +} + +asynStatus MCB4BAxis::stop(double acceleration ) +{ + asynStatus status; + //static const char *functionName = "MCB4BAxis::stop"; + + sprintf(pC_->outString_, "#%02dQ", axisNo_); + status = pC_->writeController(); + return status; +} + +asynStatus MCB4BAxis::setPosition(double position) +{ + asynStatus status; + //static const char *functionName = "MCB4BAxis::setPosition"; + + sprintf(pC_->outString_, "#%02dP=%+d", axisNo_, NINT(position)); + status = pC_->writeController(); + return status; +} + +asynStatus MCB4BAxis::setClosedLoop(bool closedLoop) +{ + asynStatus status; + //static const char *functionName = "MCB4BAxis::setClosedLoop"; + + sprintf(pC_->outString_, "#%02dW=%d", axisNo_, closedLoop ? 1:0); + status = pC_->writeController(); + return status; +} + +/** Polls the axis. + * This function reads the controller position, encoder position, the limit status, the moving status, + * and the drive power-on status. It does not current detect following error, etc. but this could be + * added. + * It calls setIntegerParam() and setDoubleParam() for each item that it polls, + * and then calls callParamCallbacks() at the end. + * \param[out] moving A flag that is set indicating that the axis is moving (1) or done (0). */ +asynStatus MCB4BAxis::poll(bool *moving) +{ + int done; + int driveOn; + int limit; + double position; + asynStatus comStatus; + + // Read the current motor position + sprintf(pC_->outString_, "#%02dP", axisNo_); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + // The response string is of the form "#01P=+1000" + position = atof(&pC_->inString_[5]); + setDoubleParam(pC_->motorPosition_, position); + + // Read the moving status of this motor + sprintf(pC_->outString_, "#%02dX", axisNo_); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + // The response string is of the form "#01X=1" + done = (pC_->inString_[5] == '0') ? 1:0; + setIntegerParam(pC_->motorStatusDone_, done); + *moving = done ? false:true; + + // Read the limit status + sprintf(pC_->outString_, "#%02dE", axisNo_); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + // The response string is of the form "#01E=1" + limit = (pC_->inString_[5] == '1') ? 1:0; + setIntegerParam(pC_->motorStatusHighLimit_, limit); + limit = (pC_->inString_[6] == '1') ? 1:0; + setIntegerParam(pC_->motorStatusLowLimit_, limit); + limit = (pC_->inString_[7] == '1') ? 1:0; + setIntegerParam(pC_->motorStatusAtHome_, limit); + + // Read the drive power on status + sprintf(pC_->outString_, "#%02dE", axisNo_); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + driveOn = (pC_->inString_[5] == '1') ? 1:0; + 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 MCB4BCreateControllerArg0 = {"Port name", iocshArgString}; +static const iocshArg MCB4BCreateControllerArg1 = {"MCB-4B port name", iocshArgString}; +static const iocshArg MCB4BCreateControllerArg2 = {"Number of axes", iocshArgInt}; +static const iocshArg MCB4BCreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt}; +static const iocshArg MCB4BCreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt}; +static const iocshArg * const MCB4BCreateControllerArgs[] = {&MCB4BCreateControllerArg0, + &MCB4BCreateControllerArg1, + &MCB4BCreateControllerArg2, + &MCB4BCreateControllerArg3, + &MCB4BCreateControllerArg4}; +static const iocshFuncDef MCB4BCreateControllerDef = {"MCB4BCreateController", 5, MCB4BCreateControllerArgs}; +static void MCB4BCreateContollerCallFunc(const iocshArgBuf *args) +{ + MCB4BCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival); +} + +static void MCB4BRegister(void) +{ + iocshRegister(&MCB4BCreateControllerDef, MCB4BCreateContollerCallFunc); +} + +extern "C" { +epicsExportRegistrar(MCB4BRegister); +} diff --git a/acsApp/src/MCB4BDriver.h b/acsApp/src/MCB4BDriver.h new file mode 100644 index 0000000..fa7ef0a --- /dev/null +++ b/acsApp/src/MCB4BDriver.h @@ -0,0 +1,49 @@ +/* +FILENAME... MCB4BDriver.h +USAGE... Motor driver support for the ACS MCB-4B controller. + +Mark Rivers +March 1, 2012 + +*/ + +#include "asynMotorController.h" +#include "asynMotorAxis.h" + +#define MAX_MCB4B_AXES 4 + +// No controller-specific parameters yet +#define NUM_MCB4B_PARAMS 0 + +class MCB4BAxis : public asynMotorAxis +{ +public: + /* These are the methods we override from the base class */ + MCB4BAxis(class MCB4BController *pC, int axis); + 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 setClosedLoop(bool closedLoop); + +private: + MCB4BController *pC_; /**< Pointer to the asynMotorController to which this axis belongs. + * Abbreviated because it is used very frequently */ + asynStatus sendAccelAndVelocity(double accel, double velocity); + +friend class MCB4BController; +}; + +class MCB4BController : public asynMotorController { +public: + MCB4BController(const char *portName, const char *MCB4BPortName, int numAxes, double movingPollPeriod, double idlePollPeriod); + + void report(FILE *fp, int level); + MCB4BAxis* getAxis(asynUser *pasynUser); + MCB4BAxis* getAxis(int axisNo); + +friend class MCB4BAxis; +};