From 634f2023b104f73cf629e6e60b1ddf552afc8e4e Mon Sep 17 00:00:00 2001 From: Ferdi Franceschini Date: Mon, 12 Feb 2007 12:06:19 +1100 Subject: [PATCH] Modified during MK's visit. r1462 | ffr | 2007-02-12 12:06:19 +1100 (Mon, 12 Feb 2007) | 2 lines --- site_ansto/Makefile | 11 +- site_ansto/make_gen_variables | 7 +- site_ansto/motor_dmc2280.c | 189 ++++++++++++++++++++++++++-------- 3 files changed, 158 insertions(+), 49 deletions(-) diff --git a/site_ansto/Makefile b/site_ansto/Makefile index 16e66905..1ca0292b 100644 --- a/site_ansto/Makefile +++ b/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)\ - -I /usr/local/include/json/ + -fwritable-strings -DCYGNUS -DNONINTF -g $(DFORTIFY) 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 - $(CC) -g -o SICServer $(COREOBJ:%=../%) $(SUBLIBS) $(PSI_SLIBS:%=../%) $(PSI_LIBS) $(GHTTP_LIBS) +all: ../matrix/libmatrix.a $(COREOBJ:%=../%) $(EXTRA:%=../%) libansto.a libhardsup + $(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 diff --git a/site_ansto/make_gen_variables b/site_ansto/make_gen_variables index 386f94bb..f8fd0b6e 100644 --- a/site_ansto/make_gen_variables +++ b/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\ - $(EXTRA) + sinfox.o sicslist.o cone.o hipadaba.o sicshipadaba.o statistics.o \ + 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 diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 042d045f..60fde224 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -8,9 +8,8 @@ * Ferdi Franceschini November 2005 * * TODO - * - check for ESTOP flag on controller. - * - ESTOP=1 means emergency stop, motion on all axes must stop. - * - ESTOP=0 means all OK. + * - check for motors enabled on plc + * - Check error bit, see Dan's email */ #include /* ISO C Standard: 7.16 Boolean type and values */ @@ -26,6 +25,7 @@ #include #include #include +#include #include "anstoutil.h" /* @@ -61,9 +61,9 @@ typedef struct __MoDriv { /* general motor driver interface fields. _REQUIRED! */ - float fUpper; /* upper limit */ - float fLower; /* lower limit */ - char *name; + float fUpper; /**< hard upper limit */ + float fLower; /**< hard lower limit */ + 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; icontroller, 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; icontroller, 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 (FAILURE == DMC2280Send(self, cmd)) + if (readAbsEnc(self, &absEncPos) == FAILURE) 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) { - self->errorCode = BADBSY; - return HWBusy; + /* 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; } - return MOTFAIL; } /** \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 */ - if(strcmp(name,HARDLOWERLIM) == 0) { + /* Set interval between blocked motor checks, + * managers only */ + if(strcmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) { if(!SCMatchRights(pCon,usMugger)) return 1; else { - self->fLower = newValue; - return 1; - } - } - if(strcmp(name,HARDUPPERLIM) == 0) { - if(!SCMatchRights(pCon,usMugger)) - return 1; - else { - self->fUpper = newValue; + self->blockage_ckInterval = 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; }