Modified during MK's visit.

r1462 | ffr | 2007-02-12 12:06:19 +1100 (Mon, 12 Feb 2007) | 2 lines

site_ansto/Makefile

@@ -16,8 +16,7 @@ default: all
 EXTRA=nintf.o
 PSI_CLEAN_MATRIX = rm -f ../*.o; $(MAKE) -C ../matrix $(MFLAGS) clean
 PSI_CFLAGS = -I./ -I$(HDFROOT)/include -DHDF5 -DNXXML\
-       -fwritable-strings -DCYGNUS -DNONINTF -g $(DFORTIFY)\
+       -fwritable-strings -DCYGNUS -DNONINTF -g $(DFORTIFY)
-	   -I /usr/local/include/json/
 PSI_SLIBS = matrix/libmatrix.a
 PSI_LIBS =  -L$(HDFROOT)/lib $(NILIB)\
        -ltcl8.4 $(HDFROOT)/lib/libhdf5.a  \
@@ -76,8 +75,12 @@ OBJ= site_ansto.o anstoutil.o\
         anstohttp.o \
         hmcontrol_ansto.o
 
-all: ../matrix/libmatrix.a $(COREOBJ:%=../%) libansto.a libhardsup
+all: ../matrix/libmatrix.a $(COREOBJ:%=../%) $(EXTRA:%=../%) libansto.a libhardsup
-	$(CC) -g -o SICServer  $(COREOBJ:%=../%) $(SUBLIBS) $(PSI_SLIBS:%=../%) $(PSI_LIBS) $(GHTTP_LIBS)
+	$(CC) -g -o SICServer  $(COREOBJ:%=../%) $(EXTRA:%=../%) $(SUBLIBS) $(PSI_SLIBS:%=../%) $(PSI_LIBS) $(GHTTP_LIBS)
+
+#TODO Add targets for other instruments
+echidna: all
+	make -C instrument/hrpd
 
 libansto.a: $(OBJ)
 	rm -f libansto.a

site_ansto/make_gen_variables

@@ -17,15 +17,16 @@ SOBJ =  network.o ifile.o conman.o SCinter.o splitter.o passwd.o \
        simchop.o choco.o chadapter.o  trim.o scaldate.o tasub.o\
        hklscan.o xytable.o   exebuf.o exeman.o ubfour.o ubcalc.o\
        circular.o  maximize.o sicscron.o  scanvar.o tasublib.o\
-       d_sign.o d_mod.o tcldrivable.o stdscan.o diffscan.o nxxml.o \
+       d_sign.o d_mod.o tcldrivable.o stdscan.o diffscan.o nxxml.o\
        synchronize.o definealias.o  oscillate.o tasdrive.o \
        hmcontrol.o userscan.o  rs232controller.o lomax.o tasscanub.o \
        fourlib.o motreg.o motreglist.o anticollider.o nxdataset.o \
        s_rnge.o sig_die.o gpibcontroller.o $(NIOBJ) mcreader.o mccontrol.o\
        hmdata.o  nxscript.o  tclintimpl.o sicsdata.o mcstascounter.o \
        mcstashm.o initializer.o remob.o tclmotdriv.o protocol.o \
-       sinfox.o sicslist.o cone.o hipadaba.o sicshipadaba.o statistics.o\
+       sinfox.o sicslist.o cone.o hipadaba.o sicshipadaba.o statistics.o \
-	   $(EXTRA) 
+       moregress.o hdbcommand.o multicounter.o regresscter.o histregress.o \
+       sicshdbadapter.o polldriv.o sicspoll.o statemon.o
  
 MOTOROBJ = motor.o  simdriv.o  
 COUNTEROBJ =  countdriv.o simcter.o counter.o

site_ansto/motor_dmc2280.c

@@ -8,9 +8,8 @@
  * Ferdi Franceschini November 2005
  *
  * TODO
- * - check for ESTOP flag on controller.
+ * - check for motors enabled on plc
- *   - ESTOP=1  means emergency stop, motion on all axes must stop.
+ * - Check error bit, see Dan's email
- *   - ESTOP=0  means all OK. 
  */
 #include <stdlib.h>
 /* ISO C Standard:  7.16  Boolean type and values  <stdbool.h> */
@@ -26,6 +25,7 @@
 #include <modriv.h>
 #include <motor.h>
 #include <dynstring.h>
+#include <time.h>
 #include "anstoutil.h"
 
 /*
@@ -61,9 +61,9 @@ typedef struct __MoDriv {
   /* general motor driver interface 
      fields. _REQUIRED!
      */
-  float fUpper; /* upper limit */
+  float fUpper; /**< hard upper limit */
-  float fLower; /* lower limit */
+  float fLower; /**< hard lower limit */
-  char *name;
+  char *name; /**< motor name */
   int (*GetPosition)(void *self, float *fPos);
   int   (*RunTo)(void *self, float fNewVal);
   int   (*GetStatus)(void *self);
@@ -102,6 +102,9 @@ typedef struct __MoDriv {
   int absEncHome; /**<  Home position in counts for abs enc */
   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 */
+  struct timeval time_lastPos_set; /**< Time when lastPosition was set */
+  float blockage_ckInterval; /**< Interval for checking blocked motors, seconds */
 } DMC2280Driv, *pDMC2280Driv;
 /*------------------- error codes ----------------------------------*/
 #define BADADR     -1 // NOT SET: Unknown host/port?
@@ -118,6 +121,7 @@ typedef struct __MoDriv {
 #define ERRORLIM   -13
 #define IMPOSSIBLE_LIM_SW -14
 #define BGFAIL     -15 // NOT SET
+#define BLOCKED    -16
 /*--------------------------------------------------------------------*/
 #define STATUSMOVING 128 /* Motor is moving */
 #define STATUSERRORLIMIT 64 /* Number of errorss exceed limit */
@@ -142,7 +146,10 @@ typedef struct __MoDriv {
 #define MAXACCEL "maxaccel"
 #define DECEL "decel"
 #define MAXDECEL "maxdecel"
+#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);
@@ -194,7 +201,7 @@ static int motDecel(pDMC2280Driv self, float axisDecel) {
  * \see SUCCESS FAILURE
  */
 static int DMC2280ReadChar(pDMC2280Driv self, /*@out@*/char *reply) {
-  int i, status, retries=20, dataLen=1;
+  int i, status, retries=1, dataLen=1;
   reply[0] = '\0';
   for (i=0; i<retries; i++) {
     status=readRS232(self->controller, reply, &dataLen);
@@ -287,7 +294,7 @@ static int DMC2280Send(pDMC2280Driv self, char *command) {
  * \see SUCCESS FAILURE
  */
 static int DMC2280Receive(pDMC2280Driv self, /*@out@*/char *reply) {
-  int i, status, retries=20, dataLen=255;
+  int i, status, retries=1, dataLen=255;
   reply[0] = '\0';
   for (i=0; i<retries; i++) {
     status=readRS232TillTerm(self->controller, reply, &dataLen);
@@ -370,6 +377,26 @@ static int getDMCSetting(void *pData, enum dmcsetting cmdIndex){
     }
 }
 
+/** \brief Reads absolute encoder.
+ *
+ * \param *pos is the absolute encoder reading on SUCCESS.
+ * \return
+ * SUCCESS
+ * FAILURE
+ */
+static int readAbsEnc(pDMC2280Driv self, float *pos) {
+  char reply[1024];
+  char cmd[CMDLEN];
+
+  snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
+  if (FAILURE == DMC2280Send(self, cmd)) 
+    return FAILURE; 
+  if (FAILURE == DMC2280Receive(self, reply)) 
+    return FAILURE;
+
+  *pos = (float) atoi(reply);
+  return SUCCESS;
+}
 
 /** \brief Reads motor position, implements the GetPosition
  * method in the MotorDriver interface.
@@ -390,12 +417,8 @@ static int DMC2280GetPos(void *pData, float *fPos){
   self = (pDMC2280Driv)pData;
   assert(self != NULL);
   if (1 == self->abs_endcoder) {
-    snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
+    if (readAbsEnc(self, &absEncPos) == FAILURE)
-    if (FAILURE == DMC2280Send(self, cmd))
       return HWFault;
-    if (FAILURE == DMC2280Receive(self, reply))
-      return HWFault;
-    absEncPos =(float)atof(reply);
     *fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home;
   } else {
     snprintf(cmd, ERRLEN, "TD%c", self->axisLabel);
@@ -423,10 +446,11 @@ 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;
+  float target, currPos;
-
   self = (pDMC2280Driv)pData;
   assert(self != NULL);
+  DMC2280GetPos(pData, &currPos);
+  set_lastPos(pData, currPos);
   axis=self->axisLabel;
   motorHome = self->motorHome;
   stepsPerX=self->stepsPerX;
@@ -459,6 +483,53 @@ static int DMC2280Run(void *pData,float fValue){
   return OKOK;
 }
 
+/** \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(void *pData, float posn) {
+  pDMC2280Driv self;
+  self = (pDMC2280Driv)pData;
+  assert(self != NULL);
+  self->lastPosition = posn;
+  gettimeofday(&(self->time_lastPos_set), NULL);
+}
+
+/** \brief Check if the axis position has changed significantly since
+ * the last check.
+ *
+ * The position change is checked against the 'precision' at intervals of
+ * pDMC2280Driv->blockage_ckInterval. 
+ *
+ * \param *pData provides access to a motor's data
+ * \return
+ * - 1 MOTOR OK, position has changed significantly during move
+ * - 0 MOTOR BLOCKED, no significant change in position detected. 
+ */
+static int checkPosition(void *pData) {
+  float precision, currPos;
+  long int usec_TimeDiff;
+  struct timeval now;
+
+  pDMC2280Driv self;
+  self = (pDMC2280Driv)pData;
+  assert(self != NULL);
+  gettimeofday(&now, NULL);
+  usec_TimeDiff = (now.tv_sec - (self->time_lastPos_set).tv_sec)*1e6 + (now.tv_usec - (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);
+  DMC2280GetPos(pData, &currPos);
+  if ( precision - fabs(self->lastPosition - currPos) >= FLT_EPSILON) {
+    return 0;
+  } else {
+    set_lastPos(pData, currPos);
+    return 1;
+  }
+}
 
 /** \brief Returns the motor status while it's moving,
  * implements the GetStatus method in the MotorDriver interface.
@@ -505,8 +576,16 @@ static int DMC2280Status(void *pData){
     return HWFault;
   }
   if (moving) {
+    /* If pos hasn't changed since last 
+     * check then stop and scream */
+    if (checkPosition(pData) == 0) {
+      DMC2280Halt(pData);
+      self->errorCode = BLOCKED;
+      return HWFault;
+    } else {
       self->errorCode = BADBSY;
       return HWBusy;
+    }
   } else {
     /* If motor stopped check limits and error status */
     if (fwd_limit_active) {
@@ -632,6 +711,9 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
     case IMPOSSIBLE_LIM_SW:
       strncpy(error,"Both limit switches seem active, maybe the polarity is set 'active low'. You should configure the controller with CN 1,-1,-1,0", (size_t)errLen);
       break;
+    case BLOCKED:
+      strncpy(error,"STOPPING MOTOR, MOTION SEEMS TO BE BLOCKED", (size_t)errLen);
+      break;
     default:
       /* FIXME What's the default */
       break;
@@ -661,6 +743,7 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
     case BADCMD:
       //case TIMEOUT:
     case BADPAR:
+    case BLOCKED:
       return MOTFAIL;
     case POSFAULT:
     case BADSEND:  
@@ -671,8 +754,11 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
     case NOTCONNECTED:
       initRS232(self->controller);
       return MOTREDO;
-  }
+      break;
+    default:
       return MOTFAIL;
+      break;
+  }
 }
 /** \brief Emergency halt. Implements the Halt
  * method in the MotorDriver interface.
@@ -692,6 +778,9 @@ static int DMC2280Halt(void *pData){
   assert(self != NULL);
   /* Stop motor */
   snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
+  if (FAILURE == DMC2280Send(self, cmd))
+    return HWFault; 
+  snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
   if (FAILURE == DMC2280Send(self, cmd))
     return HWFault; 
   else
@@ -761,6 +850,18 @@ static int DMC2280GetPar(void *pData, char *name,
     *fValue = self->maxDecel;
     return 1;
   }
+  if(strcmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
+    *fValue = self->blockage_ckInterval;
+    return 1;
+  }
+  if (self->abs_endcoder != 0) {
+    if (strcmp(name,"absenc") == 0) {
+      if (readAbsEnc(self, fValue) == SUCCESS)
+        return 1;
+      else
+        return 0;
+    }
+  }
   return 0;
 }
 
@@ -793,12 +894,12 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
       return 1;
     else {
       if ( (self->fLower - newValue) > FLT_EPSILON) {
-        snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", HOME, self->fLower);
+        snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, HOME, self->fLower);
         SCWrite(pCon, pError, eError);
         return 1;
       }
       if ( (newValue - self->fUpper) > FLT_EPSILON) {
-        snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", HOME, self->fUpper);
+        snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, HOME, self->fUpper);
         SCWrite(pCon, pError, eError);
         return 1;
       }
@@ -816,20 +917,13 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
     return 1;
   }
 
-  /* Set hard limits, managers only */
+  /* Set interval between blocked motor checks,
-  if(strcmp(name,HARDLOWERLIM) == 0) {
+   * managers only */
+  if(strcmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
     if(!SCMatchRights(pCon,usMugger))
       return 1;
     else {
-      self->fLower = newValue;
+      self->blockage_ckInterval = newValue;
-      return 1;
-    }
-  }
-  if(strcmp(name,HARDUPPERLIM) == 0) {
-    if(!SCMatchRights(pCon,usMugger))
-      return 1;
-    else {
-      self->fUpper = newValue;
       return 1;
     }
   }
@@ -837,12 +931,12 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
   /* Set speed */
   if(strcmp(name,SPEED) == 0) {
     if ((0.0 - newValue) > FLT_EPSILON) {
-      snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", SPEED, 0.0);
+      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, SPEED, 0.0);
       SCWrite(pCon, pError, eError);
       return 1;
     }
     if ((newValue - self->maxSpeed ) > FLT_EPSILON) {
-      snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", SPEED, self->maxSpeed);
+      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, SPEED, self->maxSpeed);
       SCWrite(pCon, pError, eError);
       return 1;
     }
@@ -856,12 +950,12 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
   /* Set Acceleration */
   if(strcmp(name,ACCEL) == 0) {
     if ((0.0 - newValue) > FLT_EPSILON) {
-      snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", ACCEL, 0.0);
+      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, ACCEL, 0.0);
       SCWrite(pCon, pError, eError);
       return 1;
     }
     if ((newValue - self->maxAccel ) > FLT_EPSILON) {
-      snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", ACCEL, self->maxAccel);
+      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, ACCEL, self->maxAccel);
       SCWrite(pCon, pError, eError);
       return 1;
     }
@@ -875,12 +969,12 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
   /* Set Deceleration */
   if(strcmp(name,DECEL) == 0) {
     if ((0.0 - newValue) > FLT_EPSILON) {
-      snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", DECEL, 0.0);
+      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, DECEL, 0.0);
       SCWrite(pCon, pError, eError);
       return 1;
     }
     if ((newValue - self->maxDecel ) > FLT_EPSILON) {
-      snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", DECEL, self->maxDecel);
+      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, DECEL, self->maxDecel);
       SCWrite(pCon, pError, eError);
       return 1;
     }
@@ -945,7 +1039,7 @@ static void KillDMC2280(/*@only@*/void *pData){
 /*@null@*/ /*@only@*/ static prs232 DMC2280Connect(/*@dependent@*/SConnection *pCon, char *host, int port) {
   prs232 controller=NULL;
   char pError[ERRLEN];
-  int usecTimeout = 50000; /* 50msec timeout */
+  int msecTimeout = 5000;
 
   controller=createRS232(host,port);
   if (controller==NULL)  {
@@ -965,7 +1059,7 @@ static void KillDMC2280(/*@only@*/void *pData){
   }
   setRS232ReplyTerminator(controller,"&\r\n:");
   setRS232SendTerminator(controller,"\r\n");
-  setRS232Timeout(controller, usecTimeout);
+  setRS232Timeout(controller, msecTimeout);
   return controller;
 }
 
@@ -1067,8 +1161,19 @@ static void KillDMC2280(/*@only@*/void *pData){
   pNew->SetDriverPar = DMC2280SetPar;
   pNew->ListDriverPar = DMC2280List;
   pNew->KillPrivate = KillDMC2280;
+  pNew->blockage_ckInterval = 0.5;
 
   /* PARAMETERS: Fetch parameter values */
+  if ((pPtr=getParam(pCon, interp, params,HARDLOWERLIM,_REQUIRED)) == NULL) {
+    KillDMC2280(pNew);
+    return NULL;
+  }
+  sscanf(pPtr,"%f",&(pNew->fLower));
+  if ((pPtr=getParam(pCon, interp, params,HARDUPPERLIM,_REQUIRED)) == NULL) {
+    KillDMC2280(pNew);
+    return NULL;
+  }
+  sscanf(pPtr,"%f",&(pNew->fUpper));
   if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) {
     KillDMC2280(pNew);
     return NULL;
@@ -1133,15 +1238,15 @@ static void KillDMC2280(/*@only@*/void *pData){
     /* Set speed */
     snprintf(cmd,CMDLEN,"SP%c=%d", pNew->axisLabel, motSpeed(pNew, pNew->speed));
     if (FAILURE == DMC2280Send(pNew, cmd))
-      exit(EXIT_FAILURE);
+      return NULL;
     /* Set acceleration */
     snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel));
     if (FAILURE == DMC2280Send(pNew, cmd))
-      exit(EXIT_FAILURE);
+      return NULL;
     /* Set deceleration */
     snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel));
     if (FAILURE == DMC2280Send(pNew, cmd))
-      exit(EXIT_FAILURE);
+      return NULL;
     /* TODO Initialise current position and target to get a sensible initial list output */
   return (MotorDriver *)pNew;
 }