Bug fixes and improved error handling, more on controller variables

r2232 | dcl | 2007-11-06 08:50:18 +1100 (Tue, 06 Nov 2007) | 2 lines

site_ansto/motor_dmc2280.c

@@ -78,12 +78,13 @@ struct EvtEvent_s {
   } event;
 };
 
-#define VAR_REQ 1
+#define VAR_REQ (1<<0)
-#define VAR_RSP 2
+#define VAR_RSP (1<<1)
-#define VAR_RUN 4
+#define VAR_RUN (1<<2)
-#define VAR_DST 8
+#define VAR_DST (1<<3)
-#define VAR_POS 16
+#define VAR_POS (1<<4)
-#define VAR_HLT 32
+#define VAR_HLT (1<<5)
+#define VAR_ENC (1<<6)
 
 static pAsyncProtocol DMC2280_Protocol = NULL;
 
@@ -151,6 +152,7 @@ struct __MoDriv {
   unsigned short int input0; /**< last read of _TI0 */
   unsigned short int input1; /**< last read of _TI1 */
   bool ampError; /**< amplifier error */
+  bool runError; /**< motor error */
   bool threadError; /**< thread error */
   bool moving; /**< true if moving */
   struct timeval time_lastPos_set; /**< Time when lastPosition was set */
@@ -221,6 +223,7 @@ int DMC2280MotionControl = 1; /* defaults to enabled */
 #define STATEERROR -20
 #define THREADZERO -21 /* Thread zero not running */
 //#define AMPERROR   -22 /* Amplifier OK not asserted */
+#define RUNERROR -23 /* Error from RUN variable */
 /*--------------------------------------------------------------------*/
 #define STATUSMOVING 128 /* Motor is moving */
 #define STATUSERRORLIMIT 64 /* Number of errors exceed limit */
@@ -1173,7 +1176,7 @@ static bool has_var_x(pDMC2280Driv self, const char* vars, const char* name) {
   if (p == NULL)  /* not found at all */
     return false;
   if (p == vars)  /* found at start of first line */
-    return false;
+    return true;
   if (p[-1] == '\n') /* found at start of another line */
     return true;
   return false; /* found partial (ZVVVx=) only */
@@ -1190,6 +1193,19 @@ static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd);
  *
  * \param self access to the drive data
  */
+#if 1
+static int cmdStatus(pDMC2280Driv self) {
+  char cmd[CMDLEN];
+
+  /* TODO: Use POSx, ENCx, RUNx if it has these variables */
+  snprintf(cmd, CMDLEN, "MG {F10.0} %s%c,%s%c,_TS%c,%s%c,_TI0,_TI1,_XQ0",
+      (self->variables & VAR_POS) ? "POS" : "_TD", self->axisLabel,
+      (self->variables & VAR_ENC) ? "ENC" : "_TP", self->axisLabel,
+       self->axisLabel,
+      (self->variables & VAR_RUN) ? "RUN" : "_BG", self->axisLabel);
+  return DMC_SendCmd(self, cmd, state_msg_callback);
+}
+#else
 static int cmdStatus(pDMC2280Driv self) {
   char cmd[CMDLEN];
 
@@ -1201,6 +1217,7 @@ static int cmdStatus(pDMC2280Driv self) {
         self->axisLabel, self->axisLabel, self->axisLabel, self->axisLabel);
   return DMC_SendCmd(self, cmd, state_msg_callback);
 }
+#endif
 
 static int cmdVars(pDMC2280Driv self) {
   char cmd[CMDLEN];
@@ -1293,10 +1310,17 @@ static int rspStatus(pDMC2280Driv self, const char* text) {
   self->thread0 = iXQ0;
   self->input0 = iTI0;
   self->input1 = iTI1;
-  if (self->axisLabel >= 'A' && self->axisLabel <= 'D')
+#ifdef AMPERROR
+  if (self->variables & VAR_RUN)
+    self->ampError = 0;
+  else if (self->axisLabel >= 'A' && self->axisLabel <= 'D')
     self->ampError = !(1 & (iTI0 >> (self->axisLabel - 'A')));
   else
     self->ampError = !(1 & (iTI1 >> (self->axisLabel - 'E')));
+#endif
+  self->runError = iBG < 0 ? iBG : 0;
+  if (iBG < 0)
+    snprintf(self->dmc2280Error, CMDLEN, "MOTOR CONTROLLER RUN ERROR: %d", iBG);
   self->threadError = self->thread0 < 0;
   self->moving = iBG > 0;
   return 1;
@@ -1316,6 +1340,8 @@ static int rspVars(pDMC2280Driv self, const char* text) {
     self->variables |= VAR_POS;
   if (has_var_x(self, text, "HLT"))
     self->variables |= VAR_HLT;
+  if (has_var_x(self, text, "ENC"))
+    self->variables |= VAR_ENC;
   if ((self->variables & VAR_REQ) &&
       (self->variables & VAR_RSP)) {
     self->has_airpads = 2;
@@ -1577,9 +1603,15 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
     case eStateEvent:
       if (self->state_timer)
         NetWatchRemoveTimer(self->state_timer);
-      NetWatchRegisterTimer(&self->state_timer,
+      if ((self->variables & VAR_RUN) && self->runError) {
-          MOTOR_POLL_SLOW,
+        char cmd[CMDLEN];
-          state_tmr_callback, self);
+        snprintf(cmd, CMDLEN, "RUN%c=0", self->axisLabel);
+        DMC_SendCmd(self, cmd, state_msg_callback);
+      }
+      else
+        NetWatchRegisterTimer(&self->state_timer,
+            MOTOR_POLL_SLOW,
+            state_tmr_callback, self);
       self->subState = 0;
       return;
     case eTimerEvent:
@@ -1589,6 +1621,12 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
     case eMessageEvent:
       do {
         pAsyncTxn pCmd = event->event.msg.cmd;
+        if (self->subState == 0) { /* RUNx reset */
+          NetWatchRegisterTimer(&self->state_timer,
+              MOTOR_POLL_SLOW,
+              state_tmr_callback, self);
+          return;
+        }
         if (self->subState == 1) { /* Status Response */
           int iRet;
           float fDelta;
@@ -1649,7 +1687,6 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
 }
 
 static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
-  char cmd[CMDLEN];
   switch (event->event_type) {
     case eStateEvent:
       if (self->state_timer) {
@@ -1669,6 +1706,10 @@ static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event) {
         self->errorCode = THREADZERO;
         self->driver_status = HWFault;
       }
+      else if (self->runError) {
+        self->errorCode = RUNERROR;
+        self->driver_status = HWFault;
+      }
 #ifdef AMPERROR
       else if (self->ampError) {
         self->errorCode = AMPERROR;
@@ -1768,14 +1809,20 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
           if (self->threadError) {
             self->errorCode = THREADZERO;
             self->driver_status = HWFault;
-            change_state(self, DMCState_Error);
+            change_state(self, DMCState_MotorStop);
+            return;
+          }
+          else if (self->runError) {
+            self->errorCode = RUNERROR;
+            self->driver_status = HWFault;
+            change_state(self, DMCState_MotorStop);
             return;
           }
 #ifdef AMPERROR
           else if (self->ampError) {
             self->errorCode = AMPERROR;
             self->driver_status = HWFault;
-            change_state(self, DMCState_Error);
+            change_state(self, DMCState_MotorStop);
             return;
           }
 #endif
@@ -1874,27 +1921,31 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
         }
         else if (self->subState == 2) { /* MG */
           int iRet, iFlags;
-          bool moving, fwd_limit_active, rvrs_limit_active, errorlimit;
+          bool fwd_limit_active, rvrs_limit_active, errorlimit;
           iRet = rspStatus(self, pCmd->inp_buf);
           if (iRet == 0)
             break;
           if (self->threadError) {
             self->errorCode = THREADZERO;
             self->driver_status = HWFault;
-            change_state(self, DMCState_Error);
+            change_state(self, DMCState_MotorHalt);
+            return;
+          }
+          else if (self->runError) {
+            self->errorCode = RUNERROR;
+            self->driver_status = HWFault;
+            change_state(self, DMCState_MotorHalt);
             return;
           }
 #ifdef AMPERROR
           else if (self->ampError) {
             self->errorCode = AMPERROR;
             self->driver_status = HWFault;
-            change_state(self, DMCState_Error);
+            change_state(self, DMCState_MotorHalt);
             return;
           }
 #endif
-          iFlags = self->currFlags;
+          if (self->moving) {
-          moving = (iFlags & STATUSMOVING);
-          if (moving) {
             /* If Motion Control is off, report HWFault */
             if (DMC2280MotionControl != 1) {
               if (DMC2280MotionControl == 0)
@@ -1921,6 +1972,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
           }
           /* Motor has stopped */
           /* Handle limit switches */
+          iFlags = self->currFlags;
           fwd_limit_active = !(iFlags & STATUSFWDLIMIT);
           rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT);
           errorlimit = (iFlags & STATUSERRORLIMIT);
@@ -2082,7 +2134,6 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
 }
 
 static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) {
-  char cmd[CMDLEN];
   switch (event->event_type) {
     case eStateEvent:
       if (self->state_timer)
@@ -2104,12 +2155,11 @@ static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event) {
           return;
         }
         else if (self->subState == 1) { /* Status Response */
-          int iRet, iFlags;
+          int iRet;
           iRet = rspStatus(self, pCmd->inp_buf);
           if (iRet == 0)
             break;
-          iFlags = self->currFlags;
+          if (self->moving) {
-          if (iFlags & STATUSMOVING) {
             NetWatchRegisterTimer(&self->state_timer,
                 MOTOR_POLL_FAST,
                 state_tmr_callback, self);
@@ -2164,7 +2214,6 @@ static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
 }
 
 static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event) {
-  char cmd[CMDLEN];
   switch (event->event_type) {
     case eStateEvent:
       cmdOff(self);
@@ -2469,6 +2518,9 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
       strncpy(error, "DRIVE AMPLIFIER ERROR", (size_t)errLen);
       break;
 #endif
+    case RUNERROR:
+      strncpy(error, self->dmc2280Error, (size_t)errLen);
+      break;
     default:
       /* FIXME What's the default */
       snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode);
@@ -2506,6 +2558,7 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
 #ifdef AMPERROR
     case AMPERROR:
 #endif
+    case RUNERROR:
       return MOTFAIL;
     case POSFAULT:
 #if HAS_RS232