motor: implemented framework for moveToHome functionality in model 3 asyn base classes.

motorApp/MotorSrc/asynMotorAxis.cpp

@@ -8,6 +8,10 @@
 #include <stdlib.h>
 #include <string.h>
 
+#include <iostream>
+using std::endl;
+using std::cout;
+
 #include <epicsThread.h>
 
 #include <asynPortDriver.h>
@@ -49,6 +53,11 @@ asynMotorAxis::asynMotorAxis(class asynMotorController *pC, int axisNo)
   profileReadbacks_       = NULL;
   profileFollowingErrors_ = NULL;
   
+  /* Used to keep track of referencing mode in the driver.*/
+  referencingMode_ = 0;
+  /* Used to enable/disable move to home, and to tell driver how far to move.*/
+  referencingModeMove_ = 0;
+
   // Create the asynUser, connect to this axis
   pasynUser_ = pasynManager->createAsynUser(NULL, NULL);
   pasynManager->connectDevice(pasynUser_, pC->portName, axisNo);
@@ -113,6 +122,33 @@ asynStatus asynMotorAxis::poll(bool *moving)
   return asynError;
 }
 
+/**
+ * Default implementation of doMoveToHome. 
+ * Derived classes need to implement this to actually perform the 
+ * axis move to the home position.
+ */
+asynStatus asynMotorAxis::doMoveToHome()
+{
+  cout << "Dummy implementation of asynMotorAxis::doMoveToHome. Axis: " << pC_->moveToHomeAxis_ << endl;
+  return asynError;
+}
+
+/**
+ * Set method for referencingModeMove_
+ */
+void  asynMotorAxis::setReferencingModeMove(int distance)
+{
+  referencingModeMove_ = distance; 
+}
+
+/**
+ * Get method for referencingModeMove_
+ */
+int  asynMotorAxis::getReferencingModeMove() 
+{
+  return referencingModeMove_;
+}
+
 
 
 /** Set the current position of the motor.

motorApp/MotorSrc/asynMotorAxis.h

@@ -32,6 +32,7 @@ class epicsShareFunc asynMotorAxis {
   virtual asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards);
   virtual asynStatus stop(double acceleration);
   virtual asynStatus poll(bool *moving);
+  virtual asynStatus doMoveToHome();
   virtual asynStatus setPosition(double position);
 
   virtual asynStatus initializeProfile(size_t maxPoints);
@@ -41,6 +42,9 @@ class epicsShareFunc asynMotorAxis {
   virtual asynStatus abortProfile();
   virtual asynStatus readbackProfile();
 
+  void setReferencingModeMove(int distance);
+  int getReferencingModeMove();
+
   protected:
   class asynMotorController *pC_;    /**< Pointer to the asynMotorController to which this axis belongs.
                                       *   Abbreviated because it is used very frequently */
@@ -49,10 +53,12 @@ class epicsShareFunc asynMotorAxis {
   double *profilePositions_;         /**< Array of target positions for profile moves */
   double *profileReadbacks_;         /**< Array of readback positions for profile moves */
   double *profileFollowingErrors_;   /**< Array of following errors for profile moves */   
+  int referencingMode_;
 
   private:
   MotorStatus status_;
   int statusChanged_;
+  int referencingModeMove_;
   
   friend class asynMotorController;
 };

motorApp/MotorSrc/asynMotorController.cpp

@@ -8,7 +8,13 @@
 #include <stdlib.h>
 #include <string.h>
 
+#include <iostream>
+using std::endl;
+using std::cout;
+
 #include <epicsThread.h>
+#include <epicsExport.h>
+#include <iocsh.h>
 
 #include <asynPortDriver.h>
 #define epicsExportSharedSymbols
@@ -18,6 +24,8 @@
 
 static const char *driverName = "asynMotorController";
 static void asynMotorPollerC(void *drvPvt);
+static void asynMotorMoveToHomeC(void *drvPvt);
+
 
 /** Creates a new asynMotorController object.
   * All of the arguments are simply passed to the constructor for the asynPortDriver base class. 
@@ -49,6 +57,7 @@ asynMotorController::asynMotorController(const char *portName, int numAxes, int
   createParam(motorPositionString,               asynParamFloat64,    &motorPosition_);
   createParam(motorEncoderPositionString,        asynParamFloat64,    &motorEncoderPosition_);
   createParam(motorDeferMovesString,             asynParamInt32,      &motorDeferMoves_);
+  createParam(motorMoveToHomeString,             asynParamInt32,      &motorMoveToHome_);
   createParam(motorResolutionString,             asynParamFloat64,    &motorResolution_);
   createParam(motorEncRatioString,               asynParamFloat64,    &motorEncRatio_);
   createParam(motorPgainString,                  asynParamFloat64,    &motorPgain_);
@@ -113,11 +122,14 @@ asynMotorController::asynMotorController(const char *portName, int numAxes, int
 
   pAxes_ = (asynMotorAxis**) calloc(numAxes, sizeof(asynMotorAxis*));
   pollEventId_ = epicsEventMustCreate(epicsEventEmpty);
+  moveToHomeId_ = epicsEventMustCreate(epicsEventEmpty);
 
   maxProfilePoints_ = 0;
   profileTimes_ = NULL;
   setIntegerParam(profileExecuteState_, PROFILE_EXECUTE_DONE);
 
+  moveToHomeAxis_ = 0;
+
   asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
     "%s:%s: constructor complete\n",
     driverName, functionName);
@@ -168,6 +180,13 @@ asynStatus asynMotorController::writeInt32(asynUser *pasynUser, epicsInt32 value
     status = abortProfile();
   } else if (function == profileReadback_) {
     status = readbackProfile();
+  } else if (function == motorMoveToHome_) {
+    if (value == 1) {
+      asynPrint(pasynUser, ASYN_TRACE_FLOW, 
+		"%s:%s:: Starting a move to home for axis %d\n",  driverName, functionName, axis);
+      moveToHomeAxis_ = axis;
+      epicsEventSignal(moveToHomeId_);
+    }
   }
 
   /* Do callbacks so higher layers see any changes */
@@ -418,6 +437,7 @@ asynStatus asynMotorController::startPoller(double movingPollPeriod, double idle
   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. */
@@ -502,6 +522,55 @@ void asynMotorController::asynMotorPoller()
   }
 }
 
+/**
+ * Start the thread which deals with moving axes to their home position.
+ * This is called by the derived concrete controller class at object instatiation, so
+ * that drivers that don't need this functionality don't have the overhead of the thread.
+ */
+asynStatus asynMotorController::startMoveToHomeThread()
+{
+  epicsThreadCreate("motorMoveToHome", 
+                    epicsThreadPriorityMedium,
+                    epicsThreadGetStackSize(epicsThreadStackMedium),
+                    (EPICSTHREADFUNC)asynMotorMoveToHomeC, (void *)this);
+  return asynSuccess;
+}
+
+static void asynMotorMoveToHomeC(void *drvPvt)
+{
+  asynMotorController *pController = (asynMotorController*)drvPvt;
+  pController->asynMotorMoveToHome();
+}
+
+
+
+/**
+ * Default move to home thread. Not normally overridden.
+ */
+void asynMotorController::asynMotorMoveToHome()
+{
+  
+  asynMotorAxis *pAxis;
+  int status = 0;
+  static const char *functionName = "asynMotorMoveToHome";
+
+  while(1) {
+    status = epicsEventWait(moveToHomeId_);
+    if (status == epicsEventWaitOK) { 
+      pAxis = getAxis(this->moveToHomeAxis_);
+      if (!pAxis) continue;
+      status = pAxis->doMoveToHome();
+      if (status) {
+	asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
+      "%s:%s: move to home failed in asynMotorController::asynMotorMoveToHome. Axis number=%d\n", 
+      driverName, functionName, this->moveToHomeAxis_);
+      }
+    } 
+  } 
+}
+
+
+
 /* These are the functions for profile moves */
 /** Initialize a profile move of multiple axes. */
 asynStatus asynMotorController::initializeProfile(size_t maxProfilePoints)
@@ -589,3 +658,61 @@ asynStatus asynMotorController::readbackProfile()
   }
   return asynSuccess;
 }
+
+
+
+/** The following functions have C linkage, and can be called directly or from iocsh */
+
+extern "C" {
+
+
+asynStatus asynMotorEnableMoveToHome(const char *portName, int axis, int distance)
+{
+  asynMotorController *pC = NULL;
+  asynMotorAxis *pA = NULL;
+  static const char *functionName = "asynMotorEnableMoveToHome";
+
+  pC = (asynMotorController*) findAsynPortDriver(portName);
+  if (!pC) {
+    cout << driverName << "::" << functionName << " Error port " << portName << " not found." << endl;
+    return asynError;
+  }
+  
+  pA = pC->getAxis(axis);
+  if (!pA) {
+    cout << driverName << "::" << functionName << " Error axis " << axis << " not found." << endl;
+    return asynError;
+  }
+
+  if (distance<=0) {
+    cout << "Error in asynMotorEnableMoveToHome. distance must be positive integer." << endl;
+  } else {
+    pA->setReferencingModeMove(distance);
+  }
+
+  return asynSuccess;
+}
+
+
+/* asynMotorEnableMoveToHome */
+static const iocshArg asynMotorEnableMoveToHomeArg0 = {"Controller port name", iocshArgString};
+static const iocshArg asynMotorEnableMoveToHomeArg1 = {"Axis number", iocshArgInt};
+static const iocshArg asynMotorEnableMoveToHomeArg2 = {"Distance", iocshArgInt};
+static const iocshArg * const asynMotorEnableMoveToHomeArgs[] = {&asynMotorEnableMoveToHomeArg0,
+                                                                 &asynMotorEnableMoveToHomeArg1,
+                                                                 &asynMotorEnableMoveToHomeArg2};
+static const iocshFuncDef enableMoveToHome = {"asynMotorEnableMoveToHome", 3, asynMotorEnableMoveToHomeArgs};
+
+static void enableMoveToHomeCallFunc(const iocshArgBuf *args)
+{
+  asynMotorEnableMoveToHome(args[0].sval, args[1].ival, args[2].ival);
+}
+
+
+static void asynMotorControllerRegister(void)
+{
+  iocshRegister(&enableMoveToHome, enableMoveToHomeCallFunc);
+}
+epicsExportRegistrar(asynMotorControllerRegister);
+
+} //extern C

motorApp/MotorSrc/asynMotorController.h

@@ -158,6 +158,10 @@ class epicsShareFunc asynMotorController : public asynPortDriver {
   virtual asynStatus poll();
   void asynMotorPoller();  // This should be private but is called from C function
   
+  /* Functions to deal with moveToHome.*/
+  virtual asynStatus startMoveToHomeThread();
+  void asynMotorMoveToHome();
+
   /* These are the functions for profile moves */
   virtual asynStatus initializeProfile(size_t maxPoints);
   virtual asynStatus buildProfile();
@@ -183,6 +187,7 @@ class epicsShareFunc asynMotorController : public asynPortDriver {
   int motorPosition_;
   int motorEncoderPosition_;
   int motorDeferMoves_;
+  int motorMoveToHome_;
   int motorResolution_;
   int motorEncRatio_;
   int motorPgain_;
@@ -251,6 +256,7 @@ class epicsShareFunc asynMotorController : public asynPortDriver {
   int numAxes_;                 /**< Number of axes this controller supports */
   asynMotorAxis **pAxes_;       /**< Array of pointers to axis objects */
   epicsEventId pollEventId_;    /**< Event ID to wake up poller */
+  epicsEventId moveToHomeId_;    /**< Event ID to wake up move to home thread */
   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 */
@@ -258,6 +264,8 @@ class epicsShareFunc asynMotorController : public asynPortDriver {
   size_t maxProfilePoints_;     /**< Maximum number of profile points */
   double *profileTimes_;        /**< Array of times per profile point */
 
+  int moveToHomeAxis_;
+
   friend class asynMotorAxis;
 };
 #define NUM_MOTOR_DRIVER_PARAMS (&LAST_MOTOR_PARAM - &FIRST_MOTOR_PARAM + 1)

motorApp/MotorSrc/motorSupport.dbd

@@ -4,5 +4,6 @@ registrar(motorUtilRegister)
 #variable(motordrvComdebug)
 #variable(motorUtil_debug)
 registrar(motorRegister)
+registrar(asynMotorControllerRegister)
 device(motor,INST_IO,devMotorAsyn,"asynMotor")