Separate motor states for move, backlash and creep

site_ansto/motor_dmc2280.c

@@ -207,6 +207,7 @@ struct __MoDriv {
   double fPreseek; /**< preseek target when preseek is active */
   int settle; /**< motor settling time in milliseconds */
   struct timeval time_settle_done; /**< time when settling will be over */
+  bool doSettle; /**< flag to request settle after move, autoreset */
   int debug;
   int stepCount; /**< number of step operations for this move cycle */
   float creep_offset; /**< last little bit to creep in units */
@@ -220,6 +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 */
   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 */
@@ -237,6 +239,7 @@ struct __MoDriv {
   int bias_bits; /**< number of bits to mask */
   int bias_bias; /**< bias to add to encoder value */
   char ao_id[256];
+  bool testing; /**< flag for testing new code */
 };
 
 int DMC2280MotionControl = 1; /* defaults to enabled */
@@ -1632,6 +1635,10 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event);
 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_Creeping(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);
@@ -1644,6 +1651,10 @@ static char* state_name(StateFunc func) {
   if (func == DMCState_MotorStart) return "DMCState_MotorStart";
   if (func == DMCState_MotorOn) return "DMCState_MotorOn";
   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_Creeping) return "DMCState_Creeping";
   if (func == DMCState_MotorHalt) return "DMCState_MotorHalt";
   if (func == DMCState_OffTimer) return "DMCState_OffTimer";
   if (func == DMCState_MotorStop) return "DMCState_MotorStop";
@@ -2412,6 +2423,18 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
           cmdAccel(self); /* No Response */
           cmdDecel(self); /* No Response */
 #endif
+          if (self->testing) {
+            self->myMoveCallerReturn = NULL;
+            if (self->backlash_offset != 0) {
+              change_state(self, DMCState_Backlash);
+              return;
+            } else if (self->creep_offset != 0) {
+              change_state(self, DMCState_Creeping);
+              return;
+            }
+            change_state(self, DMCState_SimpleMove);
+            return;
+          }
           set_lastMotion(self, self->currSteps, self->currCounts);
           /* compute position for PA command */
           target = self->fTarget;
@@ -2512,6 +2535,433 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
   return;
 }
 
+/*
+ * This state moves the motor from its current position to fTarget.
+ * There is no backlash and no creep.
+ * If there is a fault it transfers out
+ */
+static void DMCState_SimpleMove(pDMC2280Driv self, pEvtEvent event) {
+  double target;
+  int absolute;
+
+  switch (event->event_type) {
+    case eStateEvent:
+      /*
+       * 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;
+      /* decide if we should be letting the motor settle */
+      cmdPosition(self, absolute);
+      return;
+    case eTimerEvent:
+      cmdStatus(self);
+      return;
+    case eMessageEvent:
+      if (self->run_flag != 0) {
+        change_state(self, DMCState_MotorHalt);
+        return;
+      }
+      self->myMoveCallerReturn = DMCState_SimpleMove;
+      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 moves the motor from its current position to fTarget + backlash_offset.
+ * There is no creep.
+ * If there is a fault it transfers out
+ */
+static void DMCState_Backlash(pDMC2280Driv self, pEvtEvent event) {
+  double target;
+  int absolute;
+
+  switch (event->event_type) {
+    case eStateEvent:
+      /*
+       * If we are the myMoveCallerReturn then continue
+       */
+      if (DMCState_Backlash == self->myMoveCallerReturn) {
+        float precision;
+        /* TODO: precision check and retry */
+        self->myMoveCallerReturn = NULL;
+        target = self->fTarget;
+        self->preseek = 0;
+        /* take precision into account */
+        MotorGetPar(self->pMot, "precision", &precision);
+        if (self->backlash_offset > 0) {
+          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) {
+            self->preseek = 1;
+            target += self->backlash_offset - precision;
+            if (target < self->fLower)
+              target = self->fLower;
+            self->fPreseek = target;
+          }
+        }
+
+        if (self->preseek && self->stepCount > 10) {
+          /* limit the maximum number of tries */
+          if (self->debug) {
+            char line[CMDLEN];
+            snprintf(line, CMDLEN, "Motor=%s preseek stopped, stepcount = %d",
+                self->name, self->stepCount);
+            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);
+        } else {
+          change_state(self, DMCState_SimpleMove);
+        }
+        return;
+      }
+      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;
+      }
+      self->myMoveCallerReturn = DMCState_Backlash;
+      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 moves the motor from its current position to fTarget by creeping.
+ * There is no backlash.
+ * If there is a fault it transfers out
+ */
+static void DMCState_Creeping(pDMC2280Driv self, pEvtEvent event) {
+  double target;
+  int absolute;
+
+  switch (event->event_type) {
+    case eStateEvent:
+      /*
+       * If we are the myMoveCallerReturn then continue
+       */
+      if (DMCState_Creeping == self->myMoveCallerReturn) {
+        self->myMoveCallerReturn = NULL;
+        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;
+      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;
+      }
+      self->myMoveCallerReturn = DMCState_Creeping;
+      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 moves the motor one step from its current position to the current destination
+ * set by PA (or PR) by the caller in myMoveCallerReturn.
+ * If there is a fault it transfers out, otherwise it returns to the previous
+ * state when the move is successfully completed.
+ */
+static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) {
+  switch (event->event_type) {
+      /*
+       * The PA has already been confirmed by the caller, now do the move
+       */
+    case eStateEvent:
+      /* save pre-motion values for logging */
+      set_lastMotion(self, self->currSteps, self->currCounts);
+      self->origTime = DoubleTime();
+      self->origPosition = self->currPosition;
+      self->origCounts = self->currCounts;
+      self->origSteps = self->currSteps;
+      self->minRatio = 0.0;
+      self->maxRatio = 0.0;
+      /* begin moving */
+      cmdBegin(self);
+      self->stepCount = 1;
+      self->subState = 1;
+      return;
+    case eTimerEvent:
+      /*
+       * Find out how the motor is travelling,
+       * Note that the substate has already been set
+       */
+      cmdStatus(self);
+      return;
+    case eMessageEvent:
+      if (self->run_flag != 0) {
+        change_state(self, DMCState_MotorHalt);
+        return;
+      }
+      do {
+        pAsyncTxn pCmd = event->event.msg.cmd;
+        if (self->subState == 1) { /* BG */
+          /* Check if BG worked (reply != '?') */
+          if (pCmd->inp_buf[0] == '?') {
+            /* TODO: what happens when BGx fails? */
+            self->errorCode = BADCMD;
+            self->faultPending = true; /* defer fault */
+            change_state(self, DMCState_MotorHalt);
+            return;
+          }
+          self->subState = 2;
+          DMC_SetTimer(self, self->motorPollFast);
+          return;
+        } else if (self->subState == 2) { /* Status */
+          int iRet;
+          /* parse the status response and set error codes */
+          iRet = rspStatus(self, pCmd->inp_buf);
+          /* if the parse failed break out of here */
+          if (iRet == 0)
+            break;
+          /* if the status response can be handled here, return */
+          if (motorHandleStatus(self))
+            return;
+          /*
+           * We get here when the motor stops normally
+           */
+          if (self->doSettle) {
+            self->doSettle = false;
+            DMC_SetTimer(self, self->settle);
+            return;
+          }
+          if (true /*self->debug*/) {
+            double units = self->currPosition - self->origPosition;
+            long int steps = self->currSteps - self->origSteps;
+            long int counts = self->currCounts - self->origCounts;
+            double time = DoubleTime() - self->origTime;
+            char line[CMDLEN];
+            snprintf(line, CMDLEN, "Motor=%s stopped: units=%.6f,"
+                " steps=%ld, counts=%ld, stepsPerX=%.6f,"
+                " ratio=(%.6f-%.6f), time=%.3f",
+                self->name,
+                units,
+                steps,
+                counts,
+                (double) steps / units,
+                self->minRatio,
+                self->maxRatio,
+                time);
+            SICSLogWrite(line, eStatus);
+          }
+          change_state(self, self->myMoveCallerReturn);
+          return;
+        }
+      } while (0);
+      break;
+    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;
+}
+
 static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
   /*
    * Substates:
@@ -4469,6 +4919,17 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
       SCWrite(pCon, line, eValue);
       return 1;
     }
+    else if(strcasecmp("testing", argv[1]) == 0) {
+      if (argc > 2 && strcasecmp("on", argv[2]) == 0) {
+        self->testing = true;
+        SCWrite(pCon, "TESTING ON", eValue);
+      }
+      else {
+        self->testing = false;
+        SCWrite(pCon, "TESTING OFF", eValue);
+      }
+      return 1;
+    }
     else if(strcasecmp("trace", argv[1]) == 0) {
       if (argc > 2 && strcasecmp("on", argv[2]) == 0) {
         self->trace = SCCopyConnection(pCon);