First pass at Universal Unidirectional Motor Driving (UUMD)

r1849 | dcl | 2007-04-12 16:59:32 +1000 (Thu, 12 Apr 2007) | 2 lines

site_ansto/motor_dmc2280.c

@@ -100,13 +100,14 @@ typedef struct __MoDriv {
   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;
+  float backlash_offset; /**< signed offset to drive from */
-  int settle;
+  float fTarget; /**< target passed from SICS to timer callback */
-  struct timeval time_settle_done;
+  int settle; /**< motor settling time in seconds */
-  int airpad_state;
+  struct timeval time_settle_done; /**< time when settling will be over */
-  int airpad_counter;
+  int airpad_state; /**< state of the airpads finite state machine */
-  pNWTimer airpad_timer;
+  int airpad_counter; /**< airpad timer retry counter */
-  pNWTimer motor_timer;
+  pNWTimer airpad_timer; /**< timer waiting for airpad action to complete */
+  pNWTimer motor_timer; /**< motor off timer */
   int debug;
 } DMC2280Driv, *pDMC2280Driv;
 
@@ -711,6 +712,17 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
   snprintf(BGx, CMDLEN, "BG%c", axis);
   target = fValue - self->home;
 
+  if (self->backlash_offset > FLT_EPSILON && target < self->lastPosition) {
+    target += self->backlash_offset;
+    if (target > self->fUpper)
+      target = self->fUpper;
+  }
+  if (self->backlash_offset < -FLT_EPSILON && target > self->lastPosition) {
+    target += self->backlash_offset;
+    if (target < self->fLower)
+      target = self->fLower;
+  }
+
   if (1 == self->abs_endcoder) {
     absEncHome = self->absEncHome;
     cntsPerX = self->cntsPerX;
@@ -762,7 +774,7 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
       else
         break;
     snprintf(absPosCmd, CMDLEN,
-        "PA%c=(((%d-_TP%c)+(%s*%s))*%s/%s+_TD%c",
+        "PA%c=((%d-_TP%c)+(%s*%s))*%s/%s+_TD%c",
         axis,
         absEncHome,
         axis,
@@ -956,12 +968,11 @@ static int DMC2280Run(void *pData,float fValue){
    * Note: this will read the current position which will block
    */
   do {
-#if 1
-    self->time_lastPos_set.tv_sec = 0;
-#else
     float currPos;
     DMC2280GetPos(pData, &currPos);
-    set_lastPos(pData, currPos);
+    self->lastPosition = currPos;
+#if 1
+    self->time_lastPos_set.tv_sec = 0;
 #endif
   } while (0);
 
@@ -1503,6 +1514,10 @@ static int DMC2280GetPar(void *pData, char *name,
     *fValue = self->blockage_fail;
     return 1;
   }
+  if(strcasecmp(name,"backlash_offset") == 0) {
+    *fValue = self->backlash_offset;
+    return 1;
+  }
   if (self->abs_endcoder != 0) {
     if (strcasecmp(name,"absenc") == 0) {
       if (readAbsEnc(self, fValue) == SUCCESS)
@@ -1678,6 +1693,17 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
     }
   }
 
+  /* Set backlash offset,
+   * managers only */
+  if(strcasecmp(name,"backlash_offset") == 0) {
+    if(!SCMatchRights(pCon,usMugger))
+      return 1;
+    else {
+      self->backlash_offset = newValue;
+      return 1;
+    }
+  }
+
   /* Invoke Home Run routine in controller,
    * managers only */
   if(self->abs_endcoder == 0 && strcasecmp(name,"homerun") == 0) {
@@ -1796,6 +1822,8 @@ static void DMC2280List(void *self, char *name, SConnection *pCon){
   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.Backlash_offset = %d\n", name, ((pDMC2280Driv)self)->backlash_offset);
+  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);
@@ -1943,6 +1971,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
   pNew->blockage_thresh = 0.5;
   pNew->blockage_ratio = 2.0;
   pNew->blockage_fail = 0;
+  pNew->backlash_offset = 0.0;
 
   /* PARAMETERS: Fetch parameter values */
   if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) {
@@ -2019,6 +2048,13 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
     sscanf(pPtr,"%d",&(pNew->settle));
   }
 
+  /* BACKLASH: this motor need airpads */
+  if ((pPtr=getParam(pCon, interp, params,"backlash_offset",_OPTIONAL)) == NULL)
+    pNew->backlash_offset=0.0;
+  else {
+    sscanf(pPtr,"%f",&(pNew->backlash_offset));
+  }
+
   /* AIRPADS: this motor need airpads */
   if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL)
     pNew->has_airpads=0;
@@ -2036,7 +2072,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
     else
       sscanf(pPtr,"%d",&(pNew->absEncHome));
     if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL)
-      pNew->cntsPerX=1;
+      pNew->cntsPerX=1.0;
     else
       sscanf(pPtr,"%f",&(pNew->cntsPerX));
   }