diff --git a/motorApp/Db/Makefile b/motorApp/Db/Makefile index f872ed32..1c29a256 100644 --- a/motorApp/Db/Makefile +++ b/motorApp/Db/Makefile @@ -44,6 +44,7 @@ DB += profileMoveControllerXPS.template DB += profileMoveAxisXPS.template DB += PI_Support.db PI_SupportCtrl.db DB += Phytron_motor.db Phytron_I1AM01.db Phytron_MCM01.db +DB += asyn_auto_power.db #---------------------------------------------------- # Declare template files which do not show up in DB diff --git a/motorApp/Db/asyn_auto_power.db b/motorApp/Db/asyn_auto_power.db new file mode 100644 index 00000000..c80875c2 --- /dev/null +++ b/motorApp/Db/asyn_auto_power.db @@ -0,0 +1,90 @@ + +############################################################ +# +# Template to provide records that enable automatic control +# of the amplifier power. This works with the Asyn model 3 +# based drivers. Amplifier power control is done via the +# setClosedLoop driver function (which also maps to the +# motor record CNEN field). +# +# Macros: +# P, M - motor name +# PORT - asyn port +# ADDR - asyn addr +# FRAC - holding current % (optional, default 0) +# +# Matt Pearson, ORNL +# March 2015 +# +############################################################ + +# /// +# /// Automatically control amplifier power. +# /// +record(bo, "$(P)$(M):AutoEnable") +{ + field(DESC, "Auto Power Control") + field(DTYP, "asynInt32") + field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_POWER_AUTO_ONOFF") + field(VAL, "0") + field(PINI, "YES") + field(ZNAM, "Disabled") + field(ONAM, "Enabled") + info(autosaveFields, "VAL") +} + +# /// +# /// Delay the move after the amplifier has been +# /// automatically enabled. +# /// +record(ao, "$(P)$(M):AutoEnableDelay") +{ + field(DESC, "Delay after power on") + field(DTYP, "asynFloat64") + field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_POWER_ON_DELAY") + field(VAL, "0") + field(PINI, "YES") + field(EGU, "s") + field(PREC, "1") + info(autosaveFields, "VAL") +} + +# /// +# /// Delay the end of move after the amplifier has been +# /// automatically disabled. +# /// This is independent of the motor record DLY +# /// field, but the delay will only start once the driver +# /// notified that the move is complete (which takes +# /// into account any controller specific post move delay). +# /// +record(ao, "$(P)$(M):AutoDisableDelay") +{ + field(DESC, "Delay after power off") + field(DTYP, "asynFloat64") + field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_POWER_OFF_DELAY") + field(VAL, "0") + field(PINI, "YES") + field(EGU, "s") + field(PREC, "1") + info(autosaveFields, "VAL") +} + +# /// +# /// Instead of power off completely, set a holding current +# /// which is a fraction of the moving current. +# /// +# /// If this is non zero it needs to be handled in the +# /// controller specific driver setClosedLoop function. +# /// +record(longout, "$(P)$(M):PowerOffFraction") +{ + field(DESC, "Holding Current %") + field(DTYP, "asynInt32") + field(OUT, "@asyn($(PORT),$(ADDR))MOTOR_POWER_OFF_FRACTION") + field(VAL, "$(FRAC=0)") + field(PINI, "YES") + field(HOPR, "100") + field(LOPR, "0") + info(autosaveFields, "VAL") +} + diff --git a/motorApp/MotorSimSrc/motorSimDriver.cpp b/motorApp/MotorSimSrc/motorSimDriver.cpp index cb3d2f23..bc3e12a4 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.cpp +++ b/motorApp/MotorSimSrc/motorSimDriver.cpp @@ -390,6 +390,9 @@ void motorSimAxis::process(double delta ) { double lastpos; int done = 0; + double postMoveDelay = 0.0; + epicsTimeStamp nowTime; + double nowTimeSecs = 0.0; lastpos = nextpoint_.axis[0].p; nextpoint_.T += delta; @@ -430,12 +433,34 @@ void motorSimAxis::process(double delta ) if (nextpoint_.axis[0].v == 0) { if (!deferred_move_) { - done = 1; + if (!delayedDone_) { + done = 1; + } } } else { done = 0; } + //Post move delay + epicsTimeGetCurrent(&nowTime); + pC_->getDoubleParam(axisNo_, pC_->motorPostMoveDelay_, &postMoveDelay); + if ((lastDone_ == 0) && (done == 1)) { + if (postMoveDelay > 0) { + delayedDone_ = 1; + done = 0; + lastTimeSecs_ = nowTime.secPastEpoch + (nowTime.nsec / 1.e9); + } + } + if (delayedDone_ == 1) { + nowTimeSecs = nowTime.secPastEpoch + (nowTime.nsec / 1.e9); + if ((nowTimeSecs - lastTimeSecs_) >= postMoveDelay) { + done = 1; + delayedDone_ = 0; + } + } + + lastDone_ = done; + setDoubleParam (pC_->motorPosition_, (nextpoint_.axis[0].p+enc_offset_)); setDoubleParam (pC_->motorEncoderPosition_, (nextpoint_.axis[0].p+enc_offset_)); setIntegerParam(pC_->motorStatusDirection_, (nextpoint_.axis[0].v > 0)); diff --git a/motorApp/MotorSimSrc/motorSimDriver.h b/motorApp/MotorSimSrc/motorSimDriver.h index 9457c4ac..2c9e8146 100644 --- a/motorApp/MotorSimSrc/motorSimDriver.h +++ b/motorApp/MotorSimSrc/motorSimDriver.h @@ -52,6 +52,9 @@ private: double deferred_position_; int deferred_move_; int deferred_relative_; + double lastTimeSecs_; + int delayedDone_; + int lastDone_; friend class motorSimController; }; diff --git a/motorApp/MotorSrc/asynMotorAxis.cpp b/motorApp/MotorSrc/asynMotorAxis.cpp index 5dc0b2db..d8274c83 100644 --- a/motorApp/MotorSrc/asynMotorAxis.cpp +++ b/motorApp/MotorSrc/asynMotorAxis.cpp @@ -53,6 +53,10 @@ asynMotorAxis::asynMotorAxis(class asynMotorController *pC, int axisNo) /* Used to enable/disable move to home, and to tell driver how far to move.*/ referencingModeMove_ = 0; + wasMovingFlag_ = 0; + disableFlag_ = 0; + lastEndOfMoveTime_ = 0; + // Create the asynUser, connect to this axis pasynUser_ = pasynManager->createAsynUser(NULL, NULL); pasynManager->connectDevice(pasynUser_, pC->portName, axisNo); @@ -187,7 +191,6 @@ asynStatus asynMotorAxis::setEncoderRatio(double ratio) return asynSuccess; } - void asynMotorAxis::report(FILE *fp, int details) { } @@ -431,3 +434,60 @@ asynStatus asynMotorAxis::readbackProfile() status |= pC_->doCallbacksFloat64Array(profileFollowingErrors_, numReadbacks, pC_->profileFollowingErrors_, axisNo_); return asynSuccess; } + +/****************************************************************************/ +/* The following functions are used by the automatic drive power control in the + base class poller in the asynMotorController class.*/ + +/** + * Read the flag that indicates if the last poll was moving. + */ +int asynMotorAxis::getWasMovingFlag(void) +{ + return wasMovingFlag_; +} + +/** + * Set this to 1 if the previous poll indicated moving state + */ +void asynMotorAxis::setWasMovingFlag(int wasMovingFlag) +{ + wasMovingFlag_ = wasMovingFlag; +} + +/** + * Read the flag that indicates if the drive should be automatically + * disabled. + */ +int asynMotorAxis::getDisableFlag(void) +{ + return disableFlag_; +} + +/** + * Set this to 1 if the drive should be automatically disabled. + */ +void asynMotorAxis::setDisableFlag(int disableFlag) +{ + disableFlag_ = disableFlag; +} + +/** + * Read the time in seconds of the last end of move. + */ +double asynMotorAxis::getLastEndOfMoveTime(void) +{ + return lastEndOfMoveTime_; +} + +/** + * Set this to the current time at the end of a move. + */ +void asynMotorAxis::setLastEndOfMoveTime(double time) +{ + lastEndOfMoveTime_ = time; +} + + +/********************************************************************/ + diff --git a/motorApp/MotorSrc/asynMotorAxis.h b/motorApp/MotorSrc/asynMotorAxis.h index 2904b7d8..e7882692 100644 --- a/motorApp/MotorSrc/asynMotorAxis.h +++ b/motorApp/MotorSrc/asynMotorAxis.h @@ -56,6 +56,13 @@ class epicsShareClass asynMotorAxis { void setReferencingModeMove(int distance); int getReferencingModeMove(); + int getWasMovingFlag(); + void setWasMovingFlag(int wasMoving); + int getDisableFlag(); + void setDisableFlag(int disableFlag); + double getLastEndOfMoveTime(); + void setLastEndOfMoveTime(double time); + protected: class asynMotorController *pC_; /**< Pointer to the asynMotorController to which this axis belongs. * Abbreviated because it is used very frequently */ @@ -70,6 +77,9 @@ class epicsShareClass asynMotorAxis { MotorStatus status_; int statusChanged_; int referencingModeMove_; + int wasMovingFlag_; + int disableFlag_; + double lastEndOfMoveTime_; friend class asynMotorController; }; diff --git a/motorApp/MotorSrc/asynMotorController.cpp b/motorApp/MotorSrc/asynMotorController.cpp index 9d0adb09..b2f85644 100644 --- a/motorApp/MotorSrc/asynMotorController.cpp +++ b/motorApp/MotorSrc/asynMotorController.cpp @@ -64,6 +64,11 @@ asynMotorController::asynMotorController(const char *portName, int numAxes, int createParam(motorHighLimitString, asynParamFloat64, &motorHighLimit_); createParam(motorLowLimitString, asynParamFloat64, &motorLowLimit_); createParam(motorClosedLoopString, asynParamInt32, &motorClosedLoop_); + createParam(motorPowerAutoOnOffString, asynParamInt32, &motorPowerAutoOnOff_); + createParam(motorPowerOnDelayString, asynParamFloat64, &motorPowerOnDelay_); + createParam(motorPowerOffDelayString, asynParamFloat64, &motorPowerOffDelay_); + createParam(motorPowerOffFractionString, asynParamInt32, &motorPowerOffFraction_); + createParam(motorPostMoveDelayString, asynParamFloat64, &motorPostMoveDelay_); createParam(motorStatusString, asynParamInt32, &motorStatus_); createParam(motorUpdateStatusString, asynParamInt32, &motorUpdateStatus_); createParam(motorStatusDirectionString, asynParamInt32, &motorStatusDirection_); @@ -255,6 +260,8 @@ asynStatus asynMotorController::writeFloat64(asynUser *pasynUser, epicsFloat64 v asynMotorAxis *pAxis; int axis; int forwards; + int autoPower = 0; + double autoPowerOnDelay = 0.0; asynStatus status = asynError; static const char *functionName = "writeFloat64"; @@ -262,6 +269,9 @@ asynStatus asynMotorController::writeFloat64(asynUser *pasynUser, epicsFloat64 v if (!pAxis) return asynError; axis = pAxis->axisNo_; + getIntegerParam(axis, motorPowerAutoOnOff_, &autoPower); + getDoubleParam(axis, motorPowerOnDelay_, &autoPowerOnDelay); + /* Set the parameter and readback in the parameter library. */ status = pAxis->setDoubleParam(function, value); @@ -270,6 +280,10 @@ asynStatus asynMotorController::writeFloat64(asynUser *pasynUser, epicsFloat64 v getDoubleParam(axis, motorAccel_, &acceleration); if (function == motorMoveRel_) { + if (autoPower == 1) { + status = pAxis->setClosedLoop(true); + epicsThreadSleep(autoPowerOnDelay); + } status = pAxis->move(value, 1, baseVelocity, velocity, acceleration); pAxis->setIntegerParam(motorStatusDone_, 0); pAxis->callParamCallbacks(); @@ -279,6 +293,10 @@ asynStatus asynMotorController::writeFloat64(asynUser *pasynUser, epicsFloat64 v driverName, functionName, portName, pAxis->axisNo_, value, baseVelocity, velocity, acceleration ); } else if (function == motorMoveAbs_) { + if (autoPower == 1) { + status = pAxis->setClosedLoop(true); + epicsThreadSleep(autoPowerOnDelay); + } status = pAxis->move(value, 0, baseVelocity, velocity, acceleration); pAxis->setIntegerParam(motorStatusDone_, 0); pAxis->callParamCallbacks(); @@ -288,6 +306,10 @@ asynStatus asynMotorController::writeFloat64(asynUser *pasynUser, epicsFloat64 v driverName, functionName, portName, pAxis->axisNo_, value, baseVelocity, velocity, acceleration ); } else if (function == motorMoveVel_) { + if (autoPower == 1) { + status = pAxis->setClosedLoop(true); + epicsThreadSleep(autoPowerOnDelay); + } status = pAxis->moveVelocity(baseVelocity, value, acceleration); pAxis->setIntegerParam(motorStatusDone_, 0); pAxis->callParamCallbacks(); @@ -298,6 +320,10 @@ asynStatus asynMotorController::writeFloat64(asynUser *pasynUser, epicsFloat64 v // Note, the motorHome command happens on the asynFloat64 interface, even though the value (direction) is really integer } else if (function == motorHome_) { + if (autoPower == 1) { + status = pAxis->setClosedLoop(true); + epicsThreadSleep(autoPowerOnDelay); + } forwards = (value == 0) ? 0 : 1; status = pAxis->home(baseVelocity, velocity, acceleration, forwards); pAxis->setIntegerParam(motorStatusDone_, 0); @@ -569,7 +595,11 @@ void asynMotorController::asynMotorPoller() int forcedFastPolls=0; bool anyMoving; bool moving; + epicsTimeStamp nowTime; + double nowTimeSecs = 0.0; asynMotorAxis *pAxis; + int autoPower = 0; + double autoPowerOffDelay = 0.0; int status; timeout = idlePollPeriod_; @@ -592,12 +622,42 @@ void asynMotorController::asynMotorPoller() unlock(); break; } + poll(); for (i=0; ipoll(&moving); - if (moving) anyMoving = true;; + pAxis=getAxis(i); + if (!pAxis) continue; + + getIntegerParam(i, motorPowerAutoOnOff_, &autoPower); + getDoubleParam(i, motorPowerOffDelay_, &autoPowerOffDelay); + + pAxis->poll(&moving); + if (moving) { + anyMoving = true; + pAxis->setWasMovingFlag(1); + } else { + if ((pAxis->getWasMovingFlag() == 1) && (autoPower == 1)) { + pAxis->setDisableFlag(1); + pAxis->setWasMovingFlag(0); + epicsTimeGetCurrent(&nowTime); + pAxis->setLastEndOfMoveTime(nowTime.secPastEpoch + (nowTime.nsec / 1.e9)); + } + } + + //Auto power off drive, if: + // We have detected an end of move + // We are not moving again + // Auto power off is enabled + // Auto power off delay timer has expired + if ((!moving) && (autoPower == 1) && (pAxis->getDisableFlag() == 1)) { + epicsTimeGetCurrent(&nowTime); + nowTimeSecs = nowTime.secPastEpoch + (nowTime.nsec / 1.e9); + if ((nowTimeSecs - pAxis->getLastEndOfMoveTime()) >= autoPowerOffDelay) { + pAxis->setClosedLoop(0); + pAxis->setDisableFlag(0); + } + } + } if (forcedFastPolls > 0) { timeout = movingPollPeriod_; diff --git a/motorApp/MotorSrc/asynMotorController.h b/motorApp/MotorSrc/asynMotorController.h index e1ff9868..ceba670b 100644 --- a/motorApp/MotorSrc/asynMotorController.h +++ b/motorApp/MotorSrc/asynMotorController.h @@ -38,6 +38,11 @@ #define motorHighLimitString "MOTOR_HIGH_LIMIT" #define motorLowLimitString "MOTOR_LOW_LIMIT" #define motorClosedLoopString "MOTOR_CLOSED_LOOP" +#define motorPowerAutoOnOffString "MOTOR_POWER_AUTO_ONOFF" +#define motorPowerOnDelayString "MOTOR_POWER_ON_DELAY" +#define motorPowerOffDelayString "MOTOR_POWER_OFF_DELAY" +#define motorPowerOffFractionString "MOTOR_POWER_OFF_FRACTION" +#define motorPostMoveDelayString "MOTOR_POST_MOVE_DELAY" #define motorStatusString "MOTOR_STATUS" #define motorUpdateStatusString "MOTOR_UPDATE_STATUS" #define motorStatusDirectionString "MOTOR_STATUS_DIRECTION" @@ -213,6 +218,11 @@ class epicsShareClass asynMotorController : public asynPortDriver { int motorHighLimit_; int motorLowLimit_; int motorClosedLoop_; + int motorPowerAutoOnOff_; + int motorPowerOnDelay_; + int motorPowerOffDelay_; + int motorPowerOffFraction_; + int motorPostMoveDelay_; int motorStatus_; int motorUpdateStatus_;