diff --git a/documentation/motorRecord.html b/documentation/motorRecord.html index 8976a121..ddd4f210 100644 --- a/documentation/motorRecord.html +++ b/documentation/motorRecord.html @@ -69,7 +69,7 @@ Channel support): MicroMo model MVP 2001 B02.
  • - Micos model MoCo dc controller. + Micos model MoCo dc controller, SMC hydra controller.
  • Delta Tau PMAC2-VME controller. diff --git a/iocBoot/iocWithAsyn/motor.cmd.SMChydra b/iocBoot/iocWithAsyn/motor.cmd.SMChydra new file mode 100644 index 00000000..90130ec2 --- /dev/null +++ b/iocBoot/iocWithAsyn/motor.cmd.SMChydra @@ -0,0 +1,32 @@ +### motor.cmd.SMChydra +# +# This file is designed to be sourced: +# * after the communication port has been configured +# * before iocInit +# + +# Show serial communication +#asynSetTraceMask("serial5", 0, 3) +# Only show errors +asynSetTraceMask("serial5", 0, 1) +# Leave ascii selected so traces can be turned on with a single click +asynSetTraceIOMask("serial5", 0, 1) + +dbLoadTemplate("motor.substitutions.SMChydra") + +# SMChydraCreateController( +# port name, +# asyn port name, +# num axes, +# moving poll period (ms), +# idle poll period (ms) ) +SMChydraCreateController("Hydra1", "serial5", 2, 100, 500) +# Use the following line to disable idle polling +#!SMChydraCreateController("Hydra1", "serial5", 2, 100, 0) + +# This sleep is useful when asyn traces are enabled +#!epicsThreadSleep(3.0) + +# Include an asyn record for each motor to make it easy to enable diagnostic messages +dbLoadRecords("$(ASYN)/db/asynRecord.db","P=IOC:,R=hydraAsyn1,PORT=Hydra1,ADDR=0,OMAX=256,IMAX=256") +dbLoadRecords("$(ASYN)/db/asynRecord.db","P=IOC:,R=hydraAsyn2,PORT=Hydra1,ADDR=1,OMAX=256,IMAX=256") diff --git a/iocBoot/iocWithAsyn/motor.substitutions.SMChydra b/iocBoot/iocWithAsyn/motor.substitutions.SMChydra new file mode 100644 index 00000000..1e60736a --- /dev/null +++ b/iocBoot/iocWithAsyn/motor.substitutions.SMChydra @@ -0,0 +1,8 @@ +file "$(TOP)/db/basic_asyn_motor.db" +{ + pattern + {P, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VMAX, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DHLM, DLLM, INIT} + {IOC:, m1, "asynMotor", Hydra1, 0, "m1", degrees, Pos, 15., 3., .05, .5, 0, 1.0, 2, 0.01, 2, 175, -175, ""} + {IOC:, m2, "asynMotor", Hydra1, 1, "m2", degrees, Pos, 15., 3., .05, .5, 0, 1.0, 2, 0.01, 2, 175, -175, ""} +} + diff --git a/iocBoot/iocWithAsyn/st.cmd.Vx b/iocBoot/iocWithAsyn/st.cmd.Vx index a544e664..50f3cb2e 100644 --- a/iocBoot/iocWithAsyn/st.cmd.Vx +++ b/iocBoot/iocWithAsyn/st.cmd.Vx @@ -196,6 +196,9 @@ dbLoadRecords("$(MOTOR)/db/motorUtil.db", "P=IOC:") #!MicosConfig(0, "a-Serial[0]") #!drvMicosDebug = 4 +# Micos SMC hydra +#!< motor.cmd.SMChydra + # IMS MDrive driver setup parameters: # (1) maximum number of controllers in system # (2) motor task polling rate (min=1Hz,max=60Hz) diff --git a/motorApp/MicosSrc/Makefile b/motorApp/MicosSrc/Makefile index 4e5532fe..86a68cbf 100644 --- a/motorApp/MicosSrc/Makefile +++ b/motorApp/MicosSrc/Makefile @@ -14,6 +14,9 @@ LIBRARY_IOC = Micos SRCS += MicosRegister.cc SRCS += devMicos.cc drvMicos.cc +# SMC hydra support +SRCS += SMChydraDriver.cpp + Micos_LIBS += motor asyn Micos_LIBS += $(EPICS_BASE_IOC_LIBS) diff --git a/motorApp/MicosSrc/SMChydraDriver.cpp b/motorApp/MicosSrc/SMChydraDriver.cpp new file mode 100644 index 00000000..dab3174b --- /dev/null +++ b/motorApp/MicosSrc/SMChydraDriver.cpp @@ -0,0 +1,483 @@ +/* +FILENAME... SMChydraDriver.cpp +USAGE... Motor driver support for the Micos SMC hydra controller. + +Note: This driver was tested with the Micos SMC hydra CM and + motor forms 0 (stepper) and 1 (linear). + +*/ + + +#include +#include +#include +#include + +#include +#include + +#include + +#include "SMChydraDriver.h" +#include + +#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5) + +/** Creates a new SMChydraController object. + * \param[in] portName The name of the asyn port that will be created for this driver + * \param[in] SMChydraPortName The name of the drvAsynSerialPort that was created previously to connect to the SMC hydra 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 + */ +SMChydraController::SMChydraController(const char *portName, const char *SMChydraPortName, int numAxes, + double movingPollPeriod, double idlePollPeriod) + : asynMotorController(portName, numAxes, NUM_SMCHYDRA_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; + SMChydraAxis *pAxis; + static const char *functionName = "SMChydraController::SMChydraController"; + + /* Connect to SMC hydra controller */ + status = pasynOctetSyncIO->connect(SMChydraPortName, 0, &pasynUserController_, NULL); + if (status) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: cannot connect to SMC hydra 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 SMChydraController::report(FILE *fp, int level) +{ + fprintf(fp, "SMC hydra 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 SMChydraAxis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] pasynUser asynUser structure that encodes the axis index number. */ +SMChydraAxis* SMChydraController::getAxis(asynUser *pasynUser) +{ + return static_cast(asynMotorController::getAxis(pasynUser)); +} + +/** Returns a pointer to an SMChydraAxis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] axisNo Axis index number. */ +SMChydraAxis* SMChydraController::getAxis(int axisNo) +{ + return static_cast(asynMotorController::getAxis(axisNo)); +} + + +// These are the SMChydraAxis methods + +/** Creates a new SMChydraAxis object. + * \param[in] pC Pointer to the SMChydraController to which this axis belongs. + * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. + * + * Initializes register numbers, etc. + */ +SMChydraAxis::SMChydraAxis(SMChydraController *pC, int axisNo) + : asynMotorAxis(pC, axisNo), + pC_(pC) +{ + sprintf(pC_->outString_, "%i getmotor", (axisNo + 1)); + pC_->writeReadController(); + motorForm_ = atoi( (char *) &pC_->inString_ ); + + sprintf(pC_->outString_, "%i getpitch", (axisNo + 1)); + pC_->writeReadController(); + pitch_ = atof( (char *) &pC_->inString_ ); + + sprintf(pC_->outString_, "%i getpolepairs", (axisNo + 1)); + pC_->writeReadController(); + polePairs_ = atoi( (char *) &pC_->inString_ ); + + sprintf(pC_->outString_, "%i getclperiod", (axisNo + 1)); + pC_->writeReadController(); + clPeriod_ = atof( (char *) &pC_->inString_ ); + + switch (motorForm_) + { + case 0: + // Stepper motor + axisRes_ = pitch_ / ( 4.0 * polePairs_); + break; + + case 1: + // Linear or torque motor + axisRes_ = clPeriod_; + break; + + default: + // For now assume clPeriod_ works for other motor forms + axisRes_ = clPeriod_; + break; + } + + /* Enable gain support so that the CNEN field can be used to send + the init command to clear a motor fault for stepper motors, even + though they lack closed-loop support. */ + setIntegerParam(pC_->motorStatusGainSupport_, 1); + + // Determine the travel limits (will change after homing) + sprintf(pC_->outString_, "%i getnlimit", (axisNo + 1)); + pC_->writeReadController(); + sscanf(pC_->inString_, "%lf %lf", &negTravelLimit_, &posTravelLimit_); + +} + +/** 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 SMChydraAxis::report(FILE *fp, int level) +{ + if (level > 0) { + fprintf(fp, " axis %d\n", axisNo_); + fprintf(fp, " motorForm %d\n", motorForm_); + fprintf(fp, " pitch %f\n", pitch_); + fprintf(fp, " polePairs %d\n", polePairs_); + fprintf(fp, " clPeriod %f\n", clPeriod_); + fprintf(fp, " axisRes %f\n", axisRes_); + fprintf(fp, " lowLimitConfig %d\n", lowLimitConfig_); + fprintf(fp, " highLimitConfig %d\n", highLimitConfig_); + fprintf(fp, " posTravelLimit %f\n", posTravelLimit_); + fprintf(fp, " negTravelLimit %f\n", negTravelLimit_); + } + + // Call the base class method + asynMotorAxis::report(fp, level); +} + +asynStatus SMChydraAxis::sendAccelAndVelocity(double acceleration, double velocity) +{ + asynStatus status; + // static const char *functionName = "SMChydraAxis::sendAccelAndVelocity"; + + // Send the velocity + sprintf(pC_->outString_, "%f %i snv", (velocity * axisRes_), (axisNo_ + 1)); + status = pC_->writeController(); + + // Send the acceleration + // acceleration is in units/sec/sec + sprintf(pC_->outString_, "%f %i sna", (acceleration * axisRes_), (axisNo_ + 1)); + status = pC_->writeController(); + return status; +} + + +asynStatus SMChydraAxis::move(double position, int relative, double baseVelocity, double slewVelocity, double acceleration) +{ + asynStatus status; + // static const char *functionName = "SMChydraAxis::move"; + + status = sendAccelAndVelocity(acceleration, slewVelocity); + + if (relative) { + sprintf(pC_->outString_, "%f %i nr", (position * axisRes_), (axisNo_ + 1)); + } else { + sprintf(pC_->outString_, "%f %i nm", (position * axisRes_), (axisNo_ + 1)); + } + status = pC_->writeController(); + return status; +} + +asynStatus SMChydraAxis::home(double baseVelocity, double slewVelocity, double acceleration, int forwards) +{ + asynStatus status; + // static const char *functionName = "SMChydraAxis::home"; + + status = sendAccelAndVelocity(acceleration, slewVelocity); + + if (forwards) { + sprintf(pC_->outString_, "%i nrm", (axisNo_ + 1)); + } else { + sprintf(pC_->outString_, "%i ncal", (axisNo_ + 1)); + } + status = pC_->writeController(); + return status; +} + +asynStatus SMChydraAxis::moveVelocity(double baseVelocity, double slewVelocity, double acceleration) +{ + asynStatus status; + static const char *functionName = "SMChydraAxis::moveVelocity"; + + asynPrint(pasynUser_, ASYN_TRACE_FLOW, + "%s: baseVelocity=%f, slewVelocity=%f, acceleration=%f\n", + functionName, baseVelocity, slewVelocity, acceleration); + + /* SMC hydra does not have jog command. Move to a limit*/ + if (slewVelocity > 0.) { + status = sendAccelAndVelocity(acceleration, slewVelocity); + sprintf(pC_->outString_, "%f %i nm", posTravelLimit_, (axisNo_ + 1)); + } else { + status = sendAccelAndVelocity(acceleration, (slewVelocity * -1.0)); + sprintf(pC_->outString_, "%f %i nm", negTravelLimit_, (axisNo_ + 1)); + } + status = pC_->writeController(); + return status; +} + +asynStatus SMChydraAxis::stop(double acceleration ) +{ + asynStatus status; + //static const char *functionName = "SMChydraAxis::stop"; + + sprintf(pC_->outString_, "%i nabort", (axisNo_ + 1)); + status = pC_->writeController(); + return status; +} + +asynStatus SMChydraAxis::setPosition(double position) +{ + asynStatus status; + //static const char *functionName = "SMChydraAxis::setPosition"; + + sprintf(pC_->outString_, "%f %i setnpos", (position * axisRes_), (axisNo_ + 1)); + status = pC_->writeController(); + return status; +} + +asynStatus SMChydraAxis::setClosedLoop(bool closedLoop) +{ + asynStatus status = asynSuccess; + //static const char *functionName = "SMChydraAxis::setClosedLoop"; + + switch (motorForm_) + { + case 0: + // Stepper motor + + // reinit to clear a motor fault when CNEN=1 + if (closedLoop) + { + sprintf(pC_->outString_, "%i init", (axisNo_ + 1)); + status = pC_->writeController(); + + // a delay is required after the init command is sent + epicsThreadSleep(0.2); + + } + + break; + + case 1: + // Linear or torque motor + + // enable closed-loop control + sprintf(pC_->outString_, "%i %i setcloop", closedLoop ? 1:0, (axisNo_ + 1)); + status = pC_->writeController(); + + // reinit so the closed-loop setting takes effect + sprintf(pC_->outString_, "%i init", (axisNo_ + 1)); + status = pC_->writeController(); + + // a delay is required after the init command is sent + epicsThreadSleep(0.2); + + break; + + default: + // Do nothing + break; + } + + 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 SMChydraAxis::poll(bool *moving) +{ + int done; + int driveOn; + int lowLimit; + int highLimit; + int ignoreLowLimit; + int ignoreHighLimit; + int axisStatus=-1; + double position=0.0; + asynStatus comStatus; + + static const char *functionName = "SMChydraAxis::poll"; + + // Read the current motor position + sprintf(pC_->outString_, "%i np", (axisNo_ + 1)); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + // The response string is a double + position = atof( (char *) &pC_->inString_); + setDoubleParam(pC_->motorPosition_, (position / axisRes_) ); + + // Read the status of this motor + sprintf(pC_->outString_, "%i nst", (axisNo_ + 1)); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + // The response string is an int + axisStatus = atoi( (char *) &pC_->inString_); + + // Check the moving bit + done = !(axisStatus & 0x1); + setIntegerParam(pC_->motorStatusDone_, done); + setIntegerParam(pC_->motorStatusMoving_, !done); + *moving = done ? false:true; + + // Read the commanded velocity and acceleration + sprintf(pC_->outString_, "%i gnv", (axisNo_ + 1)); + comStatus = pC_->writeReadController(); + + sprintf(pC_->outString_, "%i gna", (axisNo_ + 1)); + comStatus = pC_->writeReadController(); + + + // Check the limit bit (0x40) + if (axisStatus & 0x40) + { + asynPrint(this->pasynUser_, ASYN_TRACEIO_DRIVER, + "%s: axis %i limit indicator active.\n", + functionName, (axisNo_ + 1)); + + // query limits? + } + + // Check the e-stop bit (0x80) + if (axisStatus & 0x80) + { + asynPrint(this->pasynUser_, ASYN_TRACEIO_DRIVER, + "%s: axis %i emergency stopped.\n", + functionName, (axisNo_ + 1)); + } + + // Check the e-stop switch active bit (0x200) + if (axisStatus & 0x200) + { + asynPrint(this->pasynUser_, ASYN_TRACEIO_DRIVER, + "%s: axis %i emergency stop switch active.\n", + functionName, (axisNo_ + 1)); + } + + // Check the device busy bit (0x400) + if (axisStatus & 0x400) + { + asynPrint(this->pasynUser_, ASYN_TRACE_ERROR, + "%s: axis %i device is busy - move commands discarded.\n", + functionName, (axisNo_ + 1)); + } + + // Read the limit status + // Note: calibration switch = low limit; range measure switch = high limit + // also need to read the switch confiruation to see if limits are ignored" + + // Read switch confiruation + // Bit 0: polarity (0 = NO, 1 = NC) + // Bit 1: mask (0 = enabled, 1 = disabled) + sprintf(pC_->outString_, "%i getsw", (axisNo_ + 1)); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + sscanf(pC_->inString_, "%i %i", &lowLimitConfig_, &highLimitConfig_); + ignoreLowLimit = lowLimitConfig_ & 0x2; + ignoreHighLimit = highLimitConfig_ & 0x2; + + // Read status of switches 0=inactive 1=active + sprintf(pC_->outString_, "%i getswst", (axisNo_ + 1)); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + // The response string is of the form "0 0" + sscanf(pC_->inString_, "%i %i", &lowLimit, &highLimit); + // + if (ignoreLowLimit) + setIntegerParam(pC_->motorStatusLowLimit_, 0); + else + setIntegerParam(pC_->motorStatusLowLimit_, lowLimit); + + if (ignoreHighLimit) + setIntegerParam(pC_->motorStatusHighLimit_, 0); + else + setIntegerParam(pC_->motorStatusHighLimit_, highLimit); + + /*setIntegerParam(pC_->motorStatusAtHome_, limit);*/ + + // Check the drive power bit (0x100) + driveOn = (axisStatus & 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 SMChydraCreateControllerArg0 = {"Port name", iocshArgString}; +static const iocshArg SMChydraCreateControllerArg1 = {"SMC hydra port name", iocshArgString}; +static const iocshArg SMChydraCreateControllerArg2 = {"Number of axes", iocshArgInt}; +static const iocshArg SMChydraCreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt}; +static const iocshArg SMChydraCreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt}; +static const iocshArg * const SMChydraCreateControllerArgs[] = {&SMChydraCreateControllerArg0, + &SMChydraCreateControllerArg1, + &SMChydraCreateControllerArg2, + &SMChydraCreateControllerArg3, + &SMChydraCreateControllerArg4}; +static const iocshFuncDef SMChydraCreateControllerDef = {"SMChydraCreateController", 5, SMChydraCreateControllerArgs}; +static void SMChydraCreateControllerCallFunc(const iocshArgBuf *args) +{ + SMChydraCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival); +} + +static void SMChydraRegister(void) +{ + iocshRegister(&SMChydraCreateControllerDef, SMChydraCreateControllerCallFunc); +} + +extern "C" { +epicsExportRegistrar(SMChydraRegister); +} diff --git a/motorApp/MicosSrc/SMChydraDriver.h b/motorApp/MicosSrc/SMChydraDriver.h new file mode 100644 index 00000000..b54ab97c --- /dev/null +++ b/motorApp/MicosSrc/SMChydraDriver.h @@ -0,0 +1,56 @@ +/* +FILENAME... SMChydraDriver.h +USAGE... Motor driver support for the Micos SMC hydra controller. + +*/ + +#include "asynMotorController.h" +#include "asynMotorAxis.h" + +#define MAX_SMCHYDRA_AXES 2 + +// No controller-specific parameters yet +#define NUM_SMCHYDRA_PARAMS 0 + +class SMChydraAxis : public asynMotorAxis +{ +public: + /* These are the methods we override from the base class */ + SMChydraAxis(class SMChydraController *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: + SMChydraController *pC_; /* Pointer to the asynMotorController to which this axis belongs. + Abbreviated because it is used very frequently */ + asynStatus sendAccelAndVelocity(double accel, double velocity); + int motorForm_; + double pitch_; + int polePairs_; + double clPeriod_; + double axisRes_; + double mres_; + int lowLimitConfig_; + int highLimitConfig_; + double posTravelLimit_; + double negTravelLimit_; + +friend class SMChydraController; +}; + +class SMChydraController : public asynMotorController { +public: + SMChydraController(const char *portName, const char *SMChydraPortName, int numAxes, double movingPollPeriod, double idlePollPeriod); + + void report(FILE *fp, int level); + SMChydraAxis* getAxis(asynUser *pasynUser); + SMChydraAxis* getAxis(int axisNo); + +friend class SMChydraAxis; +};