Break down the motor states further to make them even simpler

site_ansto/motor_dmc2280.c

@@ -221,7 +221,7 @@ struct __MoDriv {
   int timerValue; /**< save for debug printing */
   StateFunc myState; /**< pointer to state action method */
   StateFunc myPrevState; /**< save for debug printing */
-  StateFunc myMoveCallerReturn; /**< state to return to */
+  StateFunc myNextState; /**< state to return to */
   int subState; /**< tracks substate within state method */
   bool waitResponse;    /**< true is a message sent and we wait for response */
   pNWTimer state_timer; /**< motor state timer */
@@ -1656,8 +1656,10 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event);
-static void DMCState_Backlash(pDMC2280Driv self, pEvtEvent event);
+static void DMCState_BacklashStart(pDMC2280Driv self, pEvtEvent event);
-static void DMCState_Creeping(pDMC2280Driv self, pEvtEvent event);
+static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event);
+static void DMCState_CreepStart(pDMC2280Driv self, pEvtEvent event);
+static void DMCState_CreepCont(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event);
@@ -1672,8 +1674,10 @@ static char* state_name(StateFunc func) {
   if (func == DMCState_Moving) return "DMCState_Moving";
   if (func == DMCState_StepMove) return "DMCState_StepMove";
   if (func == DMCState_SimpleMove) return "DMCState_SimpleMove";
-  if (func == DMCState_Backlash) return "DMCState_Backlash";
+  if (func == DMCState_BacklashStart) return "DMCState_BacklashStart";
-  if (func == DMCState_Creeping) return "DMCState_Creeping";
+  if (func == DMCState_BacklashCont) return "DMCState_BacklashCont";
+  if (func == DMCState_CreepStart) return "DMCState_CreepStart";
+  if (func == DMCState_CreepCont) return "DMCState_CreepCont";
   if (func == DMCState_MotorHalt) return "DMCState_MotorHalt";
   if (func == DMCState_OffTimer) return "DMCState_OffTimer";
   if (func == DMCState_MotorStop) return "DMCState_MotorStop";
@@ -2443,12 +2447,12 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
           cmdDecel(self); /* No Response */
 #endif
           if (self->testing) {
-            self->myMoveCallerReturn = NULL;
+            self->myNextState = NULL;
             if (self->backlash_offset != 0) {
-              change_state(self, DMCState_Backlash);
+              change_state(self, DMCState_BacklashStart);
               return;
             } else if (self->creep_offset != 0) {
-              change_state(self, DMCState_Creeping);
+              change_state(self, DMCState_CreepStart);
               return;
             }
             change_state(self, DMCState_SimpleMove);
@@ -2565,15 +2569,7 @@ static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event) {
 
   switch (event->event_type) {
     case eStateEvent:
-      /*
+      self->myNextState = NULL;
-       * If we are the myMoveCallerReturn then continue
-       */
-      if (DMCState_SimpleMove == self->myMoveCallerReturn) {
-        /* TODO: precision check and retry */
-        self->myMoveCallerReturn = NULL;
-        change_state(self, DMCState_OffTimer);
-        return;
-      }
       target = self->fTarget;
       absolute = motAbsol(self, target);
       self->doSettle = self->settle > 0;
@@ -2588,7 +2584,110 @@ static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event) {
         change_state(self, DMCState_MotorHalt);
         return;
       }
-      self->myMoveCallerReturn = DMCState_SimpleMove;
+      /* set the next state after this move */
+      self->myNextState = DMCState_OffTimer;
+      change_state(self, DMCState_StepMove);
+      return;
+    case eCommandEvent:
+      switch (event->event.cmd.cmd_type) {
+        case CMD_RUN:
+          /* TODO: FIXME RUN command while running */
+          if (self->driver_status == HWIdle)
+            self->driver_status = HWBusy;
+          self->run_flag = 1;
+          if (self->waitResponse == false) {
+            change_state(self, DMCState_MotorHalt);
+          }
+          return;
+        case CMD_HALT:
+          /* handle halt command, send message */
+          self->run_flag = -1;
+          if (self->waitResponse == false) {
+            change_state(self, DMCState_MotorHalt);
+          }
+          return;
+      }
+      break;
+    case eTimeoutEvent:
+      strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
+      self->errorCode = MOTCMDTMO;
+      self->driver_status = HWFault;
+      change_state(self, DMCState_MotorHalt);
+      return;
+  }
+  unhandled_event(self, event);
+  self->errorCode = STATEERROR;
+  change_state(self, DMCState_Error);
+  return;
+}
+
+/*
+ * This state sets up to moves the motor from its current position to fTarget + backlash_offset.
+ * If there is creep, it happens later.
+ * If there is a fault it transfers out
+ */
+static void DMCState_BacklashStart(pDMC2280Driv self, pEvtEvent event) {
+  double target;
+  int absolute;
+  bool doPreseek;
+
+  switch (event->event_type) {
+    case eStateEvent:
+      self->myNextState = NULL;
+      target = self->fTarget;
+      doPreseek = false;
+      if (self->backlash_offset != 0) {
+        if (self->backlash_offset > 0) {
+          /*
+           * We want to be moving from high to low, if the target is higher
+           * than the current position we must pre-seek to the higher side
+           */
+          if (target > self->currPosition) {
+            doPreseek = true;
+            target += self->backlash_offset;
+            if (target > self->fUpper)
+              target = self->fUpper;
+          }
+        }
+        else if (self->backlash_offset < 0) {
+          /*
+           * We want to be moving from low to high, if the target is lower than
+           * the current position we must pre-seek to the lower side
+           */
+          if (target < self->currPosition) {
+            doPreseek = true;
+            target += self->backlash_offset;
+            if (target < self->fLower)
+              target = self->fLower;
+          }
+        }
+      }
+      if (doPreseek == false) {
+        /* preseek is not required, handle as a simple move */
+        if (self->creep_offset != 0) {
+          change_state(self, DMCState_CreepStart);
+        } else {
+          change_state(self, DMCState_SimpleMove);
+        }
+        return;
+      }
+      self->preseek = 1;
+      self->fPreseek = target;
+      absolute = motAbsol(self, target);
+      /* decide if we should be letting the motor settle */
+      self->doSettle = self->settle > 0;
+      cmdPosition(self, absolute);
+      return;
+    case eTimerEvent:
+      cmdStatus(self);
+      return;
+    case eMessageEvent:
+      if (self->run_flag != 0) {
+        change_state(self, DMCState_MotorHalt);
+        return;
+      }
+      /* set the next state after this move */
+      self->myNextState = DMCState_BacklashCont;
       change_state(self, DMCState_StepMove);
       return;
     case eCommandEvent:
@@ -2626,96 +2725,60 @@ static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event) {
 
 /*
  * This state moves the motor from its current position to fTarget + backlash_offset.
- * There is no creep.
+ * If there is creep, it happens after this.
  * If there is a fault it transfers out
  */
-static void DMCState_Backlash(pDMC2280Driv self, pEvtEvent event) {
+static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event) {
   double target;
+  float precision;
   int absolute;
 
   switch (event->event_type) {
     case eStateEvent:
-      /*
+      self->myNextState = NULL;
-       * If we are the myMoveCallerReturn then continue
+      target = self->fTarget;
-       */
+      self->preseek = 0;
-      if (DMCState_Backlash == self->myMoveCallerReturn) {
+      /* take precision into account */
-        float precision;
+      MotorGetPar(self->pMot, "precision", &precision);
-        /* TODO: precision check and retry */
+      if (self->backlash_offset > 0) {
-        self->myMoveCallerReturn = NULL;
+        if (target + self->backlash_offset > self->currPosition + precision) {
-        target = self->fTarget;
+          self->preseek = 1;
-        self->preseek = 0;
+          target += self->backlash_offset + precision;
-        /* take precision into account */
+          if (target > self->fUpper)
-        MotorGetPar(self->pMot, "precision", &precision);
+            target = self->fUpper;
-        if (self->backlash_offset > 0) {
+          self->fPreseek = target;
-          if (target + self->backlash_offset > self->currPosition + precision) {
-            self->preseek = 1;
-            target += self->backlash_offset + precision;
-            if (target > self->fUpper)
-              target = self->fUpper;
-            self->fPreseek = target;
-          }
         }
-        else if (self->backlash_offset < 0) {
+      }
-          if (target + self->backlash_offset < self->currPosition - precision) {
+      else if (self->backlash_offset < 0) {
-            self->preseek = 1;
+        if (target + self->backlash_offset < self->currPosition - precision) {
-            target += self->backlash_offset - precision;
+          self->preseek = 1;
-            if (target < self->fLower)
+          target += self->backlash_offset - precision;
-              target = self->fLower;
+          if (target < self->fLower)
-            self->fPreseek = target;
+            target = self->fLower;
-          }
+          self->fPreseek = target;
         }
+      }
 
-        if (self->preseek && self->stepCount > 10) {
+      if (self->preseek && self->stepCount > 10) {
-          /* limit the maximum number of tries */
+        /* limit the maximum number of tries */
-          if (self->debug) {
+        if (self->debug) {
-            char line[CMDLEN];
+          char line[CMDLEN];
-            snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d",
+          snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d",
-                self->name, self->stepCount);
+              self->name, self->stepCount);
-            SICSLogWrite(line, eStatus);
+          SICSLogWrite(line, eStatus);
-          }
-          self->preseek = 0;
         }
-      } else {
-        target = self->fTarget;
         self->preseek = 0;
-        if (self->backlash_offset != 0) {
-          if (self->backlash_offset > 0) {
-            /*
-             * We want to be moving from high to low,
-             * if the target is higher than current
-             * we must pre-seek to the higher side
-             */
-            if (target > self->currPosition) {
-              self->preseek = 1;
-              target += self->backlash_offset;
-              if (target > self->fUpper)
-                target = self->fUpper;
-            }
-          }
-          else if (self->backlash_offset < 0) {
-            /*
-             * We want to be moving from low to high,
-             * if the target is lower than current
-             * we must pre-seek to the lower side
-             */
-            if (target < self->currPosition) {
-              self->preseek = 1;
-              target += self->backlash_offset;
-              if (target < self->fLower)
-                target = self->fLower;
-            }
-          }
-        }
       }
       if (self->preseek == 0) {
         /* preseek is not required, handle as a simple move */
         if (self->creep_offset != 0) {
-          change_state(self, DMCState_Creeping);
+          change_state(self, DMCState_CreepStart);
         } else {
           change_state(self, DMCState_SimpleMove);
         }
         return;
       }
+      self->preseek = 1;
       self->fPreseek = target;
       absolute = motAbsol(self, target);
       /* decide if we should be letting the motor settle */
@@ -2730,7 +2793,94 @@ static void DMCState_Backlash(pDMC2280Driv self, pEvtEvent event) {
         change_state(self, DMCState_MotorHalt);
         return;
       }
-      self->myMoveCallerReturn = DMCState_Backlash;
+      /* set the next state after this move */
+      self->myNextState = DMCState_BacklashCont;
+      change_state(self, DMCState_StepMove);
+      return;
+    case eCommandEvent:
+      switch (event->event.cmd.cmd_type) {
+        case CMD_RUN:
+          /* TODO: FIXME RUN command while running */
+          if (self->driver_status == HWIdle)
+            self->driver_status = HWBusy;
+          self->run_flag = 1;
+          if (self->waitResponse == false) {
+            change_state(self, DMCState_MotorHalt);
+          }
+          return;
+        case CMD_HALT:
+          /* handle halt command, send message */
+          self->run_flag = -1;
+          if (self->waitResponse == false) {
+            change_state(self, DMCState_MotorHalt);
+          }
+          return;
+      }
+      break;
+    case eTimeoutEvent:
+      strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
+      self->errorCode = MOTCMDTMO;
+      self->driver_status = HWFault;
+      change_state(self, DMCState_MotorHalt);
+      return;
+  }
+  unhandled_event(self, event);
+  self->errorCode = STATEERROR;
+  change_state(self, DMCState_Error);
+  return;
+}
+
+/*
+ * This state sets up to move the motor from its current position to fTarget by creeping.
+ * If there was a backlash correction, it has already happened.
+ * If there is a fault it transfers out
+ */
+static void DMCState_CreepStart(pDMC2280Driv self, pEvtEvent event) {
+  double target;
+  int absolute;
+
+  switch (event->event_type) {
+    case eStateEvent:
+      self->myNextState = NULL;
+      /* initialize the creep control variable */
+      self->creep_val = 0;
+
+      /*
+       * calculate the absolute step position of the adjusted target
+       */
+      target = self->fTarget;
+      self->preseek = 0;
+      absolute = motCreep(self, target);
+      if (self->preseek == 0) {
+        /* it is finished */
+        change_state(self, DMCState_OffTimer);
+        return;
+      }
+
+      /*
+       * decide if we should be letting the motor settle
+       */
+      self->doSettle = self->settle > 0;
+      if (self->doSettle) {
+        if (abs(absolute - self->currSteps) > fabs(self->creep_offset * self->stepsPerX))
+          self->doSettle = false;
+        if (self->creep_precision > 0.0)
+          if (abs(absolute - self->currSteps) > 10.0 * fabs(self->creep_precision * self->stepsPerX))
+          self->doSettle = false;
+      }
+      self->fPreseek = target;
+      cmdPosition(self, absolute);
+      return;
+    case eTimerEvent:
+      cmdStatus(self);
+      return;
+    case eMessageEvent:
+      if (self->run_flag != 0) {
+        change_state(self, DMCState_MotorHalt);
+        return;
+      }
+      /* set the next state after this move */
+      self->myNextState = DMCState_CreepCont;
       change_state(self, DMCState_StepMove);
       return;
     case eCommandEvent:
@@ -2771,25 +2921,17 @@ static void DMCState_Backlash(pDMC2280Driv self, pEvtEvent event) {
  * There is no backlash.
  * If there is a fault it transfers out
  */
-static void DMCState_Creeping(pDMC2280Driv self, pEvtEvent event) {
+static void DMCState_CreepCont(pDMC2280Driv self, pEvtEvent event) {
   double target;
   int absolute;
 
   switch (event->event_type) {
     case eStateEvent:
-      /*
+      self->myNextState = NULL;
-       * If we are the myMoveCallerReturn then continue
+      if (self->preseek == 0) {
-       */
+        /* it is finished */
-      if (DMCState_Creeping == self->myMoveCallerReturn) {
+        change_state(self, DMCState_OffTimer);
-        self->myMoveCallerReturn = NULL;
+        return;
-        if (self->preseek == 0) {
-          /* it is finished */
-          change_state(self, DMCState_OffTimer);
-          return;
-        }
-      } else {
-        /* first time, initialize */
-        self->creep_val = 0;
       }
       target = self->fTarget;
       self->preseek = 0;
@@ -2819,7 +2961,7 @@ static void DMCState_Creeping(pDMC2280Driv self, pEvtEvent event) {
         change_state(self, DMCState_MotorHalt);
         return;
       }
-      self->myMoveCallerReturn = DMCState_Creeping;
+      self->myNextState = DMCState_CreepCont;
       change_state(self, DMCState_StepMove);
       return;
     case eCommandEvent:
@@ -2856,9 +2998,9 @@ static void DMCState_Creeping(pDMC2280Driv self, pEvtEvent event) {
 }
 
 /*
- * This state moves the motor one step from its current position to the current destination
+ * This state moves the motor from its current position to the current destination
- * set by PA (or PR) by the caller in myMoveCallerReturn.
+ * set by PA (or PR) by the caller in myNextState.
- * If there is a fault it transfers out, otherwise it returns to the previous
+ * If there is a fault it transfers out, otherwise it moves to the myNextState
  * state when the move is successfully completed.
  */
 static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
@@ -2980,7 +3122,7 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
               }
             }
           }
-          change_state(self, self->myMoveCallerReturn);
+          change_state(self, self->myNextState);
           return;
         }
       } while (0);
@@ -5007,13 +5149,19 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
       return 1;
     }
     else if(strcasecmp("testing", argv[1]) == 0) {
-      if (argc > 2 && strcasecmp("on", argv[2]) == 0) {
+      if (argc > 2) {
-        self->testing = true;
+        if (strcasecmp("on", argv[2]) == 0) {
-        SCWrite(pCon, "TESTING ON", eValue);
+          self->testing = true;
-      }
+          SCWrite(pCon, "TESTING ON", eValue);
-      else {
+        }
-        self->testing = false;
+        else {
-        SCWrite(pCon, "TESTING OFF", eValue);
+          self->testing = false;
+          SCWrite(pCon, "TESTING OFF", eValue);
+        }
+      } else {
+        SCPrintf(pCon, eValue, "%s.testing = %s",
+            self->name,
+            self->testing ? "ON" : "OFF");
       }
       return 1;
     }