Improved logging for motion characterisation of motors.

Reject NaN and Infinity in run command.
Change error message for both limit switches active.
Check status of BG command in case it failed.

r2482 | dcl | 2008-04-24 12:08:03 +1000 (Thu, 24 Apr 2008) | 5 lines

site_ansto/motor_dmc2280.c

@@ -157,6 +157,11 @@ struct __MoDriv {
   double lastPosition; /**< Position at last position check */
   int lastSteps;
   int lastCounts;
+  double origPosition; /**< Position at last position check */
+  int origSteps;
+  int origCounts;
+  double minRatio;
+  double maxRatio;
   int thread0; /**< last read of _XQ0 */
   unsigned short int stopCode; /**< last read of _SCx */
   unsigned short int inputByte; /**< last read of _TIx */
@@ -208,7 +213,7 @@ struct __MoDriv {
 int DMC2280MotionControl = 1; /* defaults to enabled */
 
 #define AIR_POLL_TIMER  1000
-#define MOTOR_POLL_FAST 200
+#define MOTOR_POLL_FAST 100
 #define MOTOR_POLL_SLOW 5000
 #define ON_SETTLE_TIMER 200
 #define MAX_CREEP_STEPS 100
@@ -218,7 +223,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */
 #define BADADR     -1 // NOT SET: Unknown host/port?
 #define BADBSY     -2
 #define BADCMD     -3
-#define BADPAR     -4 // NOT SET: Does SICS already check parameter types?
+#define BADPAR     -4 // Does SICS already check parameter types?
 #define BADUNKNOWN -5
 #define BADSTP     -6 // NOT SET
 #define BADEMERG   -7 // NOT SET: ESTOP
@@ -1100,15 +1105,10 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
  * - 0 MOTOR BAD, position is not within hard and soft limits
  */
 static int checkPosition(pDMC2280Driv self) {
-  int iRet = 1;
   float fTarget;
   float fPrecision;
   bool moving_forward = false;
-  float fLower = self->fLower;
-  float fUpper = self->fUpper;
 
-  MotorGetPar(self->pMot, "softlowerlim", &fLower);
-  MotorGetPar(self->pMot, "softupperlim", &fUpper);
   MotorGetPar(self->pMot, "precision", &fPrecision);
 
   if (self->preseek)
@@ -1121,18 +1121,18 @@ static int checkPosition(pDMC2280Driv self) {
     moving_forward = false;
 
   if (moving_forward) {
-    if ((self->currPosition > self->fUpper + fPrecision)
+    if (self->currPosition > self->fUpper + fPrecision)
-        || (self->currPosition > fUpper + fPrecision)
+      return 0;
-        || (self->currPosition > fTarget + fPrecision))
+    if (self->currPosition > fTarget + fPrecision)
-      iRet = 0;
+      return 0;
   }
   else {
-    if ((self->currPosition < self->fLower - fPrecision)
+    if (self->currPosition < self->fLower - fPrecision)
-        || (self->currPosition < fLower - fPrecision)
+      return 0;
-        || (self->currPosition < fTarget - fPrecision))
+    if (self->currPosition < fTarget - fPrecision)
-      iRet = 0;
+      return 0;
   }
-  return iRet;
+  return 1;
 }
 
 /** \brief Check if the axis has moved significantly since
@@ -1151,6 +1151,7 @@ static int checkMotion(pDMC2280Driv self) {
   double ratio_obs, ratio_exp, ratio_cmp;
   long int usec_TimeDiff;
   struct timeval now;
+  bool bNotYet = false; /* set true if too soon */
 
   assert(self != NULL);
   /* we can only test if there is an absolute encoder */
@@ -1159,21 +1160,40 @@ static int checkMotion(pDMC2280Driv self) {
   if (self->time_lastPos_set.tv_sec == 0) {
     /* first time - initialise the data */
     set_lastMotion(self, self->currSteps, self->currCounts);
-    return 1;
+    bNotYet = true;
   }
+  /*
+   * calculate the time since the last check
+   */
   gettimeofday(&now, NULL);
   usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec;
   usec_TimeDiff *= 1000000;
   usec_TimeDiff += now.tv_usec;
   usec_TimeDiff -= self->time_lastPos_set.tv_usec;
+  /*
+   * If too soon, say not yet
+   */
   if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval))
-    return 1;
+    bNotYet = true;
-  /* If not stepping, then not blocked */
+
+  /*
+   * calculate the distance since the last check
+   * If too short, say not yet
+   */
+#if 0
   if (fabs(self->currSteps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) {
     /* just update the timestamp */
     set_lastMotion(self, self->lastSteps, self->lastCounts);
-    return 1;
+    bNotYet = true;
   }
+#else
+  /* TODO: make relative to lastPosition */
+  if (fabs(self->currSteps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) {
+    /* just update the timestamp */
+    set_lastMotion(self, self->lastSteps, self->lastCounts);
+    bNotYet = true;
+  }
+#endif
   /* calculate observed and expected steps per count ratios */
   if (self->currCounts == self->lastCounts) /* prevent divide by zero */
     ratio_obs = (self->currSteps - self->lastSteps);
@@ -1183,14 +1203,19 @@ static int checkMotion(pDMC2280Driv self) {
   }
   ratio_exp = (double) self->stepsPerX / (double) self->cntsPerX;
   ratio_cmp = ratio_obs / ratio_exp;
+  if (self->minRatio == 0.0 || self->minRatio > ratio_cmp)
+    self->minRatio = ratio_cmp;
+  if (self->maxRatio == 0.0 || self->maxRatio < ratio_cmp)
+    self->maxRatio = ratio_cmp;
   /* wrong signs, less than half, or more than double is trouble */
   if (ratio_cmp < 0.0 ||
       ratio_cmp > self->blockage_ratio ||
       (1.0 / ratio_cmp) > self->blockage_ratio) {
     char msg[132];
     double cmp = ratio_cmp;
-    if (fabs(cmp) > 0 && fabs(cmp) < 1)
+    if (fabs(cmp) > -1.0 && fabs(cmp) < 1.0)
       cmp = 1.0 / cmp;
+    if (self->debug || bNotYet == false) {
       snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f",
           self->name, ratio_obs, ratio_exp, cmp);
       SICSLogWrite(msg, eError);
@@ -1199,10 +1224,13 @@ static int checkMotion(pDMC2280Driv self) {
           self->currCounts, self->lastCounts,
           self->stepsPerX, self->cntsPerX);
       SICSLogWrite(msg, eError);
+    }
+    if (bNotYet == false) {
       if (self->blockage_fail)
         iRet = 0;
       else
         set_lastMotion(self, self->currSteps, self->currCounts);
+    }
   } else {
     if (self->debug) {
       char msg[132];
@@ -1212,9 +1240,16 @@ static int checkMotion(pDMC2280Driv self) {
       snprintf(msg, sizeof(msg), "Motor %s pass: obs=%f, exp=%f, cmp=%f",
           self->name, ratio_obs, ratio_exp, cmp);
       SICSLogWrite(msg, eError);
+      snprintf(msg, sizeof(msg), "steps=%d-%d, counts=%d-%d, exp=%f/%f",
+          self->currSteps, self->lastSteps,
+          self->currCounts, self->lastCounts,
+          self->stepsPerX, self->cntsPerX);
+      SICSLogWrite(msg, eError);
     }
+    if (bNotYet == false) {
       set_lastMotion(self, self->currSteps, self->currCounts);
     }
+  }
   return iRet;
 }
 
@@ -2088,6 +2123,12 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
           absolute = motCreep(self, target);
           cmdPosition(self, absolute);
           self->subState = 2;
+          /* save pre-motion values for logging */
+          self->origPosition = self->currPosition;
+          self->origCounts = self->currCounts;
+          self->origSteps = self->currSteps;
+          self->minRatio = 0.0;
+          self->maxRatio = 0.0;
           return;
         }
         else if (self->subState == 2) { /* PA */
@@ -2152,7 +2193,14 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
       do {
         pAsyncTxn pCmd = event->event.msg.cmd;
         if (self->subState == 1) { /* BG */
-          /* TODO: Check if BG worked (reply != '?') */
+          /* Check if BG worked (reply != '?') */
+          if (pCmd->inp_buf[0] == '?') {
+            /* TODO: what happens when BGx fails? */
+            self->errorCode = BADCMD;
+            self->driver_status = HWFault;
+            change_state(self, DMCState_MotorHalt);
+            return;
+          }
           self->subState = 2;
           DMC_SetTimer(self, MOTOR_POLL_FAST);
           return;
@@ -2385,6 +2433,25 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
             DMC_SetTimer(self, self->settle);
             return;
           }
+          /* We get here when the motor stops normally
+           */
+          if (true /*self->debug*/) {
+            double units = self->currPosition - self->origPosition;
+            long int steps = self->currSteps - self->origSteps;
+            long int counts = self->currCounts - self->origCounts;
+            char line[CMDLEN];
+            snprintf(line, CMDLEN, "Motor=%s stopped: units=%.6f,"
+                " steps=%ld, counts=%ld, stepsPerX=%.6f,"
+                " minRatio=%.6f, maxRatio=%.6f",
+                self->name,
+                units,
+                steps,
+                counts,
+                (double) steps / units,
+                self->minRatio,
+                self->maxRatio);
+            SICSLogWrite(line, eStatus);
+          }
           change_state(self, DMCState_OffTimer);
           return;
         }
@@ -2703,12 +2770,28 @@ static int DMC2280Run(void *pData,float fValue){
     self->time_lastPos_set.tv_sec = 0;
   } while (0);
 
-  self->fTarget = fValue;
+  /* Reject Infinity and Not a Number (NaN) */
+  switch (fpclassify(fValue)) {
+    case FP_ZERO:
+    case FP_NORMAL:
+      break;
+    default:
+      self->errorCode = BADPAR;
+      return HWFault;
+  }
 
+  /* Only do it if it is within our hard limits */
+  if (fValue >= self->fLower && fValue <= self->fUpper) {
+    self->fTarget = fValue;
     state_cmd_execute(self, CMD_RUN);
     return 1;
   }
 
+  /* If we didn't do it then it must have been bad */
+  self->errorCode = BADPAR;
+  return HWFault;
+}
+
 /** \brief Returns the motor status while it's moving,
  * implements the GetStatus method in the MotorDriver interface.
  *
@@ -2796,7 +2879,7 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
       strncpy(error, "Axis error exceeds error limit", (size_t)errLen);
       break;
     case IMPOSSIBLE_LIM_SW:
-      strncpy(error, "Both limit switches seem active, maybe the polarity is set 'active low'. You should configure the controller with CN 1,-1,-1,0", (size_t)errLen);
+      strncpy(error, "Both limit switches seem active, are they plugged in?", (size_t)errLen);
       break;
     case BLOCKED:
       strncpy(error, "STOPPING MOTOR, MOTION SEEMS TO BE BLOCKED", (size_t)errLen);
@@ -4048,6 +4131,38 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
       }
       return 1;
     }
+    else if (self->debug && strcasecmp("hardlowerlim", argv[1]) == 0) {
+      /* Managers only */
+      if (!SCMatchRights(pCon, usMugger))
+        return 0;
+      float fValue = strtod(argv[2], NULL);
+      self->fLower = fValue;
+      return 1;
+    }
+    else if (self->debug && strcasecmp("hardupperlim", argv[1]) == 0) {
+      /* Managers only */
+      if (!SCMatchRights(pCon, usMugger))
+        return 0;
+      float fValue = strtod(argv[2], NULL);
+      self->fUpper = fValue;
+      return 1;
+    }
+    else if (self->debug && strcasecmp("stepsPerX", argv[1]) == 0) {
+      /* Managers only */
+      if (!SCMatchRights(pCon, usMugger))
+        return 0;
+      float fValue = strtod(argv[2], NULL);
+      self->stepsPerX = fValue;
+      return 1;
+    }
+    else if (self->debug && strcasecmp("cntsPerX", argv[1]) == 0) {
+      /* Managers only */
+      if (!SCMatchRights(pCon, usMugger))
+        return 0;
+      float fValue = strtod(argv[2], NULL);
+      self->cntsPerX = fValue;
+      return 1;
+    }
     if (self->posit_count > 0) {
       if(strcasecmp("posit2count", argv[1]) == 0) {
         char line[132];