From c92f7d751802f7fbb6b8c0dc0b2fcb997613e24f Mon Sep 17 00:00:00 2001 From: MarkRivers Date: Fri, 1 Apr 2011 20:00:31 +0000 Subject: [PATCH] Broke asynMotorDriver into asynMotorController and asynMotorAxis --- motorApp/MotorSrc/asynMotorAxis.cpp | 156 +++++++ motorApp/MotorSrc/asynMotorAxis.h | 90 ++++ motorApp/MotorSrc/asynMotorController.cpp | 486 ++++++++++++++++++++++ motorApp/MotorSrc/asynMotorController.h | 216 ++++++++++ 4 files changed, 948 insertions(+) create mode 100644 motorApp/MotorSrc/asynMotorAxis.cpp create mode 100644 motorApp/MotorSrc/asynMotorAxis.h create mode 100644 motorApp/MotorSrc/asynMotorController.cpp create mode 100644 motorApp/MotorSrc/asynMotorController.h diff --git a/motorApp/MotorSrc/asynMotorAxis.cpp b/motorApp/MotorSrc/asynMotorAxis.cpp new file mode 100644 index 00000000..ef17071e --- /dev/null +++ b/motorApp/MotorSrc/asynMotorAxis.cpp @@ -0,0 +1,156 @@ +/* asynMotorAxis.cpp + * + * Mark Rivers + * + * This file defines the base class for an asynMotorAxis. It is the class + * from which real motor axes are derived. + */ +#include + +#include + +#include +#define epicsExportSharedSymbols +#include +#include "asynMotorAxis.h" +#include "asynMotorController.h" + +static const char *driverName = "asynMotorAxis"; +/** Creates a new asynMotorAxis object. + * \param[in] pC Pointer to the asynMotorController to which this axis belongs. + * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. + * + * Checks that pC is not null, and that axisNo is in the valid range. + * Sets a pointer to itself in pC->pAxes[axisNo_]. + * Connects pasynUser_ to this asyn port and axisNo. + */ +asynMotorAxis::asynMotorAxis(class asynMotorController *pC, int axisNo) + : pC_(pC), axisNo_(axisNo), statusChanged_(1) +{ + static const char *functionName = "asynMotorAxis::asynMotorAxis"; + + if (!pC) { + printf("%s:%s: Error, controller is NULL\n", + driverName, functionName); + return; + } + if ((axisNo < 0) || (axisNo >= pC->numAxes_)) { + printf("%s:%s: Error, axis=%d is not in range 0 to %d\n", + driverName, functionName, axisNo, pC->numAxes_-1); + return; + } + pC->pAxes_[axisNo] = this; + status_.status = 0; + profilePositions_ = NULL; + profilePositionsRBV_ = NULL; + profileFollowingErrors_ = NULL; + + // Create the asynUser, connect to this axis + pasynUser_ = pasynManager->createAsynUser(NULL, NULL); + pasynManager->connectDevice(pasynUser_, pC->portName, axisNo); +} + + +// We implement the setIntegerParam, setDoubleParam, and callParamCallbacks methods so we can construct +// the aggregate status structure and do callbacks on it + +/** Sets the value for an integer for this axis in the parameter library. + * This function takes special action if the parameter is one of the motorStatus parameters + * (motorStatusDirection_, motorStatusHomed_, etc.). In that case it sets or clears the appropriate + * bit in its private MotorStatus.status structure and if that status has changed sets a flag to + * do callbacks to devMotorAsyn when callParamCallbacks() is called. + * \param[in] function The function (parameter) number + * \param[in] value Value to set */ +asynStatus asynMotorAxis::setIntegerParam(int function, int value) +{ + int mask; + epicsUInt32 status; + // This assumes the parameters defined above are in the same order as the bits the motor record expects! + if (function >= pC_->motorStatusDirection_ && + function <= pC_->motorStatusHomed_) { + status = status_.status; + mask = 1 << (function - pC_->motorStatusDirection_); + if (value) status |= mask; + else status &= ~mask; + if (status != status_.status) { + status_.status = status; + statusChanged_ = 1; + } + } + // Call the base class method + return pC_->setIntegerParam(axisNo_, function, value); +} + +/** Sets the value for a double for this axis in the parameter library. + * This function takes special action if the parameter is motorPosition_ or motorEncoderPosition_. + * In that case it sets the value in the private MotorStatus structure and if the value has changed + * then sets a flag to do callbacks to devMotorAsyn when callParamCallbacks() is called. + * \param[in] function The function (parameter) number + * \param[in] value Value to set */ +asynStatus asynMotorAxis::setDoubleParam(int function, double value) +{ + if (function == pC_->motorPosition_) { + if (value != status_.position) { + statusChanged_ = 1; + status_.position = value; + } + } else if (function == pC_->motorEncoderPosition_) { + if (value != status_.encoderPosition) { + statusChanged_ = 1; + status_.encoderPosition = value; + } + } + // Call the base class method + return pC_->setDoubleParam(axisNo_, function, value); +} + +/** Calls the callbacks for any parameters that have changed for this axis in the parameter library. + * This function takes special action if the aggregate MotorStatus structure has changed. + * In that case it does callbacks on the asynGenericPointer interface, typically to devMotorAsyn. */ +asynStatus asynMotorAxis::callParamCallbacks() +{ + if (statusChanged_) { + statusChanged_ = 0; + pC_->doCallbacksGenericPointer((void *)&status_, pC_->motorStatus_, axisNo_); + } + return pC_->callParamCallbacks(axisNo_); +} + +/* These are the functions for profile moves */ +asynStatus asynMotorAxis::initializeProfile(int maxProfilePoints) +{ + if (profilePositions_) free(profilePositions_); + profilePositions_ = (double *)calloc(maxProfilePoints, sizeof(double)); + if (profilePositionsRBV_) free(profilePositionsRBV_); + profilePositionsRBV_ = (double *)calloc(maxProfilePoints, sizeof(double)); + if (profileFollowingErrors_) free(profileFollowingErrors_); + profileFollowingErrors_ = (double *)calloc(maxProfilePoints, sizeof(double)); + return asynSuccess; +} + +/** Function to build a coordinated move of multiple axes. + * This is not currently implemented, as the API still needs work! */ +asynStatus asynMotorAxis::buildProfile() +{ + // static const char *functionName = "asynMotorController::buildProfile"; + + return asynSuccess; +} + +/** Function to execute a coordinated move of multiple axes. + * This is not currently implemented, as the API still needs work! */ +asynStatus asynMotorAxis::executeProfile() +{ + // static const char *functionName = "asynMotorController::executeProfile"; + + return asynSuccess; +} + +/** Function to readback the actual motor positions from a coordinated move of multiple axes. + * This is not currently implemented, as the API still needs work! */ +asynStatus asynMotorAxis::readbackProfile() +{ + // static const char *functionName = "asynMotorController::readbackProfile"; + + return asynSuccess; +} diff --git a/motorApp/MotorSrc/asynMotorAxis.h b/motorApp/MotorSrc/asynMotorAxis.h new file mode 100644 index 00000000..b19a3d91 --- /dev/null +++ b/motorApp/MotorSrc/asynMotorAxis.h @@ -0,0 +1,90 @@ +/* asynMotorAxis.h + * + * Mark Rivers + * + * This file defines the base class for an asynMotoAxis. It is the class + * from which real motor axes are derived. + */ +#ifndef asynMotorAxis_H +#define asynMotorAxis_H + +#include +#include + +#ifdef __cplusplus +#include + +#include "asynMotorController.h" + +/** Class from which motor axis objects are derived. */ +class epicsShareFunc asynMotorAxis { +public: + /* This is the constructor for the class. */ + asynMotorAxis(class asynMotorController *pController, int axisNumber); + + virtual asynStatus setIntegerParam(int index, int value); + virtual asynStatus setDoubleParam(int index, double value); + virtual asynStatus callParamCallbacks(); + + // These are pure virtual functions which derived classes must implement + /** Move the motor to an absolute location or by a relative amount. + * \param[in] position The absolute position to move to (if relative=0) or the relative distance to move + * by (if relative=1). Units=steps. + * \param[in] relative Flag indicating relative move (1) or absolute move (0). + * \param[in] minVelocity The initial velocity, often called the base velocity. Units=steps/sec. + * \param[in] maxVelocity The maximum velocity, often called the slew velocity. Units=steps/sec. + * \param[in] acceleration The acceleration value. Units=steps/sec/sec. */ + virtual asynStatus move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) = 0; + + /** Move the motor at a fixed velocity until told to stop. + * \param[in] minVelocity The initial velocity, often called the base velocity. Units=steps/sec. + * \param[in] maxVelocity The maximum velocity, often called the slew velocity. Units=steps/sec. + * \param[in] acceleration The acceleration value. Units=steps/sec/sec. */ + virtual asynStatus moveVelocity(double minVelocity, double maxVelocity, double acceleration) = 0; + + /** Move the motor to the home position. + * \param[in] minVelocity The initial velocity, often called the base velocity. Units=steps/sec. + * \param[in] maxVelocity The maximum velocity, often called the slew velocity. Units=steps/sec. + * \param[in] acceleration The acceleration value. Units=steps/sec/sec. + * \param[in] forwards Flag indicating to move the motor in the forward direction(1) or reverse direction(0). + * Some controllers need to be told the direction, others know which way to go to home. */ + virtual asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards) = 0; + + /** Stop the motor. + * \param[in] acceleration The acceleration value. Units=steps/sec/sec. */ + virtual asynStatus stop(double acceleration) = 0; + + /** Poll the axis. + * This function should read the controller position, encoder position, and as many of the motorStatus flags + * as the hardware supports. It should call setIntegerParam() and setDoubleParam() for each item that it polls, + * and then call callParamCallbacks() at the end. + * \param[out] moving A flag that the function must set indicating that the axis is moving (1) or done (0). */ + virtual asynStatus poll(int *moving) = 0; + + /** Set the current position of the motor. + * \param[in] position The new absolute motor position that should be set in the hardware. Units=steps.*/ + virtual asynStatus setPosition(double position) = 0; + + virtual asynStatus initializeProfile(int maxPoints); + virtual asynStatus buildProfile(); + virtual asynStatus executeProfile(); + virtual asynStatus readbackProfile(); + +protected: + class asynMotorController *pC_; /**< Pointer to the asynMotorController to which this axis belongs. + * Abbreviated because it is used very frequently */ + int axisNo_; /**< Index number of this axis (0 - pC_->numAxes_-1) */ + asynUser *pasynUser_; /**< asynUser connected to this axis for asynTrace debugging */ + +private: + MotorStatus status_; + int statusChanged_; + double *profilePositions_; /**< Array of target positions for profile moves */ + double *profilePositionsRBV_; /**< Array of readback positions for profile moves */ + double *profileFollowingErrors_; /**< Array of following errors for profile moves */ + +friend class asynMotorController; +}; + +#endif /* _cplusplus */ +#endif /* asynMotorAxis_H */ diff --git a/motorApp/MotorSrc/asynMotorController.cpp b/motorApp/MotorSrc/asynMotorController.cpp new file mode 100644 index 00000000..1c7f79b3 --- /dev/null +++ b/motorApp/MotorSrc/asynMotorController.cpp @@ -0,0 +1,486 @@ +/* asynMotorController.cpp + * + * Mark Rivers + * + * This file defines the base class for an asynMotorController. It is the class + * from which real motor controllers are derived. It derives from asynPortDriver. + */ +#include + +#include + +#include +#define epicsExportSharedSymbols +#include +#include "asynMotorController.h" +#include "asynMotorAxis.h" + +static const char *driverName = "asynMotorController"; +static void asynMotorPollerC(void *drvPvt); + +/** Creates a new asynMotorController object. + * All of the arguments are simply passed to the constructor for the asynPortDriver base class. + * After calling the base class constructor this method creates the motor parameters + * defined in asynMotorDriver.h. + */ +asynMotorController::asynMotorController(const char *portName, int numAxes, int numParams, + int interfaceMask, int interruptMask, + int asynFlags, int autoConnect, int priority, int stackSize) + + : asynPortDriver(portName, numAxes, NUM_MOTOR_DRIVER_PARAMS+numParams, + interfaceMask | asynInt32Mask | asynFloat64Mask | asynFloat64ArrayMask | asynGenericPointerMask | asynDrvUserMask, + interruptMask | asynInt32Mask | asynFloat64Mask | asynFloat64ArrayMask | asynGenericPointerMask, + asynFlags, autoConnect, priority, stackSize), + numAxes_(numAxes) + +{ + static const char *functionName = "asynMotorController::asynMotorController"; + + /* Create the base set of motor parameters */ + createParam(motorMoveRelString, asynParamFloat64, &motorMoveRel_); + createParam(motorMoveAbsString, asynParamFloat64, &motorMoveAbs_); + createParam(motorMoveVelString, asynParamFloat64, &motorMoveVel_); + createParam(motorHomeString, asynParamFloat64, &motorHome_); + createParam(motorStopString, asynParamInt32, &motorStop_); + createParam(motorVelocityString, asynParamFloat64, &motorVelocity_); + createParam(motorVelBaseString, asynParamFloat64, &motorVelBase_); + createParam(motorAccelString, asynParamFloat64, &motorAccel_); + createParam(motorPositionString, asynParamFloat64, &motorPosition_); + createParam(motorEncoderPositionString, asynParamFloat64, &motorEncoderPosition_); + createParam(motorDeferMovesString, asynParamInt32, &motorDeferMoves_); + createParam(motorResolutionString, asynParamFloat64, &motorResolution_); + createParam(motorEncRatioString, asynParamFloat64, &motorEncRatio_); + createParam(motorPgainString, asynParamFloat64, &motorPgain_); + createParam(motorIgainString, asynParamFloat64, &motorIgain_); + createParam(motorDgainString, asynParamFloat64, &motorDgain_); + createParam(motorHighLimitString, asynParamFloat64, &motorHighLimit_); + createParam(motorLowLimitString, asynParamFloat64, &motorLowLimit_); + createParam(motorSetClosedLoopString, asynParamInt32, &motorSetClosedLoop_); + createParam(motorStatusString, asynParamInt32, &motorStatus_); + createParam(motorUpdateStatusString, asynParamInt32, &motorUpdateStatus_); + createParam(motorStatusDirectionString, asynParamInt32, &motorStatusDirection_); + createParam(motorStatusDoneString, asynParamInt32, &motorStatusDone_); + createParam(motorStatusHighLimitString, asynParamInt32, &motorStatusHighLimit_); + createParam(motorStatusAtHomeString, asynParamInt32, &motorStatusAtHome_); + createParam(motorStatusSlipString, asynParamInt32, &motorStatusSlip_); + createParam(motorStatusPowerOnString, asynParamInt32, &motorStatusPowerOn_); + createParam(motorStatusFollowingErrorString, asynParamInt32, &motorStatusFollowingError_); + createParam(motorStatusHomeString, asynParamInt32, &motorStatusHome_); + createParam(motorStatusHasEncoderString, asynParamInt32, &motorStatusHasEncoder_); + createParam(motorStatusProblemString, asynParamInt32, &motorStatusProblem_); + createParam(motorStatusMovingString, asynParamInt32, &motorStatusMoving_); + createParam(motorStatusGainSupportString, asynParamInt32, &motorStatusGainSupport_); + createParam(motorStatusCommsErrorString, asynParamInt32, &motorStatusCommsError_); + createParam(motorStatusLowLimitString, asynParamInt32, &motorStatusLowLimit_); + createParam(motorStatusHomedString, asynParamInt32, &motorStatusHomed_); + + // These are the per-controller parameters for profile moves + createParam(profileNumAxesString, asynParamInt32, &profileNumAxes_); + createParam(profileNumSegmentsString, asynParamInt32, &profileNumSegments_); + createParam(profileNumPulsesString, asynParamInt32, &profileNumPulses_); + createParam(profileStartPulsesString, asynParamInt32, &profileStartPulses_); + createParam(profileEndPulsesString, asynParamInt32, &profileEndPulses_); + createParam(profileActualPulsesString, asynParamInt32, &profileActualPulses_); + createParam(profileTimeArrayString, asynParamFloat64Array, &profileTimeArray_); + createParam(profileAccelString, asynParamFloat64, &profileAccel_); + createParam(profileBuildString, asynParamInt32, &profileBuild_); + createParam(profileBuildStateString, asynParamInt32, &profileBuildState_); + createParam(profileBuildStatusString, asynParamOctet, &profileBuildStatus_); + createParam(profileBuildMessageString, asynParamOctet, &profileBuildMessage_); + createParam(profileExecString, asynParamInt32, &profileExec_); + createParam(profileExecStateString, asynParamInt32, &profileExecState_); + createParam(profileExecStatusString, asynParamOctet, &profileExecStatus_); + createParam(profileExecMessageString, asynParamOctet, &profileExecMessage_); + createParam(profileReadbackString, asynParamInt32, &profileReadback_); + createParam(profileReadbackStateString, asynParamInt32, &profileReadbackState_); + createParam(profileReadbackStatusString, asynParamOctet, &profileReadbackStatus_); + createParam(profileReadbackMessageString, asynParamOctet, &profileReadbackMessage_); + + // These are the per-axis parameters for profile moves + createParam(profileUseAxisString, asynParamInt32, &profileUseAxis_); + createParam(profilePositionString, asynParamFloat64Array, &profilePosition_); + createParam(profilePositionRBVString, asynParamFloat64Array, &profilePositionRBV_); + createParam(profileFollowingErrorString,asynParamFloat64Array, &profileFollowingError_); + createParam(profileMotorDirectionString, asynParamInt32, &profileMotorDirection_); + createParam(profileMotorOffsetString, asynParamFloat64, &profileMotorOffset_); + + pAxes_ = (asynMotorAxis**) calloc(numAxes, sizeof(asynMotorAxis*)); + pollEventId_ = epicsEventMustCreate(epicsEventEmpty); + + maxProfilePoints_ = 0; + profileTimes_ = NULL; + + asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, + "%s:%s: constructor complete\n", + driverName, functionName); +} + +/** Called when asyn clients call pasynInt32->write(). + * Extracts the function and axis number from pasynUser. + * Sets the value in the parameter library. + * If the function is motorStop_ then it calls pAxis->stop(). + * If the function is motorUpdateStatus_ then it does a poll and forces a callback. + * Calls any registered callbacks for this pasynUser->reason and address. + * Motor drivers will reimplement this function if they support + * controller-specific parameters on the asynInt32 interface. They should call this + * base class method for any parameters that are not controller-specific. + * \param[in] pasynUser asynUser structure that encodes the reason and address. + * \param[in] value Value to write. */ +asynStatus asynMotorController::writeInt32(asynUser *pasynUser, epicsInt32 value) +{ + int axis; + int function = pasynUser->reason; + asynStatus status=asynSuccess; + asynMotorAxis *pAxis; + double accel; + int moving; + static const char *functionName = "asynMotorController::writeInt32"; + + pAxis = getAxis(pasynUser); + if (!pAxis) return asynError; + axis = pAxis->axisNo_; + + /* Set the parameter and readback in the parameter library. */ + pAxis->setIntegerParam(function, value); + + if (function == motorStop_) { + getDoubleParam(axis, motorAccel_, &accel); + status = pAxis->stop(accel); + + } else if (function == motorUpdateStatus_) { + /* Do a poll, and then force a callback */ + poll(); + pAxis->poll(&moving); + pAxis->statusChanged_ = 1; + } + + /* Do callbacks so higher layers see any changes */ + pAxis->callParamCallbacks(); + if (status) + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "%s:%s error, status=%d axis=%d, function=%d, value=%d\n", + driverName, functionName, status, axis, function, value); + else + asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, + "%s:%s:: axis=%d, function=%d, value=%d\n", + driverName, functionName, axis, function, value); + return status; +} + +/** Called when asyn clients call pasynFloat64->write(). + * Extracts the function and axis number from pasynUser. + * Sets the value in the parameter library. + * If the function is motorMoveRel_, motorMoveAbs_, motorMoveVel_, motorHome_, or motorPosition_, + * then it calls pAxis->move(), pAxis->moveVelocity(), pAxis->home(), or pAxis->setPosition(). + * Calls any registered callbacks for this pasynUser->reason and address. + * Motor drivers will reimplement this function if they support + * controller-specific parameters on the asynFloat64 interface. They should call this + * base class method for any parameters that are not controller-specific. + * \param[in] pasynUser asynUser structure that encodes the reason and address. + * \param[in] value Value to write. */ +asynStatus asynMotorController::writeFloat64(asynUser *pasynUser, epicsFloat64 value) +{ + int function = pasynUser->reason; + double baseVelocity, velocity, acceleration; + asynMotorAxis *pAxis; + int axis; + int forwards; + asynStatus status = asynError; + static const char *functionName = "asynMotorController::writeFloat64"; + + pAxis = getAxis(pasynUser); + if (!pAxis) return asynError; + axis = pAxis->axisNo_; + + /* Set the parameter and readback in the parameter library. */ + status = pAxis->setDoubleParam(function, value); + + getDoubleParam(axis, motorVelBase_, &baseVelocity); + getDoubleParam(axis, motorVelocity_, &velocity); + getDoubleParam(axis, motorAccel_, &acceleration); + + if (function == motorMoveRel_) { + status = pAxis->move(value, 1, baseVelocity, velocity, acceleration); + pAxis->setIntegerParam(motorStatusDone_, 0); + pAxis->callParamCallbacks(); + wakeupPoller(); + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s: Set driver %s, axis %d move relative by %f, base velocity=%f, velocity=%f, acceleration=%f\n", + driverName, functionName, portName, pAxis->axisNo_, value, baseVelocity, velocity, acceleration ); + + } else if (function == motorMoveAbs_) { + status = pAxis->move(value, 0, baseVelocity, velocity, acceleration); + pAxis->setIntegerParam(motorStatusDone_, 0); + pAxis->callParamCallbacks(); + wakeupPoller(); + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s: Set driver %s, axis %d move absolute to %f, base velocity=%f, velocity=%f, acceleration=%f\n", + driverName, functionName, portName, pAxis->axisNo_, value, baseVelocity, velocity, acceleration ); + + } else if (function == motorMoveVel_) { + status = pAxis->moveVelocity(baseVelocity, value, acceleration); + pAxis->setIntegerParam(motorStatusDone_, 0); + pAxis->callParamCallbacks(); + wakeupPoller(); + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s: Set port %s, axis %d move with velocity of %f, acceleration=%f\n", + driverName, functionName, portName, pAxis->axisNo_, value, acceleration); + + // Note, the motorHome command happens on the asynFloat64 interface, even though the value (direction) is really integer + } else if (function == motorHome_) { + forwards = (value == 0) ? 0 : 1; + status = pAxis->home(baseVelocity, velocity, acceleration, forwards); + pAxis->setIntegerParam(motorStatusDone_, 0); + pAxis->callParamCallbacks(); + wakeupPoller(); + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s: Set driver %s, axis %d to home %s, base velocity=%f, velocity=%f, acceleration=%f\n", + driverName, functionName, portName, pAxis->axisNo_, (forwards?"FORWARDS":"REVERSE"), baseVelocity, velocity, acceleration); + + } else if (function == motorPosition_) { + status = pAxis->setPosition(value); + pAxis->callParamCallbacks(); + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s: Set driver %s, axis %d to position=%f\n", + driverName, functionName, portName, pAxis->axisNo_, value); + + } + /* Do callbacks so higher layers see any changes */ + pAxis->callParamCallbacks(); + + if (status) + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "%s:%s error, status=%d axis=%d, function=%d, value=%f\n", + driverName, functionName, status, axis, function, value); + else + asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, + "%s:%s:: axis=%d, function=%d, value=%f\n", + driverName, functionName, axis, function, value); + return status; + +} + +/** Called when asyn clients call pasynFloat64Array->write(). + * The base class implementation simply prints an error message. + * Derived classes may reimplement this function if required. + * \param[in] pasynUser pasynUser structure that encodes the reason and address. + * \param[in] value Pointer to the array to write. + * \param[in] nElements Number of elements to write. */ +asynStatus asynMotorController::writeFloat64Array(asynUser *pasynUser, epicsFloat64 *value, + size_t nElements) +{ + static const char *functionName = "asynMotorController::writeFloat64Array"; + + asynPrint(pasynUserSelf, ASYN_TRACE_ERROR, + "%s:%s: not implemented in this driver\n", + driverName, functionName); + return asynError ; +} + +/** Called when asyn clients call pasynGenericPointer->read(). + * Builds an aggregate MotorStatus structure at the memory location of the + * input pointer. + * \param[in] pasynUser asynUser structure that encodes the reason and address. + * \param[in] pointer Pointer to the MotorStatus object to return. */ +asynStatus asynMotorController::readGenericPointer(asynUser *pasynUser, void *pointer) +{ + static const char *functionName = "asynMotorController::readGenericPointer"; + MotorStatus *pStatus = (MotorStatus *)pointer; + int axis; + + getAddress(pasynUser, &axis); + getIntegerParam(axis, motorStatus_, (int *)&pStatus->status); + getDoubleParam(axis, motorPosition_, &pStatus->position); + getDoubleParam(axis, motorEncoderPosition_, &pStatus->encoderPosition); + getDoubleParam(axis, motorVelocity_, &pStatus->velocity); + asynPrint(pasynUser, ASYN_TRACE_FLOW, + "%s:%s: MotorStatus = status%d, position=%f, encoder position=%f, velocity=%f\n", + driverName, functionName, pStatus->status, pStatus->position, pStatus->encoderPosition, pStatus->velocity); + return asynSuccess; +} + +/** Returns a pointer to an asynMotorAxis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * Derived classes will reimplement this function to return a pointer to the derived + * axis type. + * \param[in] pasynUser asynUser structure that encodes the axis index number. */ +asynMotorAxis* asynMotorController::getAxis(asynUser *pasynUser) +{ + int axisNo; + + getAddress(pasynUser, &axisNo); + return getAxis(axisNo); +} + +/** Returns a pointer to an asynMotorAxis object. + * Returns NULL if the axis number is invalid. + * Derived classes will reimplement this function to return a pointer to the derived + * axis type. + * \param[in] axisNo Axis index number. */ +asynMotorAxis* asynMotorController::getAxis(int axisNo) +{ + if ((axisNo < 0) || (axisNo >= numAxes_)) return NULL; + return pAxes_[axisNo]; +} + +/** Starts the motor poller thread. + * Derived classes will typically call this at near the end of their constructor. + * Derived classes can typically use the base class implementation of the poller thread, + * but are free to reimplement it if necessary. + * \param[in] movingPollPeriod The time between polls when any axis is moving. + * \param[in] idlePollPeriod The time between polls when no axis is moving. + * \param[in] forcedFastPolls The number of times to force the movingPollPeriod after waking up the poller. + * This can need to be non-zero for controllers that do not immediately + * report that an axis is moving after it has been told to start. */ +asynStatus asynMotorController::startPoller(double movingPollPeriod, double idlePollPeriod, int forcedFastPolls) +{ + movingPollPeriod_ = movingPollPeriod; + idlePollPeriod_ = idlePollPeriod; + forcedFastPolls_ = forcedFastPolls; + epicsThreadCreate("motorPoller", + epicsThreadPriorityLow, + epicsThreadGetStackSize(epicsThreadStackMedium), + (EPICSTHREADFUNC)asynMotorPollerC, (void *)this); + return asynSuccess; +} + +/** Wakes up the poller thread to make it start polling at the movingPollingPeriod_. + * This is typically called after an axis has been told to move, so the poller immediately + * starts polling quickly. */ +asynStatus asynMotorController::wakeupPoller() +{ + epicsEventSignal(pollEventId_); + return asynSuccess; +} + +/** Polls the asynMotorController (not a specific asynMotorAxis). + * The base class asynMotorPoller thread calls this method once just before it calls asynMotorAxis::poll + * for each axis. + * This base class implementation does nothing. Derived classes can implement this method if there + * are controller-wide parameters that need to be polled. It can also be used for efficiency in some + * cases. For example some controllers can return the status or positions for all axes in a single + * command. In that case asynMotorController::poll() could read that information, and then + * asynMotorAxis::poll() might just extract the axis-specific information from the result. */ +asynStatus asynMotorController::poll() +{ + return asynSuccess; +} + +static void asynMotorPollerC(void *drvPvt) +{ + asynMotorController *pController = (asynMotorController*)drvPvt; + pController->asynMotorPoller(); +} + + +/** Default poller function that runs in the thread created by asynMotorController::startPoller(). + * This base class implementation can be used by most derived classes. + * It polls at the idlePollPeriod_ when no axes are moving, and at the movingPollPeriod_ when + * any axis is moving. It will immediately do a poll when asynMotorController::wakeupPoller() is + * called, and will then do forcedFastPolls_ loops at the movingPollPeriod, before reverting back + * to the idlePollPeriod_ if no axes are moving. It takes the lock on the port driver when it is polling. + */ +void asynMotorController::asynMotorPoller() +{ + double timeout; + int i; + int forcedFastPolls=0; + int anyMoving; + int moving; + asynMotorAxis *pAxis; + int status; + + timeout = idlePollPeriod_; + wakeupPoller(); /* Force on poll at startup */ + + while(1) { + if (timeout != 0.) status = epicsEventWaitWithTimeout(pollEventId_, timeout); + else status = epicsEventWait(pollEventId_); + if (status == epicsEventWaitOK) { + /* We got an event, rather than a timeout. This is because other software + * knows that an axis should have changed state (started moving, etc.). + * Force a minimum number of fast polls, because the controller status + * might not have changed the first few polls + */ + forcedFastPolls = forcedFastPolls_; + } + anyMoving = 0; + lock(); + this->poll(); + for (i=0; ipoll(&moving); + if (moving) anyMoving=1; + } + unlock(); + if (forcedFastPolls > 0) { + timeout = movingPollPeriod_; + forcedFastPolls--; + } else if (anyMoving) { + timeout = movingPollPeriod_; + } else { + timeout = idlePollPeriod_; + } + } +} + +/* These are the functions for profile moves */ +/** Initialize a profile move of multiple axes. */ +asynStatus asynMotorController::initializeProfile(int maxProfilePoints) +{ + int axis; + asynMotorAxis *pAxis; + // static const char *functionName = "asynMotorController::initializeProfile"; + + maxProfilePoints_ = maxProfilePoints; + if (profileTimes_) free(profileTimes_); + profileTimes_ = (double *)calloc(maxProfilePoints, sizeof(double)); + for (axis=0; axisinitializeProfile(maxProfilePoints); + } + return asynSuccess; +} + +/** Build a profile move of multiple axes. */ +asynStatus asynMotorController::buildProfile() +{ + // static const char *functionName = "asynMotorController::buildProfile"; + int axis; + asynMotorAxis *pAxis; + + for (axis=0; axisbuildProfile(); + } + return asynSuccess; +} + +/** Execute a profile move of multiple axes. */ +asynStatus asynMotorController::executeProfile() +{ + // static const char *functionName = "asynMotorController::executeProfile"; + int axis; + asynMotorAxis *pAxis; + + for (axis=0; axisexecuteProfile(); + } + return asynSuccess; +} + +/** Readback the actual motor positions from a profile move of multiple axes. */ +asynStatus asynMotorController::readbackProfile() +{ + // static const char *functionName = "asynMotorController::readbackProfile"; + int axis; + asynMotorAxis *pAxis; + + for (axis=0; axisreadbackProfile(); + } + return asynSuccess; +} diff --git a/motorApp/MotorSrc/asynMotorController.h b/motorApp/MotorSrc/asynMotorController.h new file mode 100644 index 00000000..9a02256f --- /dev/null +++ b/motorApp/MotorSrc/asynMotorController.h @@ -0,0 +1,216 @@ +/* asynMotorController.h + * + * Mark Rivers + * + * This file defines the base class for an asynMotorController. It is the class + * from which real motor controllers are derived. It derives from asynPortDriver. + */ +#ifndef asynMotorController_H +#define asynMotorController_H + +#include +#include + +/** Strings defining parameters for the driver. + * These are the values passed to drvUserCreate. + * The driver will place in pasynUser->reason an integer to be used when the + * standard asyn interface methods are called. */ +#define motorMoveRelString "MOTOR_MOVE_REL" +#define motorMoveAbsString "MOTOR_MOVE_ABS" +#define motorMoveVelString "MOTOR_MOVE_VEL" +#define motorHomeString "MOTOR_HOME" +#define motorStopString "MOTOR_STOP_AXIS" +#define motorVelocityString "MOTOR_VELOCITY" +#define motorVelBaseString "MOTOR_VEL_BASE" +#define motorAccelString "MOTOR_ACCEL" +#define motorPositionString "MOTOR_POSITION" +#define motorEncoderPositionString "MOTOR_ENCODER_POSITION" +#define motorDeferMovesString "MOTOR_DEFER_MOVES" +#define motorResolutionString "MOTOR_RESOLUTION" +#define motorEncRatioString "MOTOR_ENC_RATIO" +#define motorPgainString "MOTOR_PGAIN" +#define motorIgainString "MOTOR_IGAIN" +#define motorDgainString "MOTOR_DGAIN" +#define motorHighLimitString "MOTOR_HIGH_LIMIT" +#define motorLowLimitString "MOTOR_LOW_LIMIT" +#define motorSetClosedLoopString "MOTOR_SET_CLOSED_LOOP" +#define motorStatusString "MOTOR_STATUS" +#define motorUpdateStatusString "MOTOR_UPDATE_STATUS" +#define motorStatusDirectionString "MOTOR_STATUS_DIRECTION" +#define motorStatusDoneString "MOTOR_STATUS_DONE" +#define motorStatusHighLimitString "MOTOR_STATUS_HIGH_LIMIT" +#define motorStatusAtHomeString "MOTOR_STATUS_AT_HOME" +#define motorStatusSlipString "MOTOR_STATUS_SLIP" +#define motorStatusPowerOnString "MOTOR_STATUS_POWERED" +#define motorStatusFollowingErrorString "MOTOR_STATUS_FOLLOWING_ERROR" +#define motorStatusHomeString "MOTOR_STATUS_HOME" +#define motorStatusHasEncoderString "MOTOR_STATUS_HAS_ENCODER" +#define motorStatusProblemString "MOTOR_STATUS_PROBLEM" +#define motorStatusMovingString "MOTOR_STATUS_MOVING" +#define motorStatusGainSupportString "MOTOR_STATUS_GAIN_SUPPORT" +#define motorStatusCommsErrorString "MOTOR_STATUS_COMMS_ERROR" +#define motorStatusLowLimitString "MOTOR_STATUS_LOW_LIMIT" +#define motorStatusHomedString "MOTOR_STATUS_HOMED" + +// These are the per-controller parameters for profile moves (coordinated motion) +#define profileNumAxesString "PROFILE_NUM_AXES" +#define profileNumSegmentsString "PROFILE_NUM_SEGMENTS" +#define profileNumPulsesString "PROFILE_NUM_PULSES" +#define profileStartPulsesString "PROFILE_START_PULSES" +#define profileEndPulsesString "PROFILE_START_PULSES" +#define profileActualPulsesString "PROFILE_ACTUAL_PULSES" +#define profileTimeArrayString "PROFILE_TIME_ARRAY" +#define profileAccelString "PROFILE_ACCEL" +#define profileBuildString "PROFILE_BUILD_START" +#define profileBuildStateString "PROFILE_BUILD_STATE" +#define profileBuildStatusString "PROFILE_BUILD_STATUS" +#define profileBuildMessageString "PROFILE_BUILD_MESSAGE" +#define profileExecString "PROFILE_EXEC_START" +#define profileExecStateString "PROFILE_EXEC_STATE" +#define profileExecStatusString "PROFILE_EXEC_STATUS" +#define profileExecMessageString "PROFILE_EXEC_MESSAGE" +#define profileReadbackString "PROFILE_EXEC_START" +#define profileReadbackStateString "PROFILE_EXEC_STATE" +#define profileReadbackStatusString "PROFILE_EXEC_STATUS" +#define profileReadbackMessageString "PROFILE_EXEC_MESSAGE" + +// These are the per-axis parameters for profile moves +#define profileUseAxisString "PROFILE_USE_AXIS" +#define profilePositionString "PROFILE_POSITION" +#define profilePositionRBVString "PROFILE_POSITION_RBV" +#define profileFollowingErrorString "PROFILE_FOLLOWING_ERROR" +#define profileMotorDirectionString "PROFILE_MOTOR_DIRECTION" +#define profileMotorOffsetString "PROFILE_MOTOR_OFFSET" + +/** The structure that is passed back to devMotorAsyn when the status changes. */ +typedef struct MotorStatus { + double position; /**< Commanded motor position */ + double encoderPosition; /**< Actual encoder position */ + double velocity; /**< Actual velocity */ + epicsUInt32 status; /**< Word containing status bits (motion done, limits, etc.) */ +} MotorStatus; + +#ifdef __cplusplus +#include + +class asynMotorAxis; + +class epicsShareFunc asynMotorController : public asynPortDriver { +public: + /* This is the constructor for the class. */ + asynMotorController(const char *portName, int numAxes, int numParams, + int interfaceMask, int interruptMask, + int asynFlags, int autoConnect, int priority, int stackSize); + + /* These are the methods that we override from asynPortDriver */ + virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + virtual asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value); + virtual asynStatus writeFloat64Array(asynUser *pasynUser, epicsFloat64 *value, size_t nEelements); + virtual asynStatus readGenericPointer(asynUser *pasynUser, void *pointer); + + /* These are the methods that are new to this class */ + virtual asynMotorAxis* getAxis(asynUser *pasynUser); + virtual asynMotorAxis* getAxis(int axisNo); + virtual asynStatus startPoller(double movingPollPeriod, double idlePollPeriod, int forcedFastPolls); + virtual asynStatus wakeupPoller(); + virtual asynStatus poll(); + void asynMotorPoller(); // This should be private but is called from C function + + /* These are the functions for profile moves */ + virtual asynStatus initializeProfile(int maxPoints); + virtual asynStatus buildProfile(); + virtual asynStatus executeProfile(); + virtual asynStatus readbackProfile(); + +protected: + /** These are the index numbers for the parameters in the parameter library. + * They are the values of pasynUser->reason in calls from device support */ + // These are the motor commands + int motorMoveRel_; + #define FIRST_MOTOR_PARAM motorMoveRel_ + int motorMoveAbs_; + int motorMoveVel_; + int motorHome_; + int motorStop_; + int motorVelocity_; + int motorVelBase_; + int motorAccel_; + int motorPosition_; + int motorEncoderPosition_; + int motorDeferMoves_; + int motorResolution_; + int motorEncRatio_; + int motorPgain_; + int motorIgain_; + int motorDgain_; + int motorHighLimit_; + int motorLowLimit_; + int motorSetClosedLoop_; + int motorStatus_; + int motorUpdateStatus_; + + // These are the status bits + int motorStatusDirection_; + int motorStatusDone_; + int motorStatusHighLimit_; + int motorStatusAtHome_; + int motorStatusSlip_; + int motorStatusPowerOn_; + int motorStatusFollowingError_; + int motorStatusHome_; + int motorStatusHasEncoder_; + int motorStatusProblem_; + int motorStatusMoving_; + int motorStatusGainSupport_; + int motorStatusCommsError_; + int motorStatusLowLimit_; + int motorStatusHomed_; + + // These are the per-controller parameters for profile moves + int profileNumAxes_; + int profileNumSegments_; + int profileNumPulses_; + int profileStartPulses_; + int profileEndPulses_; + int profileActualPulses_; + int profileTimeArray_; + int profileAccel_; + int profileBuild_; + int profileBuildState_; + int profileBuildStatus_; + int profileBuildMessage_; + int profileExec_; + int profileExecState_; + int profileExecStatus_; + int profileExecMessage_; + int profileReadback_; + int profileReadbackState_; + int profileReadbackStatus_; + int profileReadbackMessage_; + + // These are the per-axis parameters for profile moves + int profileUseAxis_; + int profilePosition_; + int profilePositionRBV_; + int profileFollowingError_; + int profileMotorDirection_; + int profileMotorOffset_; + + #define LAST_MOTOR_PARAM profileMotorOffset_ + + int numAxes_; /**< Number of axes this controller supports */ + asynMotorAxis **pAxes_; /**< Array of pointers to axis objects */ + epicsEventId pollEventId_; /**< Event ID to wake up poller */ + double idlePollPeriod_; /**< The time between polls when no axes are moving */ + double movingPollPeriod_; /**< The time between polls when any axis is moving */ + int forcedFastPolls_; /**< The number of forced fast polls when the poller wakes up */ + + int maxProfilePoints_; /**< Maximum number of profile point */ + double *profileTimes_; /**< Array of times per profile point */ + +friend class asynMotorAxis; +}; +#define NUM_MOTOR_DRIVER_PARAMS (&LAST_MOTOR_PARAM - &FIRST_MOTOR_PARAM + 1) + +#endif /* _cplusplus */ +#endif /* asynMotorController_H */