Allow thread status to be queried and homerun for motors without absolute ebcoder

r1619 | dcl | 2007-03-09 12:34:37 +1100 (Fri, 09 Mar 2007) | 2 lines

site_ansto/motor_dmc2280.c

@@ -473,6 +473,76 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) {
   return SUCCESS;
 }
 
+/** \brief Reads Thread variable
+ *
+ * \param *pos is the thread variable value on SUCCESS.
+ * \return
+ * SUCCESS
+ * FAILURE
+ */
+static int ReadThread(pDMC2280Driv self, int thread, float *pos) {
+  char reply[1024];
+  char cmd[CMDLEN];
+
+  snprintf(cmd, CMDLEN, "MG _XQ%d", thread);
+  if (FAILURE == DMC2280Send(self, cmd)) 
+    return 0; 
+  if (FAILURE == DMC2280Receive(self, reply)) 
+    return 0;
+
+  *pos = (float) atoi(reply);
+  return 1;
+}
+
+/** \brief Reads HOMERUN variable
+ *
+ * \param *pos is the homerun variable value on SUCCESS.
+ * \return
+ * SUCCESS
+ * FAILURE
+ */
+static int readHomeRun(pDMC2280Driv self, float *pos) {
+  char reply[1024];
+  char cmd[CMDLEN];
+
+  snprintf(cmd, CMDLEN, "MG HOMERUN");
+  if (FAILURE == DMC2280Send(self, cmd)) 
+    return FAILURE; 
+  if (FAILURE == DMC2280Receive(self, reply)) 
+    return FAILURE;
+
+  *pos = (float) atoi(reply);
+  return SUCCESS;
+}
+
+/** \brief Runs the HOMERUN routine on the MC
+ *
+ * \return
+ * SUCCESS
+ * FAILURE
+ */
+static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
+  float fValue;
+  char cmd[CMDLEN];
+
+  /* Read HOMERUN should get error if it does not have one */
+  if (FAILURE == readHomeRun(self, &fValue))
+    return FAILURE;
+
+  /* 0 => reset homerun */
+  if (newValue < 0.5) {
+    snprintf(cmd, CMDLEN, "HOMERUN=0");
+    if (FAILURE == DMC2280Send(self, cmd)) 
+      return FAILURE; 
+    return SUCCESS;
+  }
+  snprintf(cmd, CMDLEN, "XQ #HOME,1");
+  if (FAILURE == DMC2280Send(self, cmd)) 
+    return FAILURE; 
+
+  return SUCCESS;
+}
+
 /** \brief Reads motor position, implements the GetPosition
  * method in the MotorDriver interface.
  *
@@ -1207,6 +1277,30 @@ static int DMC2280GetPar(void *pData, char *name,
         return 0;
     }
   }
+  else {
+    if (strcmp(name,"homerun") == 0) {
+      if (readHomeRun(self, fValue) == SUCCESS)
+        return 1;
+      else
+        return 0;
+    }
+  }
+  if(strcmp(name,"thread0") == 0)
+    return ReadThread(self, 0, fValue);
+  if(strcmp(name,"thread1") == 0)
+    return ReadThread(self, 1, fValue);
+  if(strcmp(name,"thread2") == 0)
+    return ReadThread(self, 2, fValue);
+  if(strcmp(name,"thread3") == 0)
+    return ReadThread(self, 3, fValue);
+  if(strcmp(name,"thread4") == 0)
+    return ReadThread(self, 4, fValue);
+  if(strcmp(name,"thread5") == 0)
+    return ReadThread(self, 5, fValue);
+  if(strcmp(name,"thread6") == 0)
+    return ReadThread(self, 6, fValue);
+  if(strcmp(name,"thread7") == 0)
+    return ReadThread(self, 7, fValue);
   return 0;
 }
 
@@ -1313,7 +1407,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
     }
   }
 
-  /* Set interval between blocked motor checks,
+  /* Set movement threshold for blocked motor checks,
    * managers only */
   if(strcmp(name,"blockage_thresh") == 0) {
     if(!SCMatchRights(pCon,usMugger))
@@ -1324,7 +1418,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
     }
   }
 
-  /* Set interval between blocked motor checks,
+  /* Set ratio for blocked motor checks,
    * managers only */
   if(strcmp(name,"blockage_ratio") == 0) {
     if(!SCMatchRights(pCon,usMugger))
@@ -1335,7 +1429,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
     }
   }
 
-  /* Set interval between blocked motor checks,
+  /* Set blocked motor checks failure mode,
    * managers only */
   if(strcmp(name,"blockage_fail") == 0) {
     if(!SCMatchRights(pCon,usMugger))
@@ -1346,6 +1440,21 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
     }
   }
 
+  /* Invoke Home Run routine in controller,
+   * managers only */
+  if(self->abs_endcoder == 0 && strcmp(name,"homerun") == 0) {
+    if(!SCMatchRights(pCon,usMugger))
+      return 1;
+    else {
+      if (DMC2280MotionControl != 1 && newValue > 0.5) {
+        snprintf(pError, ERRLEN,"ERROR: Motion Control must be on");
+        SCWrite(pCon, pError, eError);
+      }
+      RunHomeRoutine(self, newValue);
+      return 1;
+    }
+  }
+
   /* Set speed */
   if(strcmp(name,SPEED) == 0) {
     if ((0.0 - newValue) > FLT_EPSILON) {