Airpads, Debug, Motor Off Delay, remove "define position" (DP) command, changeing blockage detection.

r1535 | dcl | 2007-02-21 14:55:51 +1100 (Wed, 21 Feb 2007) | 2 lines
2007-02-21 14:55:51 +11:00
parent 2f1b481344
commit 3c672d622a
1 changed files with 404 additions and 19 deletions
--- a/site_ansto/motor_dmc2280.c
+++ b/site_ansto/motor_dmc2280.c
@ -19,9 +19,11 @@
 #include <assert.h>
 #include <string.h>
 #include <stdarg.h>
+#include <sys/time.h>
 #include <fortify.h>
 #include <sics.h>
 #include <rs232controller.h>
+#include <nwatch.h>
 #include <modriv.h>
 #include <motor.h>
 #include <dynstring.h>
@ -99,9 +101,24 @@ typedef struct __MoDriv {
  int cntsPerX; /**<  absolute encoder counts per physical unit */
  int motOffDelay; /**< number of msec to wait before switching motor off, default=0 */
  float lastPosition; /**< Position at last position check */
+  float lastSteps;
+  float lastCounts;
  struct timeval time_lastPos_set; /**< Time when lastPosition was set */
  float blockage_ckInterval; /**< Interval for checking blocked motors, seconds */
+  int has_airpads; /**< Flag = 1 if there is are airpads for this motor */
+  float fTarget;
+  int settle;
+  struct timeval time_settle_done;
+  int airpad_state;
+  int airpad_counter;
+  pNWTimer airpad_timer;
+  pNWTimer motor_timer;
+  int debug;
 } DMC2280Driv, *pDMC2280Driv;
+#define AIRPADS_DOWN     0
+#define AIRPADS_RAISE    1
+#define AIRPADS_UP       2
+#define AIRPADS_LOWER    3
 /*------------------- error codes ----------------------------------*/
 #define BADADR     -1 // NOT SET: Unknown host/port?
 #define BADBSY     -2
@ -142,10 +159,12 @@ typedef struct __MoDriv {
 #define MAXACCEL "maxaccel"
 #define DECEL "decel"
 #define MAXDECEL "maxdecel"
+#define MOTOFFDELAY "motoffdelay"
+#define AIRPADS "airpads"
+#define SETTLE "settle"
 #define BLOCKAGE_CHECK_INTERVAL "blockage_check_interval"

 static int DMC2280Halt(void *pData);
-static void set_lastPos(void *pData, float posn);
 static int DMC2280SetPar(void *pData, SConnection *pCon,
    char *name, float newValue);
 static int DMC2280Receive(pDMC2280Driv self, /*@out@*/ char *reply);
@ -242,6 +261,9 @@ static int DMC2280Send(pDMC2280Driv self, char *command) {
    /*@+mayaliasunique@*/
  }

+  if (self->debug)
+    SICSLogWrite(command, eStatus);
+
  /*@+matchanyintegral@ let size_t from strlen match int */
  status = writeRS232(self->controller, command, strlen(command));
  /*@-matchanyintegral@*/
@ -296,6 +318,8 @@ static int DMC2280Receive(pDMC2280Driv self, /*@out@*/char *reply) {
    status=readRS232TillTerm(self->controller, reply, &dataLen);
    switch (status) {
      case 1:
+        if (self->debug)
+          SICSLogWrite(reply, eStatus);
        return dataLen;
      case TIMEOUT:
        self->errorCode = status;
@ -309,6 +333,29 @@ static int DMC2280Receive(pDMC2280Driv self, /*@out@*/char *reply) {
  return FAILURE;
 }

+/** \brief Record the given posn and timestamp it.
+ *
+ * \param *pData provides access to a motor's data
+ * \param posn, the axis position which you want to remember.
+ * */
+static void set_lastMotion(pDMC2280Driv self, float steps, float counts) {
+  assert(self != NULL);
+  self->lastSteps = steps;
+  self->lastCounts = counts;
+  gettimeofday(&(self->time_lastPos_set), NULL);
+}
+
+/** \brief Record the given posn and timestamp it.
+ *
+ * \param *pData provides access to a motor's data
+ * \param posn, the axis position which you want to remember.
+ * */
+static void set_lastPos(pDMC2280Driv self, float posn) {
+  assert(self != NULL);
+  self->lastPosition = posn;
+  gettimeofday(&(self->time_lastPos_set), NULL);
+}
+
 /**\brief Convenience function for getting speed, acceleration
 * or deceleration
 * 
@ -373,6 +420,29 @@ static int getDMCSetting(void *pData, enum dmcsetting cmdIndex){
    }
 }

+/** \brief Reads motion.
+ *
+ * \param *steps motor steps
+ * \param *counts encoder counts
+ * \return
+ * SUCCESS
+ * FAILURE
+ */
+static int readMotion(pDMC2280Driv self, float *steps, float *counts) {
+  char reply[1024];
+  char cmd[CMDLEN];
+
+  snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c", self->axisLabel, self->axisLabel);
+  if (FAILURE == DMC2280Send(self, cmd)) 
+    return FAILURE; 
+  if (FAILURE == DMC2280Receive(self, reply)) 
+    return FAILURE;
+
+  if (2 != sscanf(reply, "%f %f", steps, counts))
+    return FAILURE;
+  return SUCCESS;
+}
+
 /** \brief Reads absolute encoder.
 *
 * \param *pos is the absolute encoder reading on SUCCESS.
@ -427,6 +497,86 @@ static int DMC2280GetPos(void *pData, float *fPos){
  }
  return OKOK;
 }
+
+static int DMC2280Run(void *pData,float fValue);
+
+static int airpad_callback(void* context, int mode) {
+  pDMC2280Driv self = (pDMC2280Driv) context;
+  char reply[1024];
+  float fReply;
+
+  self->airpad_timer = NULL;
+  if (FAILURE == DMC2280Send(self, "MG APDONE")) {
+    self->errorCode = BADCUSHION;
+    self->airpad_state = AIRPADS_DOWN;
+    return 0;
+  }
+  if (FAILURE == DMC2280Receive(self, reply)) {
+    self->errorCode = BADCUSHION;
+    self->airpad_state = AIRPADS_DOWN;
+    return 0;
+  }
+  fReply = (float) atof(reply);
+  if (self->airpad_state == AIRPADS_RAISE && fReply > 0) {
+    int iRet;
+    self->airpad_state = AIRPADS_UP;
+    iRet = DMC2280Run(self, self->fTarget);
+    if (iRet != OKOK)
+      self->errorCode = BADCUSHION;
+    return 0;
+  }
+  if (self->airpad_state == AIRPADS_LOWER && fReply == 0) {
+    self->airpad_state = AIRPADS_DOWN;
+    return 0;
+  }
+  if (self->airpad_counter <= 0) {
+    self->errorCode = BADCUSHION;
+    self->airpad_state = AIRPADS_DOWN;
+    return 0;
+  }
+  --self->airpad_counter;
+  NetWatchRegisterTimer(&self->airpad_timer, 1000,
+      airpad_callback, self);
+  return 0;
+}
+
+static int motor_callback(void* context, int mode) {
+  pDMC2280Driv self = (pDMC2280Driv) context;
+  char cmd[CMDLEN];
+
+  self->motor_timer = NULL;
+  if (self->has_airpads) {
+    if (FAILURE == DMC2280Send(self, "FTUBE=0")) {
+      self->errorCode = BADCUSHION;
+      return 0;
+    }
+    self->airpad_state = AIRPADS_LOWER;
+    self->airpad_counter = 10;
+    NetWatchRegisterTimer(&self->airpad_timer, 1000,
+        airpad_callback, self);
+    return 0;
+  }
+  snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
+  DMC2280Send(self, cmd);
+  return 0;
+}
+
+static int DMC2280RunAir(void *pData, float fValue) {
+  pDMC2280Driv self = (pDMC2280Driv)pData;
+
+  self->fTarget = fValue;
+  if (self->airpad_timer)
+    NetWatchRemoveTimer(self->airpad_timer);
+  self->airpad_timer = NULL;
+  if (FAILURE == DMC2280Send(self, "FTUBE=1"))
+    return HWFault;
+  self->airpad_state = AIRPADS_RAISE;
+  self->airpad_counter = 10;
+  NetWatchRegisterTimer(&self->airpad_timer, 1000,
+      airpad_callback, self);
+  return OKOK;
+}
+
 /** \brief DMC2280 implementation of the RunTo
 * method in the MotorDriver interface.
 *
@ -442,54 +592,132 @@ static int DMC2280Run(void *pData,float fValue){
  char axis;
  char cmd[CMDLEN], SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN];
  int absEncHome, stepsPerX, motorHome, cntsPerX, newAbsPosn;
-  float target, currPos;
+  float target;
  self = (pDMC2280Driv)pData;
  assert(self != NULL);
-  DMC2280GetPos(pData, &currPos);
-  set_lastPos(pData, currPos);
+
+  if (self->motor_timer)
+    NetWatchRemoveTimer(self->motor_timer);
+  self->motor_timer = NULL;
+
+  if (self->has_airpads)
+    if (self->airpad_state != AIRPADS_UP)
+      return DMC2280RunAir(self, fValue);
+
+  if (self->settle)
+    self->time_settle_done.tv_sec = 0;
+
+  do {
+#if 1
+    float steps, counts;
+    if (FAILURE == readMotion(self, &steps, &counts))
+      return HWFault;
+    set_lastMotion(pData, steps, counts);
+#else
+    float currPos;
+    DMC2280GetPos(pData, &currPos);
+    set_lastPos(pData, currPos);
+#endif
+  } while (0);
  axis=self->axisLabel;
  motorHome = self->motorHome;
  stepsPerX=self->stepsPerX;
  snprintf(SHx, CMDLEN, "SH%c", axis);
  snprintf(BGx, CMDLEN, "BG%c", axis);
  target = fValue - self->home;
-  newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5);
-  snprintf(absPosCmd, CMDLEN, "PA%c=%d",axis, newAbsPosn);

  if (1 == self->abs_endcoder) {
-    /* Ensure that the defined motor position matches actual position */
    absEncHome = self->absEncHome;
    cntsPerX = self->cntsPerX;
-    snprintf(cmd, CMDLEN, "DP%c=(_TP%c - %d)*(%d/%d) + %d",axis,axis,absEncHome,stepsPerX,cntsPerX,motorHome);
-    if (FAILURE == DMC2280Send(self, cmd)) 
-      return HWFault;
+    /* PAF=-((absEncHome-_TPF)/-cntsPerX + target)*stepsPerX + _TDF */
+    snprintf(absPosCmd, CMDLEN,
+        "PA%c=(((%d-_TP%c)/%d)+%f)*%d + _TD%c",
+        axis,
+        absEncHome,
+        axis,
+        cntsPerX,
+        target,
+        stepsPerX,
+        axis);
 #ifdef BACKLASHFIX
    snprintf(cmd, CMDLEN, "%cQTARGET=%d", axis, (int) (target * cntsPerX + absEncHome + 0.5));
    if (FAILURE == DMC2280Send(self, cmd))
      return HWFault;
 #endif
+  } else {
+    newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5);
+    snprintf(absPosCmd, CMDLEN, "PA%c=%d",axis, newAbsPosn);
  }

-  if (FAILURE == DMC2280Send(self, absPosCmd))
-      return HWFault;
  if (FAILURE == DMC2280Send(self, SHx))
    return HWFault;
+  if (FAILURE == DMC2280Send(self, absPosCmd))
+      return HWFault;
  if (FAILURE == DMC2280Send(self, BGx))
    return HWFault;
  return OKOK;
 }

-/** \brief Record the given posn and timestamp it.
+/** \brief Check if the axis has moved significantly since
+ * the last check.
+ *
+ * The motion is checked against the expected at intervals of
+ * pDMC2280Driv->blockage_ckInterval. 
 *
 * \param *pData provides access to a motor's data
- * \param posn, the axis position which you want to remember.
- * */
-static void set_lastPos(void *pData, float posn) {
+ * \return
+ * - 1 MOTOR OK, position has changed significantly during move
+ * - 0 MOTOR BLOCKED, no significant change in position detected. 
+ */
+static int checkMotion(void *pData) {
+#if 1
+  float precision, steps, counts, ratio_obs, ratio_exp;
+  long int usec_TimeDiff;
+  struct timeval now;
+
  pDMC2280Driv self;
  self = (pDMC2280Driv)pData;
  assert(self != NULL);
-  self->lastPosition = posn;
-  gettimeofday(&(self->time_lastPos_set), NULL);
+  gettimeofday(&now, NULL);
+  usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec;
+  usec_TimeDiff *= 1000000;
+  usec_TimeDiff += now.tv_usec;
+  usec_TimeDiff -= self->time_lastPos_set.tv_usec;
+  if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval))
+    return 1;
+  if (self->pMot == NULL)
+    self->pMot = FindMotor(pServ->pSics, self->name);
+  MotorGetPar(self->pMot,"precision",&precision);
+  if (FAILURE == readMotion(self, &steps, &counts))
+    return 0;
+  /* If not stepping, thern not blocked */
+  if (steps == self->lastSteps)
+    return 1;
+  /* calculate observed and expected steps per count ratios */
+  ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts);
+  ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX;
+  /* less than half or more than double is trouble */
+  if (ratio_obs / ratio_exp < 0.5 || ratio_obs / ratio_exp > 2.0) {
+    char msg[132];
+    snprintf(msg, sizeof(msg), "Motion check fail: obs=%f, exp=%f",
+        ratio_obs, ratio_exp);
+    SICSLogWrite(msg, eError);
+    snprintf(msg, sizeof(msg), "steps=%f-%f, counts=%f-%f, exp=%d/%d",
+        steps, self->lastSteps, counts, self->lastCounts,
+        self->stepsPerX, self->cntsPerX);
+    SICSLogWrite(msg, eError);
+#if 1
+    set_lastMotion(pData, steps, counts);
+    return 1;
+#else
+    return 0;
+#endif
+  } else {
+    set_lastMotion(pData, steps, counts);
+    return 1;
+  }
+#endif
+  return 1;
 }

 /** \brief Check if the axis position has changed significantly since
@ -552,6 +780,14 @@ static int DMC2280Status(void *pData){

  self = (pDMC2280Driv)pData;
  assert(self != NULL);
+
+  /*
+   * If we are waiting for the motor or airpads then we
+   * are busy
+   */
+  if (self->motor_timer || self->airpad_timer)
+    return HWBusy;
+
  /* Make sure that speed, accel and decel are set correctly */
  /* ckSpeedAccelDecel(self); */
  /* Get status of switches
@ -572,9 +808,15 @@ static int DMC2280Status(void *pData){
    return HWFault;
  }
  if (moving) {
+    int iRet;
    /* If pos hasn't changed since last 
     * check then stop and scream */
-    if (checkPosition(pData) == 0) {
+#if 0
+    iRet = checkPosition(pData);
+#else
+    iRet = checkMotion(pData);
+#endif
+    if (iRet == 0) {
      DMC2280Halt(pData);
      self->errorCode = BLOCKED;
      return HWFault;
@ -623,9 +865,58 @@ static int DMC2280Status(void *pData){
      }
    }
 #endif
+    /*
+     * When we get here, the motion has completed and we
+     * must determine when and how to shut off the motor
+     */
+    if (self->settle) {
+      struct timeval *then = &self->time_settle_done;
+      struct timeval now;
+      gettimeofday(&now, NULL);
+      if (then->tv_sec == 0 ||
+          (then->tv_sec - now.tv_sec) > self->settle) {
+        gettimeofday(then, NULL);
+        then->tv_sec += self->settle;
+        return HWBusy;
+      } else {
+        if ((now.tv_sec > then->tv_sec) ||
+            ((now.tv_sec == then->tv_sec) &&
+            (now.tv_usec >= then->tv_usec))) {
+          /* it's finished, fall through */
+        } else {
+          return HWBusy;
+        }
+      }
+    }
+    if (self->has_airpads) {
+      if (self->airpad_state == AIRPADS_DOWN)
+        return HWIdle;
+      if (self->airpad_state == AIRPADS_LOWER)
+        return HWBusy;
+      if (self->motOffDelay > 0 ) {
+        NetWatchRegisterTimer(&self->motor_timer,
+            self->motOffDelay,
+            motor_callback, self);
+        return HWIdle;
+      }
+      if (FAILURE == DMC2280Send(self, "FTUBE=0"))
+        return HWFault;
+      self->airpad_state = AIRPADS_LOWER;
+      self->airpad_counter = 10;
+      NetWatchRegisterTimer(&self->airpad_timer, 1000,
+          airpad_callback, self);
+      return HWIdle;
+    }
    if (self->noPowerSave == _SAVEPOWER) {
      if (self->motOffDelay > 0 ) {
+#if 0
        snprintf(cmd, CMDLEN, "AT %d; MO%c", self->motOffDelay, self->axisLabel);
+#else
+      NetWatchRegisterTimer(&self->motor_timer,
+          self->motOffDelay,
+          motor_callback, self);
+      return HWIdle;
+#endif
      } else {
        snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
      }
@ -846,6 +1137,22 @@ static int DMC2280GetPar(void *pData, char *name,
    *fValue = self->maxDecel;
    return 1;
  }
+  if(strcmp(name,MOTOFFDELAY) == 0) {
+    *fValue = self->motOffDelay;
+    return 1;
+  }
+  if(strcmp(name,"debug") == 0) {
+    *fValue = self->debug;
+    return 1;
+  }
+  if(strcmp(name,SETTLE) == 0) {
+    *fValue = self->settle;
+    return 1;
+  }
+  if(strcmp(name,AIRPADS) == 0) {
+    *fValue = self->has_airpads;
+    return 1;
+  }
  if(strcmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
    *fValue = self->blockage_ckInterval;
    return 1;
@ -913,6 +1220,46 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
    return 1;
  }

+  /* Set motor off delay, managers only */
+  if(strcmp(name,MOTOFFDELAY) == 0) {
+    if(!SCMatchRights(pCon,usMugger))
+      return 1;
+    else {
+      self->motOffDelay = newValue;
+      return 1;
+    }
+  }
+
+  /* Debug, managers only */
+  if(strcmp(name,"debug") == 0) {
+    if(!SCMatchRights(pCon,usMugger))
+      return 1;
+    else {
+      self->debug = newValue;
+      return 1;
+    }
+  }
+
+  /* Setttle, managers only */
+  if(strcmp(name,SETTLE) == 0) {
+    if(!SCMatchRights(pCon,usMugger))
+      return 1;
+    else {
+      self->settle = newValue;
+      return 1;
+    }
+  }
+
+  /* Set airpads, managers only */
+  if(strcmp(name,AIRPADS) == 0) {
+    if(!SCMatchRights(pCon,usMugger))
+      return 1;
+    else {
+      self->has_airpads = newValue;
+      return 1;
+    }
+  }
+
  /* Set interval between blocked motor checks,
   * managers only */
  if(strcmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
@ -1009,6 +1356,22 @@ static void DMC2280List(void *self, char *name, SConnection *pCon){
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, ((pDMC2280Driv)self)->maxDecel);
  SCWrite(pCon, buffer, eStatus);
+  snprintf(buffer, BUFFLEN, "%s.motOffDelay = %d\n", name, ((pDMC2280Driv)self)->motOffDelay);
+  SCWrite(pCon, buffer, eStatus);
+  snprintf(buffer, BUFFLEN, "%s.Debug = %d\n", name, ((pDMC2280Driv)self)->debug);
+  SCWrite(pCon, buffer, eStatus);
+  snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, ((pDMC2280Driv)self)->settle);
+  SCWrite(pCon, buffer, eStatus);
+  snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, ((pDMC2280Driv)self)->has_airpads);
+  SCWrite(pCon, buffer, eStatus);
+  snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, ((pDMC2280Driv)self)->abs_endcoder);
+  SCWrite(pCon, buffer, eStatus);
+  if (((pDMC2280Driv)self)->abs_endcoder) {
+    snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, ((pDMC2280Driv)self)->absEncHome);
+    SCWrite(pCon, buffer, eStatus);
+    snprintf(buffer, BUFFLEN, "%s.cntsPerX = %d\n", name, ((pDMC2280Driv)self)->cntsPerX);
+    SCWrite(pCon, buffer, eStatus);
+  }
  return;
 }
 /** \brief Free memory if motor is removed
@ -1104,6 +1467,7 @@ static void KillDMC2280(/*@only@*/void *pData){
                   eError);
    return NULL;
  }
+  memset(pNew, 0, sizeof(DMC2280Driv));
  pNew->controller = NULL;
  pNew->name = NULL;
  pNew->errorCode = 0;
@ -1216,6 +1580,27 @@ static void KillDMC2280(/*@only@*/void *pData){
  else
    sscanf(pPtr,"%d",&(pNew->motOffDelay));

+  /* Debug: this motor driver logs exchanges */
+  if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL)
+    pNew->debug=0;
+  else {
+    sscanf(pPtr,"%d",&(pNew->debug));
+  }
+
+  /* SETTLE: this motor need time to settle */
+  if ((pPtr=getParam(pCon, interp, params,"settle",_OPTIONAL)) == NULL)
+    pNew->settle=0;
+  else {
+    sscanf(pPtr,"%d",&(pNew->settle));
+  }
+
+  /* AIRPADS: this motor need airpads */
+  if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL)
+    pNew->has_airpads=0;
+  else {
+    sscanf(pPtr,"%d",&(pNew->has_airpads));
+  }
+
  /* ABSENC: If the parameter requires an abs enc add it to the else block */
  if ((pPtr=getParam(pCon, interp, params,"absenc",_OPTIONAL)) == NULL)
    pNew->abs_endcoder=0;