Begin migration of motor positions from encoder counts to motor units (mm/degree)

r2185 | dcl | 2007-10-17 08:36:19 +1000 (Wed, 17 Oct 2007) | 2 lines

site_ansto/motor_dmc2280.c

@@ -183,8 +183,10 @@ struct __MoDriv {
   pNWTimer state_timer; /**< motor state timer */
   SConnection *trace;
   int posit_count;
-  bool posit_check;
+#ifdef POSIT_COUNTS
-  unsigned long long *posit_array;
+  long long* posit_array;
+#endif
+  double* positions;
   int variables;
 };
 
@@ -251,6 +253,7 @@ static int DMC2280Halt(void *pData);
 static int DMC2280SetPar(void *pData, SConnection *pCon,
     char *name, float newValue);
 
+#ifdef POSIT_COUNTS
 static bool check_posits(pDMC2280Driv self) {
   char line[CMDLEN];
   int missing = 0;
@@ -308,6 +311,70 @@ static bool check_posits(pDMC2280Driv self) {
         i + 1,
         self->name,
         self->posit_array[i]);
+    /* TODO log me */
+  }
+  return false;
+}
+#endif
+
+static bool check_positions(pDMC2280Driv self) {
+  char line[CMDLEN];
+  int missing = 0;
+  int direction = 0;
+  int i;
+
+  if (self->posit_count < 2) {
+    snprintf(line, ERRLEN, "Insufficient positions on motor '%s': %d",
+        self->name, self->posit_count);
+    SICSLogWrite(line,eError);
+    return false;
+  }
+  for (i = 0; i < self->posit_count; ++i)
+    if (self->positions[i] < -9999999)
+      ++missing;
+  if (missing) {
+    snprintf(line, ERRLEN, "Missing positions on motor '%s': %d",
+        self->name, missing);
+    SICSLogWrite(line,eError);
+    return false;
+  }
+  for (i = 1; i < self->posit_count; ++i) {
+    if (i == 1) {
+      if (self->positions[i] > self->positions[i - 1])
+        direction = 1;
+      else if (self->positions[i] < self->positions[i - 1])
+        direction = -1;
+      else
+        direction = 0;
+    }
+    if (direction == 0) {
+      snprintf(line, ERRLEN, "Position order on motor '%s' : %d",
+          self->name, i);
+      SICSLogWrite(line,eError);
+    }
+    else {
+      switch (direction) {
+        case -1:
+          if (!(self->positions[i] < self->positions[i - 1])) {
+            direction = 0;
+          }
+          break;
+        case 1:
+          if (!(self->positions[i] > self->positions[i - 1])) {
+            direction = 0;
+          }
+          break;
+      }
+    }
+  }
+  if (direction != 0)
+    return true;
+  for (i = 0; i < self->posit_count; ++i) {
+    snprintf(line, ERRLEN, "Position %2d motor '%s' : %f",
+        i + 1,
+        self->name,
+        self->positions[i]);
+    /* TODO log me */
   }
   return false;
 }
@@ -355,6 +422,7 @@ static float count2unit(pDMC2280Driv self, long long counts) {
   return fPos;
 }
 
+#ifdef POSIT_COUNTS
 static long long posit2count(pDMC2280Driv self, float target) {
   double absolute;
   long long result;
@@ -365,6 +433,9 @@ static long long posit2count(pDMC2280Driv self, float target) {
     SICSLogWrite(line, eStatus);
     return -1;
   }
+  if (!check_posits(self))
+    return -1;
+
   int i = ((int) target) - 1;
   if (i < 0) {
     i = 0;
@@ -431,6 +502,60 @@ static float posit2unit(pDMC2280Driv self, float target) {
   return count2unit(self, posit2count(self, target));
 }
 
+#else
+
+static float unit2posit(pDMC2280Driv self, float target) {
+  int i, j;
+  float fPos;
+  if (!check_positions(self))
+    return 0;
+
+  if (self->positions[0] < self->positions[1]) {
+    for (i = 1; i < self->posit_count - 1; ++i) {
+      if (target < self->positions[i]) {
+        break;
+      }
+    }
+  }
+  else {
+    for (i = 1; i < self->posit_count - 1; ++i) {
+      if (target > self->positions[i]) {
+        break;
+      }
+    }
+  }
+  --i;
+  j = i + 1;
+  fPos = (double) j +
+    ((double)target - self->positions[i]) /
+    ((double)self->positions[j] - self->positions[i]);
+  return fPos;
+}
+
+static float posit2unit(pDMC2280Driv self, float target) {
+  double result;
+  int i = ((int) target) - 1;
+  if (i < 0) {
+    i = 0;
+  } else if (i > self->posit_count - 2) {
+    i = self->posit_count - 2;
+  }
+  result = (double)self->positions[i]
+    + ((double)target - ((double)i + 1)) *
+    ((double)self->positions[i + 1] - self->positions[i]);
+  return result;
+}
+
+static long long posit2count(pDMC2280Driv self, float target) {
+  return unit2count(self, posit2unit(self, target));
+}
+
+static float count2posit(pDMC2280Driv self, long long counts) {
+  return unit2posit(self, count2unit(self, counts));
+}
+
+#endif
+
 /** \brief Convert axis speed in physical units to
  * motor speed in steps/sec.
  *
@@ -2485,6 +2610,17 @@ static int DMC2280GetPar(void *pData, char *name,
     *fValue = self->backlash_offset;
     return 1;
   }
+  if (self->posit_count > 0) {
+    if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) {
+      int index;
+      *fValue = -1.0;
+      index = strtol(&name[6], NULL, 10);
+//      if (index < 1 || index > self->posit_count)
+//        return 0;
+      *fValue = posit2count(self, index);
+      return 1;
+    }
+  }
   if (self->abs_encoder != 0) {
     if (strcasecmp(name,"absenc") == 0) {
       if (readAbsEnc(self, fValue) == SUCCESS)
@@ -2516,18 +2652,6 @@ static int DMC2280GetPar(void *pData, char *name,
     }
   }
 
-  if (self->posit_count > 0) {
-      int index;
-      if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) {
-        *fValue = -1.0;
-        index = strtol(&name[6], NULL, 10);
-        if (index < 1 || index > self->posit_count)
-          return 0;
-        *fValue = (float) self->posit_array[index - 1];
-        return 1;
-      }
-  }
-
   if(strcasecmp(name,"thread0") == 0)
     return readThread(self, 0, fValue);
   if(strcasecmp(name,"thread1") == 0)
@@ -2798,7 +2922,10 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
         index = strtol(&name[6], NULL, 10);
         if (index < 1 || index > self->posit_count)
           return 0;
+#ifdef POSIT_COUNTS
         self->posit_array[index - 1] = newValue;
+#endif
+        self->positions[index - 1] = count2unit(self, newValue);
         return 1;
       }
   }
@@ -2886,7 +3013,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
     SCWrite(pCon, buffer, eStatus);
     for (i = 0; i < self->posit_count; ++i) {
       snprintf(buffer, BUFFLEN, "%s.posit_%d = %lld\n", name, i + 1,
-          self->posit_array[i]);
+          posit2count(self, i + 1));
       SCWrite(pCon, buffer, eStatus);
     }
   }
@@ -2945,11 +3072,17 @@ static void KillDMC2280(/*@only@*/void *pData){
     AsyncUnitDestroy(self->asyncUnit);
     self->asyncUnit = NULL;
   }
+#ifdef POSIT_COUNTS
   if (self->posit_array) {
     free(self->posit_array);
     self->posit_array = NULL;
-    self->posit_count = 0;
   }
+#endif
+  if (self->positions) {
+    free(self->positions);
+    self->positions = 0;
+  }
+  self->posit_count = 0;
 /* Not required as performed in caller
  * free(self);
  */
@@ -3238,14 +3371,23 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
       else {
         int i;
         char line[80];
+#ifdef POSIT_COUNTS
         pNew->posit_array = malloc(pNew->posit_count * sizeof(*pNew->posit_array));
+#endif
+        pNew->positions = malloc(pNew->posit_count * sizeof(*pNew->positions));
         for (i = 0; i < pNew->posit_count; ++i) {
           snprintf(line, 80, "posit_%d", i + 1);
           if ((pPtr=getParam(pCon, interp, params,line,_OPTIONAL)) == NULL) {
+#ifdef POSIT_COUNTS
             pNew->posit_array[i] = -1;
+#endif
+            pNew->positions[i] = 0.0;
           }
           else {
+#ifdef POSIT_COUNTS
             pNew->posit_array[i] = strtol(pPtr, NULL, 10);
+#endif
+            pNew->positions[i] = count2unit(pNew, strtol(pPtr, NULL, 10));
           }
         }
       }
@@ -3456,6 +3598,52 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
       SCWrite(pCon, line, eValue);
       return 1;
     }
+    else if(strcasecmp("positions", argv[1]) == 0) {
+      if (argc == 2) {
+        int i, k = 0;
+        char line[1320];
+        k += snprintf(line, sizeof(line) - k, "%s.positions ", self->name);
+        for (i = 0; i < self->posit_count; ++i) {
+          k += snprintf(&line[k], sizeof(line) - k, " %f",
+              self->positions[i]);
+        }
+        SCWrite(pCon, line, eValue);
+        return 1;
+      }
+      if (self->posit_count > 0) {
+        self->posit_count = 0;
+      }
+#ifdef POSIT_COUNTS
+      if (self->posit_array) {
+        free(self->posit_array);
+        self->posit_array = NULL;
+      }
+#endif
+      if (self->positions) {
+        free(self->positions);
+        self->positions = NULL;
+        self->posit_count = 0;
+      }
+      if (argc > 3 && strcasecmp("erase", argv[2]) == 0) {
+        char line[132];
+        snprintf(line, 132, "%s.posit_count = %d", self->name, self->posit_count);
+        SCWrite(pCon, line, eValue);
+        return 1;
+      }
+      self->posit_count = argc - 2;
+      self->positions = malloc(self->posit_count * sizeof(*self->positions));
+#ifdef POSIT_COUNTS
+      self->posit_array = malloc(self->posit_count * sizeof(*self->posit_array));
+#endif
+      int i;
+      for (i = 0; i < self->posit_count; ++i) {
+        self->positions[i] = strtod(argv[i + 2], NULL);
+#ifdef POSIT_COUNTS
+        self->posit_array[i] = unit2count(self, self->positions[i]);
+#endif
+      }
+      return 1;
+    }
     if (self->posit_count > 0) {
       if(strcasecmp("posit2count", argv[1]) == 0) {
         char line[132];