Set Motor status to HWBusy in State_Unknown

Discard outstanding message response on entering State_Unknown (timing window)
Don't break if it times out while waiting
Defer various faults until motor has stopped
Rename absenc to absencoder in list command (if has encoder)
Modify absenc to report integer encoder value
Allow manager to set various sensitive values in motor debug mode
Make motor "reset" command wait until motor reinitialised (timing window)

r2520 | dcl | 2008-05-12 11:05:35 +1000 (Mon, 12 May 2008) | 9 lines

site_ansto/motor_dmc2280.c

@@ -1018,7 +1018,7 @@ static void report_motion(pDMC2280Driv self) {
  * SUCCESS
  * FAILURE
  */
-static int readAbsEnc(pDMC2280Driv self, float *pos) {
+static int readAbsEnc(pDMC2280Driv self, long *pos) {
   char reply[CMDLEN];
   char cmd[CMDLEN];
 
@@ -1026,7 +1026,7 @@ static int readAbsEnc(pDMC2280Driv self, float *pos) {
   if (FAILURE == DMC_SendReceive(self, cmd, reply))
     return FAILURE;
 
-  *pos = (double) atol(reply);
+  *pos = atol(reply);
   return SUCCESS;
 }
 
@@ -1772,8 +1772,14 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
   switch (event->event_type) {
     case eStateEvent:
       self->run_flag = 0;
-      self->driver_status = HWIdle;
+      self->driver_status = HWBusy;
       self->errorCode = 0;
+      /* handle pending message event */
+      if (self->waitResponse) {
+        self->subState = 0;
+        /* FIXME: FIXME_DOUG */
+        return;
+      }
       /* Set speed */
       value =  motSpeed(self, self->speed);
       snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value);
@@ -1808,6 +1814,10 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
     case eMessageEvent:
       do {
         pAsyncTxn pCmd = event->event.msg.cmd;
+        if (self->subState == 0) { /* waitResponse */
+          change_state(self, DMCState_Unknown);
+          return;
+        }
         if (self->subState == 1) { /* Vars */
           rspVars(self, pCmd->inp_buf);
           cmdStatus(self);
@@ -1841,7 +1851,7 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
     case eTimeoutEvent:
       /* handle timeout */
       change_state(self, DMCState_Unknown);
-      break;
+      return;
   }
   unhandled_event(self, event);
   self->errorCode = STATEERROR;
@@ -2246,7 +2256,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
           if (pCmd->inp_buf[0] == '?') {
             /* TODO: what happens when BGx fails? */
             self->errorCode = BADCMD;
-            self->driver_status = HWFault;
+            self->faultPending = true; /* defer fault */
             change_state(self, DMCState_MotorHalt);
             return;
           }
@@ -2272,13 +2282,13 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
           errorlimit = (iFlags & STATUSERRORLIMIT);
           if (self->threadError) {
             self->errorCode = THREADZERO;
-            self->driver_status = HWFault;
+            self->faultPending = true; /* defer fault */
             change_state(self, DMCState_MotorHalt);
             return;
           }
           else if (self->runError) {
             self->errorCode = RUNERROR;
-            self->driver_status = HWFault;
+            self->faultPending = true; /* defer fault */
             change_state(self, DMCState_MotorHalt);
             return;
           }
@@ -2293,30 +2303,32 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
           if (self->stopCode != 0 && self->stopCode != 1) {
             if (self->stopCode == 2) {
               self->errorCode = FWDLIM;
-              self->driver_status = HWFault;
+              self->faultPending = true; /* defer fault */
               change_state(self, DMCState_MotorHalt);
               return;
             }
             else if (self->stopCode == 3) {
               self->errorCode = RVRSLIM;
-              self->driver_status = HWFault;
+              self->faultPending = true; /* defer fault */
               change_state(self, DMCState_MotorHalt);
               return;
             }
             else if (self->stopCode == 4) {
               if (fwd_limit_active && rvrs_limit_active) {
                 self->errorCode = IMPOSSIBLE_LIM_SW;
-                self->driver_status = HWFault;
+                self->faultPending = true; /* defer fault */
+                change_state(self, DMCState_MotorHalt);
+                return;
               }
               else if (fwd_limit_active) {
                 self->errorCode = FWDLIM;
-                self->driver_status = HWFault;
+                self->faultPending = true; /* defer fault */
+                change_state(self, DMCState_MotorHalt);
+                return;
               }
               else if (rvrs_limit_active) {
                 self->errorCode = RVRSLIM;
-                self->driver_status = HWFault;
+                self->faultPending = true; /* defer fault */
-              }
-              if (self->driver_status == HWFault) {
                 change_state(self, DMCState_MotorHalt);
                 return;
               }
@@ -2324,7 +2336,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
             self->errorCode = RUNERROR;
             snprintf(self->dmc2280Error, CMDLEN, "BAD Stop Code %d",
                 self->stopCode);
-            self->driver_status = HWFault;
+            self->faultPending = true; /* defer fault */
             change_state(self, DMCState_MotorHalt);
             return;
           }
@@ -2335,7 +2347,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
                 self->errorCode = MOTIONCONTROLOFF;
               else
                 self->errorCode = MOTIONCONTROLUNK;
-              self->driver_status = HWFault;
+              self->faultPending = true; /* defer fault */
               change_state(self, DMCState_MotorHalt);
               return;
             }
@@ -2343,14 +2355,14 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
               if (checkMotion(self) == 0) {
                 /* handle blocked */
                 self->errorCode = BLOCKED;
-                self->driver_status = HWFault;
+                self->faultPending = true; /* defer fault */
                 change_state(self, DMCState_MotorHalt);
                 return;
               }
               if (checkPosition(self) == 0) {
                 /* handle runaway */
-                self->errorCode = POSFAULT;
+                self->errorCode = POSFAULT; /* recoverable fault */
-                self->faultPending = true; /* defer recoverable fault */
+                self->faultPending = true; /* defer fault */
                 change_state(self, DMCState_MotorHalt);
                 return;
               }
@@ -3142,13 +3154,25 @@ static int DMC2280GetPar(void *pData, char *name,
       return 1;
     }
   }
+  if (strcasecmp(name,"absencoder") == 0) {
+    if (self->abs_encoder)
+      *fValue = 1.0;
+    else
+      *fValue = 0.0;
+    return 1;
+  }
   if (self->abs_encoder != 0) {
     if (strcasecmp(name,"absenc") == 0) {
-      if (readAbsEnc(self, fValue) == SUCCESS)
+      long lValue;
+      if (readAbsEnc(self, &lValue) == SUCCESS) {
+        *fValue = (double) lValue;
         return 1;
-      else
+      }
+      else {
+        *fValue = -1.0;
         return 0;
       }
+    }
     if (strcasecmp(name,"absenchome") == 0) {
       *fValue = self->absEncHome;
       return 1;
@@ -3215,27 +3239,6 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
 
   self = (pDMC2280Driv)pData;
 
-  /* XXX Maybe move this to a configuration parameter.*/
-  /* Set home, managers only. Users should set softposition */
-  if(strcasecmp(name,HOME) == 0) {
-    if(!SCMatchRights(pCon,usMugger))
-      return 1;
-    else {
-      if ( (self->fLower - newValue) > FLT_EPSILON) {
-        snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, HOME, self->fLower);
-        SCWrite(pCon, pError, eError);
-        return 1;
-      }
-      if ( (newValue - self->fUpper) > FLT_EPSILON) {
-        snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, HOME, self->fUpper);
-        SCWrite(pCon, pError, eError);
-        return 1;
-      }
-      self->fHome = newValue;
-      return 1;
-    }
-  }
-
   if(strcasecmp(name,SETPOS) == 0) {
     float oldZero, newZero;
     if (self->pMot == NULL)
@@ -3436,6 +3439,56 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
       return 0; /* FIXME should signal a HWFault */
     return 1;
   }
+  if (strcasecmp("hardlowerlim", name) == 0) {
+    /* Debug Managers only */
+    if (!(self->debug && SCMatchRights(pCon, usMugger)))
+      return 0;
+    self->fLower = newValue;
+    return 1;
+  }
+  if (strcasecmp("hardupperlim", name) == 0) {
+    /* Debug Managers only */
+    if (!(self->debug && SCMatchRights(pCon, usMugger)))
+      return 0;
+    self->fUpper = newValue;
+    return 1;
+  }
+  if (self->abs_encoder && strcasecmp("stepsPerX", name) == 0) {
+    /* Debug Managers only */
+    if (!(self->debug && SCMatchRights(pCon, usMugger)))
+      return 0;
+    self->stepsPerX = newValue;
+    return 1;
+  }
+  if (self->debug && strcasecmp("cntsPerX", name) == 0) {
+    /* Debug Managers only */
+    if (!(self->debug && SCMatchRights(pCon, usMugger)))
+      return 0;
+    self->cntsPerX = newValue;
+    return 1;
+  }
+
+  /* XXX Maybe move this to a configuration parameter.*/
+  /* Set home, managers only. Users should set softposition */
+  if(strcasecmp(name, HOME) == 0) {
+    /* Debug Managers only */
+    if(!(self->debug && SCMatchRights(pCon,usMugger)))
+      return 1;
+    else {
+      if ( (self->fLower - newValue) > FLT_EPSILON) {
+        snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, HOME, self->fLower);
+        SCWrite(pCon, pError, eError);
+        return 1;
+      }
+      if ( (newValue - self->fUpper) > FLT_EPSILON) {
+        snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, HOME, self->fUpper);
+        SCWrite(pCon, pError, eError);
+        return 1;
+      }
+      self->fHome = newValue;
+      return 1;
+    }
+  }
 
   if (self->posit_count > 0) {
       int index;
@@ -3517,7 +3570,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){
   SCWrite(pCon, buffer, eStatus);
   snprintf(buffer, BUFFLEN, "%s.Protocol = %d\n", name, self->protocol);
   SCWrite(pCon, buffer, eStatus);
-  snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, self->abs_encoder);
+  snprintf(buffer, BUFFLEN, "%s.absEncoder = %d\n", name, self->abs_encoder);
   SCWrite(pCon, buffer, eStatus);
   if (self->abs_encoder) {
     snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, self->absEncHome);
@@ -4082,6 +4135,10 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
       self->state_timer = 0;
       self->driver_status = HWIdle;
       change_state(self, DMCState_Unknown);
+      while (self->myState == DMCState_Unknown) {
+        pTaskMan pTasker = GetTasker();
+        TaskYield(pTasker);
+      }
       /* Handle further in generic motor driver */
       return MotorAction(pCon, pSics, pData, argc, argv);
     }
@@ -4191,38 +4248,37 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
       }
       return 1;
     }
-    else if (self->debug && strcasecmp("hardlowerlim", argv[1]) == 0) {
+    if (self->abs_encoder && strcasecmp("absEncHome", argv[1]) == 0) {
-      /* Managers only */
+      if (argc > 2) {
-      if (!SCMatchRights(pCon, usMugger))
+        /* Debug Managers only */
+        if (!(self->debug && SCMatchRights(pCon, usMugger)))
           return 0;
-      float fValue = strtod(argv[2], NULL);
+        long lValue = strtol(argv[2], NULL, 10);
-      self->fLower = fValue;
+        self->absEncHome = lValue;
         return 1;
       }
-    else if (self->debug && strcasecmp("hardupperlim", argv[1]) == 0) {
+      else {
-      /* Managers only */
+        char line[132];
-      if (!SCMatchRights(pCon, usMugger))
+        snprintf(line, 132, "%s.absEncHome = %d.000000",
-        return 0;
+            self->name,
-      float fValue = strtod(argv[2], NULL);
+            self->absEncHome);
-      self->fUpper = fValue;
+        SCWrite(pCon, line, eValue);
         return 1;
       }
-    else if (self->debug && strcasecmp("stepsPerX", argv[1]) == 0) {
+    }
-      /* Managers only */
+    if (self->abs_encoder && strcasecmp("absenc", argv[1]) == 0) {
-      if (!SCMatchRights(pCon, usMugger))
+      long lValue;
-        return 0;
+      if (readAbsEnc(self, &lValue) != SUCCESS) {
-      float fValue = strtod(argv[2], NULL);
+        lValue = -1;
-      self->stepsPerX = fValue;
+      }
-      return 1;
+      char line[132];
-    }
+      snprintf(line, 132, "%s.absEnc = %ld.000000",
-    else if (self->debug && strcasecmp("cntsPerX", argv[1]) == 0) {
+          self->name,
-      /* Managers only */
+          lValue);
-      if (!SCMatchRights(pCon, usMugger))
+      SCWrite(pCon, line, eValue);
-        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];