Impliment motor "oscillate" and "status" commands and prefix "driver run"

site_ansto/motor_dmc2280.c

@@ -271,6 +271,13 @@ struct __MoDriv {
   double S_r;
   bool doTracking;
   TrackEntryBuffer *trackHead, *trackTail;
+  bool doOscillate; /**< flag for motor oscillation */
+  bool oscillate_up; /**< next oscillation move is to high position */
+  bool oscillate_init; /**< initial move is not counted */
+  int oscillate_count; /**< number of moves to make or -1 for continuous */
+  int oscillate_counter; /**< number of moves made - half-oscillations */
+  double oscillate_low; /**< soft position at low end */
+  double oscillate_high; /**< soft position at high end */
 };
 
 int DMC2280MotionControl = 1; /* defaults to enabled */
@@ -361,6 +368,64 @@ static double getMotorParam(pDMC2280Driv self, char *param) {
   return fParam;
 }
 
+static bool parseNumber(const char *str, double *pNumber) {
+  char *endPtr;
+  double fNumber;
+  if (!pNumber)
+    return false;
+  fNumber = strtod(str, &endPtr);
+  if (endPtr == str || errno == ERANGE)
+    return false;
+  /* Reject Infinity and Not a Number (NaN) */
+  switch (fpclassify(fNumber)) {
+    case FP_ZERO:
+    case FP_NORMAL:
+      break;
+    default:
+      return false;
+  }
+  *pNumber = fNumber;
+  return true;
+}
+
+static bool getHardFromSoft(pDMC2280Driv self, SConnection *pCon, double fSoft, double *pHard) {
+  float fLower, fUpper, fZero, fSign, fLim;
+  double fHard;
+
+  MotorGetPar(self->pMot, "softlowerlim", &fLower);
+  MotorGetPar(self->pMot, "softupperlim", &fUpper);
+  MotorGetPar(self->pMot, "softzero", &fZero);
+  MotorGetPar(self->pMot, "sign", &fSign);
+  if (fSoft > fUpper) {
+    if (pCon)
+      SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s",
+          fSoft, fUpper, self->name);
+    return false;
+  }
+  if (fSoft < fLower) {
+    if (pCon)
+      SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s",
+          fSoft, fLower, self->name);
+    return false;
+  }
+  fHard = fSoft;
+  fHard += fZero;
+  fHard *= fSign;
+  if (fHard > self->fUpper) {
+    if (pCon)
+      SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s",
+          fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name);
+    return false;
+  } else if (fHard < self->fLower) {
+    if (pCon)
+      SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s",
+          fSoft, self->fLower * fSign + fZero, self->fLower, self->name);
+    return false;
+  }
+  *pHard = fHard;
+  return true;
+}
+
 static void TrackingRelease(pDMC2280Driv self) {
   while (self->trackHead) {
     TrackEntryBuffer *victim;
@@ -1745,6 +1810,7 @@ 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_Oscillate(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_BacklashStart(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_BacklashCont(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_CreepStart(pDMC2280Driv self, pEvtEvent event);
@@ -1763,6 +1829,7 @@ 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_Oscillate) return "DMCState_Oscillate";
   if (func == DMCState_BacklashStart) return "DMCState_BacklashStart";
   if (func == DMCState_BacklashCont) return "DMCState_BacklashCont";
   if (func == DMCState_CreepStart) return "DMCState_CreepStart";
@@ -2523,6 +2590,10 @@ static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
             if (self->trackHead)
               TrackingRelease(self);
             self->myNextState = NULL;
+            if (self->doOscillate) {
+              change_state(self, DMCState_Oscillate);
+              return;
+            }
             if (self->backlash_offset != 0) {
               change_state(self, DMCState_BacklashStart);
               return;
@@ -3072,6 +3143,87 @@ static void DMCState_CreepCont(pDMC2280Driv self, pEvtEvent event) {
   return;
 }
 
+/*
+ * This state oscillates the motor between two positions
+ */
+static void DMCState_Oscillate(pDMC2280Driv self, pEvtEvent event) {
+  double target;
+  int absolute;
+
+  switch (event->event_type) {
+    case eStateEvent:
+      self->myNextState = NULL;
+      /* handle termination conditions (e.g. count) */
+      if (self->oscillate_count > 0 && self->oscillate_counter >= self->oscillate_count)
+        self->doOscillate = false;
+      if (!self->doOscillate) {
+        change_state(self, DMCState_OffTimer); /* TODO: check this is OK */
+        return;
+      }
+      if (self->oscillate_up) {
+        double fHard;
+        if (getHardFromSoft(self, NULL, self->oscillate_high, &fHard))
+          self->fTarget = fHard;
+        self->oscillate_up = false;
+      } else {
+        double fHard;
+        if (getHardFromSoft(self, NULL, self->oscillate_low, &fHard))
+          self->fTarget = fHard;
+        self->oscillate_up = true;
+      }
+      /* TODO: set timer for optional pause */
+      /* Note: fallthrough is intentional */
+    case eTimerEvent:
+      target = self->fTarget;
+      self->preseek = 0;
+      absolute = motCreep(self, target);
+      self->doSettle = false;
+      cmdPosition(self, absolute);
+      return;
+    case eMessageEvent:
+      if (self->run_flag != 0) {
+        change_state(self, DMCState_MotorHalt);
+        return;
+      }
+      /* set the next state after this move */
+      self->myNextState = DMCState_Oscillate;
+      if (!self->oscillate_init)
+        self->oscillate_counter++;
+      self->oscillate_init = false;
+      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 the current destination
  * set by PA (or PR) by the caller in myNextState.
@@ -3819,6 +3971,7 @@ static int DMC2280Run(void *pData,float fValue){
   /* Only do it if it is within our hard limits */
   if (fValue >= self->fLower && fValue <= self->fUpper) {
     self->fTarget = fValue;
+    self->doOscillate = false;
     state_cmd_execute(self, CMD_RUN);
     return 1;
   }
@@ -5274,16 +5427,101 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
       }
       return 1;
     }
-    else if(strcasecmp("run", argv[1]) == 0) {
+    else if(strcasecmp("oscillate_count", argv[1]) == 0) {
+      if (argc > 2)
+        self->oscillate_count = atoi(argv[2]);
+      else
+        SCPrintf(pCon, eValue, "%s.oscillate_count = %d", self->name, self->oscillate_count);
+      return 1;
+    }
+    else if (strcasecmp("oscillate_high", argv[1]) == 0) {
+      double fSoft = 0.0;
+      double fHard = 0.0;
+      if (argc > 2) {
+        if (parseNumber(argv[2], &fSoft) &&
+            getHardFromSoft(self, pCon, fSoft, &fHard))
+          self->oscillate_high = fSoft;
+      }
+      else
+        SCPrintf(pCon, eValue, "%s.oscillate_high = %g", self->name, self->oscillate_high);
+      return 1;
+    }
+    else if (strcasecmp("oscillate_low", argv[1]) == 0) {
+      double fSoft = 0.0;
+      double fHard = 0.0;
+      if (argc > 2) {
+        if (parseNumber(argv[2], &fSoft) &&
+            getHardFromSoft(self, pCon, fSoft, &fHard))
+          self->oscillate_low = fSoft;
+      }
+      else
+        SCPrintf(pCon, eValue, "%s.oscillate_low = %g", self->name, self->oscillate_low);
+      return 1;
+    }
+    else if (strcasecmp("oscillate_counter", argv[1]) == 0) {
+      SCPrintf(pCon, eValue, "%s.oscillate_counter = %d", self->name, self->oscillate_counter);
+    }
+    else if(strcasecmp("osc", argv[1]) == 0 || strcasecmp("oscillate", argv[1]) == 0) {
+      if (argc > 2) {
+        if (strcasecmp("list", argv[2]) == 0) {
+          /* TODO: list the osc params */
+          SCPrintf(pCon, eValue, "%s.oscillate_count = %d", self->name, self->oscillate_count);
+          SCPrintf(pCon, eValue, "%s.oscillate_counter = %d", self->name, self->oscillate_counter);
+          SCPrintf(pCon, eValue, "%s.oscillate_low = %g", self->name, self->oscillate_low);
+          SCPrintf(pCon, eValue, "%s.oscillate_high = %g", self->name, self->oscillate_high);
+          return 1;
+        }
+        if (strcasecmp("start", argv[2]) == 0) {
+          float fFixed;
+          double fHard;
+          MotorGetPar(self->pMot, "fixed", &fFixed);
+          if (fFixed >= 0) {
+            SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name);
+            return 1;
+          }
+          if (!getHardFromSoft(self, pCon, self->oscillate_low, &fHard)) {
+            SCPrintf(pCon, eWarning, "Oscillate low point is in error");
+            return 1;
+          }
+          if (!getHardFromSoft(self, pCon, self->oscillate_high, &fHard)) {
+            SCPrintf(pCon, eWarning, "Oscillate high point is in error");
+            return 1;
+          }
+          if (fabs(self->oscillate_high - motPosit(self)) < fabs(self->oscillate_low - motPosit(self)))
+            self->oscillate_up = true;
+          else
+            self->oscillate_up = false;
+          self->doOscillate = true;
+          self->oscillate_init = true;
+          self->oscillate_counter = 0;
+          state_cmd_execute(self, CMD_RUN);
+          return 1;
+        }
+        if (strcasecmp("stop", argv[2]) == 0) {
+          self->doOscillate = false;
+          return 1;
+        }
+        if (strcasecmp("halt", argv[2]) == 0) {
+          self->doOscillate = false;
+          state_cmd_execute(self, CMD_HALT);
+          return 1;
+        }
+      } else {
+        SCPrintf(pCon, eStatus, "%s.oscillation = %s", self->name, self->doOscillate ? "on" : "off");
+        return 1;
+      }
+    }
+    else if(strcasecmp("driver", argv[1]) == 0) {
+      if(strcasecmp("run", argv[2]) == 0) {
         if (argc > 2) {
           float fLower, fUpper, fZero, fSign, fFixed, fLim;
           double fSoft;
           char *endPtr;
           errno = 0;
-        fSoft = strtod(argv[2], &endPtr);
+          fSoft = strtod(argv[3], &endPtr);
-        if (endPtr == argv[2] || errno == ERANGE) {
+          if (endPtr == argv[3] || errno == ERANGE) {
             self->errorCode = BADPAR;
-          SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[2]);
+            SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]);
             return 1;
           }
           /* Reject Infinity and Not a Number (NaN) */
@@ -5293,7 +5531,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
               break;
             default:
               self->errorCode = BADPAR;
-            SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[2]);
+              SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]);
               return 1;
           }
           MotorGetPar(self->pMot, "softlowerlim", &fLower);
@@ -5322,6 +5560,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
             } else {
               self->fTarget = fHard;
               SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard);
+              self->doOscillate = false;
               state_cmd_execute(self, CMD_RUN);
             }
           }
@@ -5356,6 +5595,36 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
         }
         return 1;
       }
+    }
+    else if(strcasecmp("status", argv[1]) == 0) {
+      char *txt = NULL;
+      if (OKOK == self->driver_status)
+        txt = "OKOK";
+      else if (HWIdle == self->driver_status)
+        txt = "HWIdle";
+      else if (HWBusy == self->driver_status)
+        txt = "HWBusy";
+      else if (HWFault == self->driver_status)
+        txt = "HWFault";
+      else if (HWPosFault == self->driver_status)
+        txt = "HWPosFault";
+      else if (HWCrash == self->driver_status)
+        txt = "HWCrash";
+      else if (NOMEMORY == self->driver_status)
+        txt = "NOMEMORY";
+      else if (HWNoBeam == self->driver_status)
+        txt = "HWNoBeam";
+      else if (HWPause == self->driver_status)
+        txt = "HWPause";
+      else if (HWWarn == self->driver_status)
+        txt = "HWWarn";
+      else if (HWRedo == self->driver_status)
+        txt = "HWRedo";
+      else
+        txt = "HWUnknown";
+      SCPrintf(pCon, eValue, "%s.status = %s", self->name, txt);
+      return 1;
+    }
     else if(strcasecmp("halt", argv[1]) == 0) {
       state_cmd_execute(self, CMD_HALT);
       return 1;