Parameterise the motion checks for detecting blocked motors

r1544 | dcl | 2007-02-22 16:42:12 +1100 (Thu, 22 Feb 2007) | 2 lines

site_ansto/motor_dmc2280.c

@@ -105,6 +105,9 @@ typedef struct __MoDriv {
   float lastCounts;
   struct timeval time_lastPos_set; /**< Time when lastPosition was set */
   float blockage_ckInterval; /**< Interval for checking blocked motors, seconds */
+  float blockage_thresh; /**< motion threshold for blockage checking */
+  float blockage_ratio; /**< ratio steps/counts must be between 1/this and this */
+  int blockage_fail; /**< flag =1 if we should fail the motor */
   int has_airpads; /**< Flag = 1 if there is are airpads for this motor */
   float fTarget;
   int settle;
@@ -670,8 +673,7 @@ static int DMC2280Run(void *pData,float fValue){
  * - 0 MOTOR BLOCKED, no significant change in position detected. 
  */
 static int checkMotion(void *pData) {
-#if 1
-  float precision, steps, counts, ratio_obs, ratio_exp;
+  float precision, steps, counts, ratio_obs, ratio_exp, ratio_cmp;
   long int usec_TimeDiff;
   struct timeval now;
 
@@ -690,14 +692,18 @@ static int checkMotion(void *pData) {
   MotorGetPar(self->pMot,"precision",&precision);
   if (FAILURE == readMotion(self, &steps, &counts))
     return 0;
-  /* If not stepping, thern not blocked */
-  if (steps == self->lastSteps)
+  /* If not stepping, then not blocked */
+  if (fabs(steps - self->lastSteps) < self->blockage_thresh * self->cntsPerX) {
+    /* just update the timestamp */
+    set_lastMotion(pData, self->lastSteps, self->lastCounts);
     return 1;
+  }
   /* calculate observed and expected steps per count ratios */
   ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts);
   ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX;
+  ratio_cmp = ratio_obs / ratio_exp;
   /* less than half or more than double is trouble */
-  if (ratio_obs / ratio_exp < 0.5 || ratio_obs / ratio_exp > 2.0) {
+  if (ratio_cmp > self->blockage_ratio || (1.0 / ratio_cmp) > self->blockage_ratio) {
     char msg[132];
     snprintf(msg, sizeof(msg), "Motion check fail: obs=%f, exp=%f",
         ratio_obs, ratio_exp);
@@ -706,17 +712,14 @@ static int checkMotion(void *pData) {
         steps, self->lastSteps, counts, self->lastCounts,
         self->stepsPerX, self->cntsPerX);
     SICSLogWrite(msg, eError);
-#if 1
+    if (self->blockage_fail)
+      return 0;
     set_lastMotion(pData, steps, counts);
     return 1;
-#else
-    return 0;
-#endif
   } else {
     set_lastMotion(pData, steps, counts);
     return 1;
   }
-#endif
   return 1;
 }
 
@@ -1157,6 +1160,18 @@ static int DMC2280GetPar(void *pData, char *name,
     *fValue = self->blockage_ckInterval;
     return 1;
   }
+  if(strcmp(name,"blockage_thresh") == 0) {
+    *fValue = self->blockage_thresh;
+    return 1;
+  }
+  if(strcmp(name,"blockage_ratio") == 0) {
+    *fValue = self->blockage_ratio;
+    return 1;
+  }
+  if(strcmp(name,"blockage_fail") == 0) {
+    *fValue = self->blockage_fail;
+    return 1;
+  }
   if (self->abs_endcoder != 0) {
     if (strcmp(name,"absenc") == 0) {
       if (readAbsEnc(self, fValue) == SUCCESS)
@@ -1271,6 +1286,39 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
     }
   }
 
+  /* Set interval between blocked motor checks,
+   * managers only */
+  if(strcmp(name,"blockage_thresh") == 0) {
+    if(!SCMatchRights(pCon,usMugger))
+      return 1;
+    else {
+      self->blockage_thresh = newValue;
+      return 1;
+    }
+  }
+
+  /* Set interval between blocked motor checks,
+   * managers only */
+  if(strcmp(name,"blockage_ratio") == 0) {
+    if(!SCMatchRights(pCon,usMugger))
+      return 1;
+    else {
+      self->blockage_ratio = newValue;
+      return 1;
+    }
+  }
+
+  /* Set interval between blocked motor checks,
+   * managers only */
+  if(strcmp(name,"blockage_fail") == 0) {
+    if(!SCMatchRights(pCon,usMugger))
+      return 1;
+    else {
+      self->blockage_fail = newValue;
+      return 1;
+    }
+  }
+
   /* Set speed */
   if(strcmp(name,SPEED) == 0) {
     if ((0.0 - newValue) > FLT_EPSILON) {
@@ -1362,6 +1410,14 @@ static void DMC2280List(void *self, char *name, SConnection *pCon){
   SCWrite(pCon, buffer, eStatus);
   snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, ((pDMC2280Driv)self)->settle);
   SCWrite(pCon, buffer, eStatus);
+  snprintf(buffer, BUFFLEN, "%s.Blockage_Check_Interval = %f\n", name, ((pDMC2280Driv)self)->blockage_ckInterval);
+  SCWrite(pCon, buffer, eStatus);
+  snprintf(buffer, BUFFLEN, "%s.Blockage_Thresh = %f\n", name, ((pDMC2280Driv)self)->blockage_thresh);
+  SCWrite(pCon, buffer, eStatus);
+  snprintf(buffer, BUFFLEN, "%s.Blockage_Ratio = %f\n", name, ((pDMC2280Driv)self)->blockage_ratio);
+  SCWrite(pCon, buffer, eStatus);
+  snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, ((pDMC2280Driv)self)->blockage_fail);
+  SCWrite(pCon, buffer, eStatus);
   snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, ((pDMC2280Driv)self)->has_airpads);
   SCWrite(pCon, buffer, eStatus);
   snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, ((pDMC2280Driv)self)->abs_endcoder);
@@ -1522,6 +1578,9 @@ static void KillDMC2280(/*@only@*/void *pData){
   pNew->ListDriverPar = DMC2280List;
   pNew->KillPrivate = KillDMC2280;
   pNew->blockage_ckInterval = 0.5;
+  pNew->blockage_thresh = 0.5;
+  pNew->blockage_ratio = 2.0;
+  pNew->blockage_fail = 0;
 
   /* PARAMETERS: Fetch parameter values */
   if ((pPtr=getParam(pCon, interp, params,HARDLOWERLIM,_REQUIRED)) == NULL) {