Implemented Homing on the EuroMove driver

iocBoot/iocsinqEPICS/euromovetest.cmd

@@ -4,12 +4,9 @@ require sinq,koennecke
 
 epicsEnvSet("TOP","/afs/psi.ch/project/sinqdev/sinqepicsapp/iocBoot/iocsinqEPICS")
 epicsEnvSet("BASE","/afs/psi.ch/project/sinqdev/sinqepicsapp")
-epicsEnvSet("dbPATH","${EPICS_BASE}/dbd:${ASYN}/dbd:${MOTOR}/dbd")
 
 cd ${TOP}
 
-## Register all support components
-dbLoadDatabase "../../dbd/sinqEPICS.dbd"
 
 
 drvAsynIPPortConfigure("serial1", "amor-ts:3002",0,0,0)

sinqEPICSApp/Db/motorMessage.db

@@ -7,3 +7,12 @@ record(waveform, "$(P)$(M)-MsgTxt") {
     field(NELM, "80")
     field(SCAN, "I/O Intr")
 }
+
+record(ao,"$(P)m$(N)-Resolution"){
+	field(DESC,"m$(N) Resolution")
+        field(DOL,"$(P)m$(N).MRES CP MS")
+	field(OMSL,"closed_loop")
+	field(DTYP,"asynFloat64")
+	field(OUT,"@asyn($(PORT),$(N))MOTOR_RESOLUTION")
+	field(PREC,"3")
+}

sinqEPICSApp/src/EuroMoveDriver.cpp

@@ -14,6 +14,7 @@ GPIB or USB. The controller is fairly simple. It can control quite a number of m
 #include <math.h>
 #include <ctype.h>
 #include <time.h>
+#include <unistd.h>
 
 #include <iocsh.h>
 #include <epicsThread.h>
@@ -192,6 +193,44 @@ asynStatus EuroMoveController::transactController(int axisNo,char command[COMLEN
   return status;
 }
 
+
+asynStatus EuroMoveController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
+{
+  int function = pasynUser->reason;
+  asynStatus status = asynError;
+  EuroMoveAxis *pAxis = NULL;
+	
+  static const char *functionName = "EuroMoveController::writeFloat64";
+
+
+  pAxis = (EuroMoveAxis *)this->getAxis(pasynUser);
+  if (!pAxis) {
+    return asynError;
+  }
+
+
+  /* Set the parameter and readback in the parameter library. */
+  status = pAxis->setDoubleParam(function, value);
+
+  errlogPrintf("%s, reason %d, value %f\n",  functionName, function, value);
+
+  // TODO: somethign is really shitty here: lowLimit and highLimit cannot be on the command
+  if (function == motorResolution_) {
+    if(value > .0){
+      pAxis->resolution = (int)(1./value);
+    } else {
+      pAxis->resolution = 1;
+    }
+  } 
+
+  //Call base class method
+  //This will handle callCallbacks even if the function was handled here.
+  status = asynMotorController::writeFloat64(pasynUser, value);
+
+  return status;
+
+}
+
 // These are the EuroMoveAxis methods
 
 /** Creates a new EuroMoveAxis object.
@@ -236,7 +275,7 @@ asynStatus EuroMoveAxis::move(double position, int relative, double minVelocity,
   if (relative) {
     position += this->position;
   }
-  homing = 0;
+  homingStatus = HomeIdle;
   sprintf(command,"G%d=%d", motNo, (int)position);
   status = pC_->transactController(motNo,command,reply);
   next_poll = -1;
@@ -247,10 +286,29 @@ asynStatus EuroMoveAxis::home(double minVelocity, double maxVelocity, double acc
 {
   asynStatus status = asynSuccess;
   //static const char *functionName = "EuroMoveAxis::home";
+  char command[COMLEN], reply[COMLEN];
 
   /*
-    I do not know if this controller actually can do homing
+    The EuroMove controller does not have a built in homing mechanism. We simulate it by:
+
+    1). Running the motor in the appropriate end swicth at high speed
+    2) Stepping back three units
+    3) Run the motor again in the end switch at low speed for better accuracy
+    4) Set the limit position with the <I> command
+
+    The state of this operation is maintained in the homingStatus variable
   */
+  homingDirection = forwards;
+  homingStatus = HomeFastRun;
+  if(homingDirection){
+    sprintf(command,"H%d=03", motNo);
+  } else {
+    sprintf(command,"H%d=07", motNo);
+  }
+
+  status = pC_->transactController(motNo,command,reply);
+  next_poll = -1;
+
   return status;
 }
 
@@ -339,6 +397,7 @@ asynStatus EuroMoveAxis::poll(bool *moving)
   asynStatus comStatus = asynSuccess;
   char command[COMLEN], reply[COMLEN];
   unsigned int axStatus;
+  double backStep, backPos, limit;
 
   // protect against excessive polling
   if(time(NULL) < next_poll){
@@ -371,6 +430,7 @@ asynStatus EuroMoveAxis::poll(bool *moving)
   /* errlogPrintf("Axis %d, status reply %s, position %d\n", axisNo_, reply, position); */
   sscanf(reply, "%x", &axStatus);
 
+  if(homingStatus == HomeIdle){
     if((axStatus & 128) > 0) { // test bit 8
       *moving = true;
       next_poll = -1;
@@ -410,7 +470,94 @@ asynStatus EuroMoveAxis::poll(bool *moving)
       errlogPrintf("Encoding anomaly on %d\n", motNo);
       comStatus = asynError;
     } 
-
+  } else {
+    next_poll = -1;
+    setIntegerParam(pC_->motorStatusDone_, false);
+    *moving = true;
+    switch(homingStatus){
+    case HomeIdle:
+      // handled above: this is here to silence the compiler
+      break;
+    case HomeFastRun:
+      //      errlogPrintf("HomeFastRun\n");
+      if(axStatus & 2 || axStatus & 1){
+	sendStop();
+	homingStatus = HomeFastStop;
+      }
+      break;
+    case HomeFastStop:
+      // errlogPrintf("HomeFastStop\n");
+      if((axStatus & 128) == 0){
+	backStep = 3 * resolution;
+	if(homingDirection){
+	  backPos = position - backStep;
+	} else {
+	  backPos = position + backStep;
+	}
+	sprintf(command,"G%d=%d", motNo, (int)backPos);
+	comStatus = pC_->transactController(motNo,command,reply);
+	if(comStatus == asynError){
+	  setIntegerParam(pC_->motorStatusProblem_, true);
+	  updateMsgTxtFromDriver("Homing problem when back stepping");
+	  errlogPrintf("Homing problem when back stepping: %s not accepted\n", command);
+	  *moving = false;
+	  setIntegerParam(pC_->motorStatusAtHome_, true);
+	  homingStatus = HomeIdle;
+	  setIntegerParam(pC_->motorStatusDone_, true);
+	  goto skip;
+	}
+	usleep(200);
+	homingStatus = HomeBackStep;
+      }
+      break;
+    case HomeBackStep:
+      //errlogPrintf("HomeBackStep\n");
+      if((axStatus & 128) == 0){
+	homingStatus = HomeSlowRun;
+	if(homingDirection){
+	  sprintf(command,"H%d=01", motNo);
+	} else {
+	  sprintf(command,"H%d=05", motNo);
+	}
+	comStatus = pC_->transactController(motNo,command,reply);
+	if(comStatus == asynError){
+	  setIntegerParam(pC_->motorStatusProblem_, true);
+	  updateMsgTxtFromDriver("Homing problem slow running");
+	  errlogPrintf("Homing problem when slow running: %s not accepted\n", command);
+	  *moving = false;
+	  setIntegerParam(pC_->motorStatusAtHome_, true);
+	  homingStatus = HomeIdle;
+	  setIntegerParam(pC_->motorStatusDone_, true);
+	  goto skip;
+	}
+      }
+      break;
+    case HomeSlowRun:
+      //errlogPrintf("HomeSlowRun\n");
+      if(axStatus & 2 || axStatus & 1){
+	sendStop();
+	homingStatus = HomeSlowStop;
+      }
+      break;
+    case HomeSlowStop:
+      //errlogPrintf("HomeSlowStop\n");
+      if((axStatus & 128) == 0) {
+	if(homingDirection) {
+	  pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&limit);
+	} else {
+	  pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&limit);
+	} 
+	setPosition(limit);
+	*moving = false;
+	setIntegerParam(pC_->motorStatusDone_, true);
+	next_poll = time(NULL)+IDLEPOLL;
+	setIntegerParam(pC_->motorStatusDone_, true);
+	setIntegerParam(pC_->motorStatusAtHome_, true);
+	homingStatus = HomeIdle;
+	setDoubleParam(pC_->motorPosition_,(double)limit);
+      }
+    }
+  }
 
   skip:
   callParamCallbacks();

sinqEPICSApp/src/EuroMoveDriver.h

@@ -12,6 +12,15 @@ January 2020
 
 #define COMLEN 80
 
+typedef enum {
+  HomeIdle,
+  HomeFastRun,
+  HomeFastStop,
+  HomeBackStep,
+  HomeSlowRun,
+  HomeSlowStop,
+} HomingStatus;
+
 class EuroMoveAxis : public SINQAxis
 {
 public:
@@ -35,8 +44,10 @@ private:
   time_t next_poll;
   int motNo; // motor number in the EuroMove controller
   int position;
-  int homing;
+  HomingStatus homingStatus;
+  int homingDirection; // 1 = forward, 0 backwards 
   asynStatus sendStop();
+  int resolution;
   friend class EuroMoveController;
 };
 
@@ -47,6 +58,7 @@ public:
   void report(FILE *fp, int level);
   EuroMoveAxis* getAxis(asynUser *pasynUser);
   EuroMoveAxis* getAxis(int axisNo);
+  asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value);
 
 friend class EuroMoveAxis;
  private: