From 7bf31b710b1c163f66256852b551c38aca9990e3 Mon Sep 17 00:00:00 2001 From: koennecke Date: Wed, 16 Mar 2005 07:59:56 +0000 Subject: [PATCH] - Fixes to el734hp.c for handling of emergency stop - Added waiting 5 sec after driving A6 to tas code --- el734driv.c | 2 +- el734hp.c | 122 +++++++++++++++++++++++++++++++++++++++++++++------- tasdrive.c | 4 ++ tasinit.c | 24 ++++++++++- 4 files changed, 135 insertions(+), 17 deletions(-) diff --git a/el734driv.c b/el734driv.c index d12ed9c..5e4f8c0 100644 --- a/el734driv.c +++ b/el734driv.c @@ -8,7 +8,7 @@ Copyright: - Labor fuer Neutronenstreuung + Labor fuer Neutrnenstreuung Paul Scherrer Institut CH-5423 Villigen-PSI diff --git a/el734hp.c b/el734hp.c index 3885691..ea2cd4f 100644 --- a/el734hp.c +++ b/el734hp.c @@ -77,6 +77,66 @@ #define POSFAULT -13 #define BADCUSHION -14 #define BADCOUNT -15 +/*------------------------------------------------------------------- + This reruns the command when an emergency stop has been detected. + This ougth to fix the problem that there is still another *ES in the + line even after releasing the emergency stop button + --------------------------------------------------------------------*/ +static int transactEL734(prs232 self, void *send, int sendLen, + void *reply, int replylen){ + int status, len, oldTimeout; + char *pReply = NULL; + + /* + try to read away rubbish on the line first + */ + oldTimeout = getRS232Timeout(self); + setRS232Timeout(self,0); + if(availableRS232(self)){ + len = replylen; + readRS232(self,reply,&len); + } + setRS232Timeout(self,oldTimeout); + + + /* + Actually do something. Some controllers send one erroneus *ES when the + emergency stop had been released. Therefore we believe an emergency stop + message only at the second try. This is the logic below. + */ + status = transactRS232(self,send,sendLen,reply,replylen); + if(status == 1){ + pReply = (char *)reply; + if(strstr(pReply,"*ES") != NULL){ + if(availableRS232(self)){ + len = replylen; + readRS232TillTerm(self,reply,&len); + } + return transactRS232(self,send,sendLen,reply,replylen); + } else { + return status; + } + } else { + return status; + } +} +/*------------------------------------------------------------------- + If we get an empty reply when we expect some response data, then, maybe, + the controller has sent us "\r*ES\r". Therefore this checks on + empty messages if there is some more, i.e: the *ES + --------------------------------------------------------------------*/ +static void checkEmpty(pEL734Driv self, char *pReply, int *replylen){ + int oldTimeout; + + if(strlen(pReply) < 1) { + oldTimeout = getRS232Timeout(self->controller); + setRS232Timeout(self->controller,0); + if(availableRS232(self->controller)){ + readRS232(self->controller,pReply,replylen); + } + setRS232Timeout(self->controller,oldTimeout); + } +} /*--------------------------------------------------------------------*/ static int checkResponse(pEL734Driv self, char *pReply){ /* @@ -111,7 +171,7 @@ static int checkResponse(pEL734Driv self, char *pReply){ /*---------------------------------------------------------------------*/ static int EL734GetPos(void *pData, float *fPos){ pEL734Driv self = NULL; - int status; + int status, replyLen; char pCommand[50],pReply[80]; self = (pEL734Driv)pData; @@ -123,12 +183,16 @@ static int EL734GetPos(void *pData, float *fPos){ } snprintf(pCommand,79,"u %d\r",self->iMotor); - status = transactRS232(self->controller,pCommand,strlen(pCommand), + status = transactEL734(self->controller,pCommand,strlen(pCommand), pReply,79); if(status != 1){ self->errorCode = status; return HWFault; } + + replyLen = 79; + checkEmpty(self,pReply,&replyLen); + if(!checkResponse(self,pReply)){ return HWFault; } @@ -148,7 +212,7 @@ static int EL734Run(void *pData,float fValue){ self->oredMsr = 0; snprintf(pCommand,79,"p %d %.3f\r",self->iMotor,fValue); - status = transactRS232(self->controller,pCommand,strlen(pCommand), + status = transactEL734(self->controller,pCommand,strlen(pCommand), pReply,79); if(status != 1){ self->errorCode = status; @@ -189,9 +253,6 @@ static int decodeMSR(pEL734Driv self, int msr){ } else if(self->oredMsr & 0x1000){ self->errorCode = BADCUSHION; return HWFault; - } else if(self->oredMsr & 0x8){ - self->errorCode = BADSTP; - return HWFault; } else if(self->oredMsr & 0x40) { self->errorCode = BADSTP; return HWFault; @@ -225,19 +286,22 @@ static int decodeMSR(pEL734Driv self, int msr){ /*------------------------------------------------------------------------*/ static int EL734Status(void *pData){ pEL734Driv self = NULL; - int status, msr; + int status, msr, replyLen = 79; char pCommand[50],pReply[80]; self = (pEL734Driv)pData; assert(self); snprintf(pCommand,79,"msr %d\r",self->iMotor); - status = transactRS232(self->controller,pCommand,strlen(pCommand), + status = transactEL734(self->controller,pCommand,strlen(pCommand), pReply,79); if(status < 0){ self->errorCode = status; return HWFault; } + + checkEmpty(self,pReply,&replyLen); + if(!checkResponse(self,pReply)){ return HWFault; } @@ -345,9 +409,17 @@ static int EL734Fix(void *pData, int iCode, float fValue){ return MOTREDO; } break; + case BADUNKNOWN: + if(availableRS232(self->controller)){ + len = 79; + readRS232TillTerm(self->controller,pReply,&len); + return MOTREDO; + } + return MOTFAIL; + break; case BADLOC: snprintf(pCommand,49,"RMT 1\r"); - transactRS232(self->controller,pCommand,strlen(pCommand),pReply,79); + transactEL734(self->controller,pCommand,strlen(pCommand),pReply,79); return MOTREDO; case NOTCONNECTED: initRS232(self->controller); @@ -384,7 +456,7 @@ static int EL734Halt(void *pData){ assert(self); snprintf(pCommand,79,"s %d\r",self->iMotor); - status = transactRS232(self->controller,pCommand,strlen(pCommand), + status = transactEL734(self->controller,pCommand,strlen(pCommand), pReply,79); if(status != 1){ self->errorCode = status; @@ -399,19 +471,22 @@ static int EL734Halt(void *pData){ static int EL734GetPar(void *pData, char *name, float *fValue){ pEL734Driv self = NULL; - int status; + int status, replyLen = 79; char pCommand[50],pReply[80]; self = (pEL734Driv)pData; assert(self); if(strcmp(name,"speed") == 0){ snprintf(pCommand,79,"J %d\r",self->iMotor); - status = transactRS232(self->controller,pCommand,strlen(pCommand), + status = transactEL734(self->controller,pCommand,strlen(pCommand), pReply,79); if(status != 1){ self->errorCode = status; return 0; } + + checkEmpty(self,pReply,&replyLen); + if(!checkResponse(self,pReply)){ return 0; } @@ -429,15 +504,32 @@ static int EL734SetPar(void *pData, SConnection *pCon, self = (pEL734Driv)pData; assert(self); + pCommand[0] ='\0'; if(strcmp(name,"speed") == 0){ snprintf(pCommand,79,"J %d %d\r",self->iMotor,(int)newValue); - status = transactRS232(self->controller,pCommand,strlen(pCommand), + } else if(strcmp(name,"forceupper") == 0){ + if(!SCMatchRights(pCon,usMugger)){ + return 0; + } + self->fUpper = newValue; + snprintf(pCommand,79,"H %d %8.3f %8.3f\r",self->iMotor,self->fLower, self->fUpper); + } else if(strcmp(name,"forcelower") == 0){ + if(!SCMatchRights(pCon,usMugger)){ + return 0; + } + self->fLower = newValue; + snprintf(pCommand,79,"H %d %8.3f %8.3f\r",self->iMotor,self->fLower, self->fUpper); + } + if(strlen(pCommand) > 1){ + status = transactEL734(self->controller,pCommand,strlen(pCommand), pReply,79); if(status != 1){ self->errorCode = status; return 0; } if(!checkResponse(self,pReply)){ + snprintf(pCommand,79,"ERROR %s while setting parameter",pReply); + SCWrite(pCon,pCommand,eError); return 0; } return 1; @@ -512,14 +604,14 @@ MotorDriver *CreateEL734HP(SConnection *pCon, int argc, char *argv[]){ pNew->KillPrivate = KillEL734; pNew->controller = controller; pNew->iMotor = motor; - + /* connection will already have been set up, read limits */ snprintf(pCommand,49,"h %d\r",pNew->iMotor); success = 0; for(i = 0; i < 3; i++){ - status = transactRS232(pNew->controller, pCommand,strlen(pCommand), + status = transactEL734(pNew->controller, pCommand,strlen(pCommand), pReply,79); if(status != 1){ getRS232Error(status,pReply,79); diff --git a/tasdrive.c b/tasdrive.c index c0693c8..239154c 100644 --- a/tasdrive.c +++ b/tasdrive.c @@ -301,6 +301,10 @@ int TASDrive(SConnection *pCon, SicsInterp *pSics, void *pData, */ status = Wait4Success(GetExecutor()); TASUpdate(self,pCon); + + /* + handle interrupts + */ if(status == DEVINT) { if(SCGetInterrupt(pCon) == eAbortOperation) diff --git a/tasinit.c b/tasinit.c index 96ccca5..77dd710 100644 --- a/tasinit.c +++ b/tasinit.c @@ -298,7 +298,18 @@ static int RecalcAction(SConnection *pCon, SicsInterp *pSics, void *pData, return TASUpdate(self,pCon); } - +/*-------------------------------------------------------------------------- + when A6 has been drive Bertrand wants to wait for some seconds in order + to allow for the analyzer shielding to settle down. This is done + through this callback function + ---------------------------------------------------------------------------*/ +static int A6WaitCallback(int iEvent, void *pEventData, void *pUserData) +{ + if(iEvent == MOTEND) + { + SicsWait(5); + } +} /*----------------------------------------------------------------------- A function for killing the TAS data structure is needed -------------------------------------------------------------------------*/ @@ -321,6 +332,7 @@ int TASFactory(SConnection *pCon, SicsInterp *pSics, void *pData, char pBueffel[132]; pSicsVariable pVar = NULL; CommandList *pCom = NULL; + pMotor pMot = NULL; /* check arguments*/ if(argc < 2) @@ -395,6 +407,16 @@ int TASFactory(SConnection *pCon, SicsInterp *pSics, void *pData, RegisterCallback(pVar->pCall,VALUECHANGE,TimerCallback,pNew,NULL); } + /* + Install the wait callback for A6 + */ + pMot = FindMotor(pSics,"a6"); + if(pMot != NULL) + { + RegisterCallback(pMot->pCall,MOTEND,A6WaitCallback,NULL,NULL); + } + + /* initialize SRO value */