linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

PSI sics-cvs-psi-complete-tree-post-site-support

Browse Source
This commit is contained in:
koennecke
2004-03-09 15:18:11 +00:00
committed by Douglas Clowes
parent 6373f6b0fb
commit ae77364de2
196 changed files with 8344 additions and 3485 deletions
Download Patch File Download Diff File Expand all files Collapse all files

459
motor.c
View File

@@ -10,13 +10,10 @@
endscript facility added: Mark Koennecke, August 2002
Modified to support driver parameters, Mark Koennecke, January 2003
TODO: currently motor drivers have to be installed in MakeMotor
and remembered in KillMotor. Sort this some day!
Copyright:
Labor fuer Neutronenstreuung
Paul Scherrer Institut
Paul Scherrer Institut
CH-5423 Villigen-PSI
@@ -55,8 +52,7 @@
#include "splitter.h"
#include "status.h"
#include "servlog.h"
#include "ecbdriv.h"
#include "site.h"
/*-------------------------------------------------------------------------
some lokal defines
*/
@@ -78,6 +74,8 @@
#define SPEED 9
#define SIGN 10
#define ECOUNT 11
#define POSCOUNT 12
#define IGNOREFAULT 13
/*------------------------------------------------------------------------
a tiny structure used in CallBack work
*/
@@ -177,6 +175,9 @@
fputs(pBueffel,fd);
sprintf(pBueffel,"%s AccessCode %f\n",name,ObVal(self->ParArray,USRIGHTS));
fputs(pBueffel,fd);
sprintf(pBueffel,"%s poscount %f\n",name,
ObVal(self->ParArray,POSCOUNT));
fputs(pBueffel,fd);
return 1;
}
/*-------------------------------------------------------------------------*/
@@ -199,77 +200,181 @@
SCSetInterrupt(pCon,iVal);
}
}
/*--------------------------------------------------------------------------
Refactor
--------------------------------------------------------------------------*/
static int MotorStatus(void *sulf, SConnection *pCon)
/*---------------------------------------------------------------------*/
static int statusRunTo(pMotor self, SConnection *pCon)
{
char pBueffel[256];
if(self->retryCount >= ObVal(self->ParArray,POSCOUNT))
{
float fHard;
pMotor self;
int iRet,i, iCode;
static int iRetry = 0;
char pError[132];
char pBueffel[256];
static int iPosFault = 0;
MotCallback sCall;
assert(sulf);
self = (pMotor)sulf;
snprintf(pBueffel,255,"ERROR: aborting motor %s after %d retries",
self->name, (int)ObVal(self->ParArray,POSCOUNT));
SCWrite(pCon,pBueffel,eError);
return HWFault;
}
self->retryCount++;
snprintf(pBueffel,255,"WARNING: restarting %s, %d time",
self->name,self->retryCount);
SCWrite(pCon,pBueffel,eWarning);
self->pDriver->RunTo(self->pDriver,self->fTarget);
return HWBusy;
}
/*--------------------------------------------------------------------*/
static int checkPosition(pMotor self, SConnection *pCon)
{
float fHard;
char pBueffel[132];
int status;
iRet = self->pDriver->GetStatus(self->pDriver);
fHard = MotorGetValue((void *)self,pCon);
if(fHard < -9999990)
MotorGetHardPosition(self,pCon,&fHard);
self->fPosition = fHard;
if(absf(fHard - self->fTarget) > ObVal(self->ParArray,PREC))
{
if(SCGetInterrupt(pCon) != eContinue)
{
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
if(iRet == MOTFAIL)
{
sprintf(pBueffel,"WARNING: %s on %s",pError,self->name);
SCWrite(pCon,pBueffel,eError);
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
iRetry = 0;
return HWFault;
}
else if(iRet == MOTREDO)
{
iRetry++;
if(iRetry >= 3)
{
iRetry = 0;
return HWFault;
}
}
else
{
iRetry = 0;
return HWBusy;
}
}
return HWFault;
}
snprintf(pBueffel,131,"WARNING: %s off position by %f",
self->name, absf(fHard - self->fTarget));
SCWrite(pCon,pBueffel, eWarning);
status = statusRunTo(self,pCon);
return status;
}
return HWIdle;
}
/*--------------------------------------------------------------------*/
static void finishDriving(pMotor self, SConnection *pCon)
{
MotCallback sCall;
MotorGetSoftPosition(self,pCon,&sCall.fVal);
sCall.pName = self->name;
InvokeCallBack(self->pCall, MOTEND, &sCall);
}
/*--------------------------------------------------------------------*/
static int reportAndFixError(pMotor self, SConnection *pCon)
{
char pBueffel[256], pError[131];
int iCode, iRet, newStatus;
/* invoke callback */
sCall.fVal = fHard;
sCall.pName = self->name;
InvokeCallBack(self->pCall, MOTDRIVE, &sCall);
/* motor thinks he is done */
if( (iRet == OKOK) || (iRet == HWIdle))
{
MotorGetSoftPosition(self,pCon,&sCall.fVal);
InvokeCallBack(self->pCall, MOTEND, &sCall);
self->fPosition = fHard;
if(absf(fHard - self->fTarget) > ObVal(self->ParArray,PREC))
{
sprintf(pBueffel,"WARNING: %s off position by %f",
self->name, absf(fHard - self->fTarget));
SCWrite(pCon,pBueffel, eWarning);
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
iRetry = 0;
return HWPosFault;
}
iRetry = 0;
return HWIdle;
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
switch(iRet)
{
case MOTFAIL:
snprintf(pBueffel,255,"ERROR: %s on %s",pError,self->name);
SCWrite(pCon,pBueffel,eError);
newStatus = HWFault;
break;
case MOTREDO:
newStatus = statusRunTo(self,pCon);
break;
case MOTOK:
newStatus = HWIdle;
break;
default:
SCWrite(pCon,"WARNING: bad status code in motor.c:reportAndFixError",
eWarning);
SCWrite(pCon,"You may continue, but show this to a SICS programmer",
eWarning);
newStatus = HWIdle;
break;
}
return newStatus;
}
/*---------------------------------------------------------------------
New version, refactored October 2003
-----------------------------------------------------------------------*/
static int evaluateStatus(pMotor self, SConnection *pCon)
{
int iRet, iCode, newStatus;
MotCallback sCall;
char pBueffel[256], pError[132];
float fHard;
iRet = self->pDriver->GetStatus(self->pDriver);
newStatus = iRet;
switch(iRet)
{
case OKOK:
case HWIdle:
newStatus = checkPosition(self,pCon);
if(newStatus != HWBusy)
{
finishDriving(self,pCon);
}
break;
case HWFault:
newStatus = reportAndFixError(self,pCon);
break;
case HWPosFault:
newStatus = reportAndFixError(self,pCon);
if(newStatus == HWFault && ObVal(self->ParArray,IGNOREFAULT) < 1)
{
newStatus = HWPosFault;
}
break;
case HWBusy:
newStatus = HWBusy;
break;
case HWWarn:
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
snprintf(pBueffel,255,"WARNING: motor reported: %s", pError);
SCWrite(pCon,pBueffel,eWarning);
newStatus = HWIdle;
break;
default:
SCWrite(pCon,"WARNING: Bad status in motor.c:evaluatStatus",eWarning);
SCWrite(pCon,"You may continue, but show this to a SICS programmer",
eWarning);
break;
}
if(newStatus == HWFault)
{
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
self->retryCount = 0;
}
return newStatus;
}
/*-----------------------------------------------------------------------
old version, kept for time being.
----------------------------------------------------------------------*/
static int eevaluateStatus(pMotor self, SConnection *pCon)
{
int iRet, iCode;
MotCallback sCall;
char pBueffel[256], pError[132];
float fHard;
iRet = self->pDriver->GetStatus(self->pDriver);
if( (iRet == OKOK) || (iRet == HWIdle))
{
MotorGetSoftPosition(self,pCon,&sCall.fVal);
sCall.pName = self->name;
InvokeCallBack(self->pCall, MOTEND, &sCall);
MotorGetHardPosition(self,pCon,&fHard);
self->fPosition = fHard;
if(absf(fHard - self->fTarget) > ObVal(self->ParArray,PREC))
{
snprintf(pBueffel,131,"WARNING: %s off position by %f",
self->name, absf(fHard - self->fTarget));
SCWrite(pCon,pBueffel, eWarning);
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
self->retryCount = 0;
/*
suppress HWPosFaults when ignore flag set
*/
if(ObVal(self->ParArray,IGNOREFAULT) > 0)
{
return HWIdle;
}
else
{
return HWPosFault;
}
}
self->retryCount = 0;
return HWIdle;
}
/* motor suggests a fault */
else if(iRet == HWFault)
@@ -278,50 +383,64 @@
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
if(iRet == MOTFAIL)
{
sprintf(pBueffel,"ERROR: %s on %s",pError,self->name);
snprintf(pBueffel,255,"ERROR: %s on %s",pError,self->name);
SCWrite(pCon,pBueffel,eError);
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
iRetry = 0;
self->retryCount = 0;
return HWFault;
}
else if(iRet == MOTREDO)
{
self->pDriver->RunTo(self->pDriver,self->fTarget);
iRetry++;
if(iRetry >= 3)
self->retryCount++;
if(self->retryCount >= 3)
{
iRetry = 0;
self->retryCount = 0;
return HWFault;
}
return HWBusy;
}
else
{
iRetry = 0;
self->retryCount = 0;
return HWBusy;
}
}
/* a positioning fault */
/* a positioning fault */
else if(iRet == HWPosFault)
{
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
if(iRet == MOTFAIL)
{
sprintf(pBueffel,"ERROR: %s on %s",pError,self->name);
snprintf(pBueffel,255,"ERROR: %s on %s",pError,self->name);
SCWrite(pCon,pBueffel,eError);
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
iRetry = 0;
self->retryCount = 0;
return HWFault;
}
else if(iRet == MOTREDO)
{
iPosFault++;
if(iPosFault == 2)
/*
abort if to many positioning faults
*/
self->posFaultCount++;
if(self->posFaultCount >= 4)
{
iPosFault = 0;
iRetry = 0;
return HWPosFault;
self->posFaultCount = 0;
self->retryCount = 0;
/*
do not do pass on positioning fault errors when the
appropriate flag has been set
*/
if(ObVal(self->ParArray,IGNOREFAULT) > 0)
{
return HWIdle;
}
else
{
return HWPosFault;
}
}
self->pDriver->RunTo(self->pDriver,self->fTarget);
return HWBusy;
@@ -334,12 +453,42 @@
else if(iRet == HWWarn)
{
self->pDriver->GetError(self->pDriver,&iCode,pError,131);
sprintf(pBueffel,"WARNING: %s on %s",pError,self->name);
snprintf(pBueffel,255,"WARNING: %s on %s",pError,self->name);
SCWrite(pCon,pBueffel,eStatus);
return HWIdle;
}
iRetry = 0;
self->retryCount = 0;
return iRet;
}
/*---------------------------------------------------------------------*/
static void handleMoveCallback(pMotor self, SConnection *pCon)
{
MotCallback sCall;
self->posCount++;
if(self->posCount >= ObVal(self->ParArray,POSCOUNT))
{
MotorGetSoftPosition(self,pCon,&sCall.fVal);
sCall.pName = self->name;
InvokeCallBack(self->pCall, MOTDRIVE, &sCall);
self->posCount = 0;
}
}
/*-----------------------------------------------------------------------*/
static int MotorStatus(void *sulf, SConnection *pCon)
{
pMotor self = NULL;
int status;
assert(sulf);
self = (pMotor)sulf;
status = evaluateStatus(self,pCon);
if(status == HWBusy)
{
handleMoveCallback(self,pCon);
}
return status;
}
/*---------------------------------------------------------------------------*/ pMotor MotorInit(char *drivername, char *name, MotorDriver *pDriv)
{
@@ -358,7 +507,7 @@
/* create and initialize parameters */
pM->ParArray = ObParCreate(12);
pM->ParArray = ObParCreate(14);
if(!pM->ParArray)
{
free(pM);
@@ -376,6 +525,8 @@
ObParInit(pM->ParArray,SPEED,"speed",0.02,usInternal);
ObParInit(pM->ParArray,SIGN,"sign",1.0,usMugger);
ObParInit(pM->ParArray,ECOUNT,"failafter",3.0,usMugger);
ObParInit(pM->ParArray,POSCOUNT,"maxretry",3.0,usMugger);
ObParInit(pM->ParArray,IGNOREFAULT,"ignorefault",0.0,usMugger);
pDriv->GetPosition(pDriv,&(pM->fPosition));
pM->fTarget = pM->fPosition;
pM->endScriptID = 0;
@@ -449,26 +600,15 @@ extern void KillPiPiezo(void *pData);
/* kill driver */
if(pM->drivername)
{ /* edit here to include more drivers */
if(strcmp(pM->drivername,"EL734") == 0)
{
KillEL734((void *)pM->pDriver);
}
else if(strcmp(pM->drivername,"EL734DC") == 0)
{
KillEL734((void *)pM->pDriver);
}
else if(strcmp(pM->drivername,"SIM") == 0)
{
if(pM->pDriver->KillPrivate != NULL)
{
KillSIM((void *)pM->pDriver);
}
else if(strcmp(pM->drivername,"PIPIEZO") == 0)
{
KillPiPiezo((void *)pM->pDriver);
}
else if(strcmp(pM->drivername,"ECB") == 0)
{
KillECBMotor( (void *)pM->pDriver);
pM->pDriver->KillPrivate(pM->pDriver);
if(pM->pDriver->name != NULL)
{
free(pM->pDriver->name);
}
free(pM->pDriver);
}
free(pM->drivername);
}
@@ -695,6 +835,15 @@ extern void KillPiPiezo(void *pData);
return 0;
}
/* check if the bad motor flag is set */
if((int)ObVal(self->ParArray,IGNOREFAULT) > 0)
{
snprintf(pBueffel,511,"WARNING: motor %s is unreliable",
self->name);
SCWrite(pCon,pBueffel,eWarning);
self->pDrivInt->iErrorCount = 0;
}
/* check our error count and interrupt if to much */
iCode = (int)ObVal(self->ParArray,ECOUNT);
if(self->pDrivInt->iErrorCount > iCode)
@@ -711,6 +860,8 @@ extern void KillPiPiezo(void *pData);
}
/* Boundaries OK, send command */
self->posFaultCount = 0;
self->retryCount = 0;
self->fTarget = fHard;
iRet = self->pDriver->RunTo(self->pDriver,fHard);
if(iRet != OKOK)
@@ -888,7 +1039,8 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
char pBueffel[512];
int iD, iRet;
Tcl_Interp *pTcl = (Tcl_Interp *)pSics->pTcl;
pSite site = NULL;
assert(pCon);
assert(pSics);
@@ -902,58 +1054,7 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
/* create the driver */
strtolower(argv[2]);
strtolower(argv[1]);
if(strcmp(argv[2],"el734") == 0)
{
iD = argc - 3;
pDriver = CreateEL734(pCon,iD,&argv[3]);
if(!pDriver)
{
return 0;
}
/* create the motor */
pNew = MotorInit("EL734",argv[1],pDriver);
if(!pNew)
{
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
}
else if(strcmp(argv[2],"ecb") == 0)
{
iD = argc - 3;
pDriver = CreateECBMotor(pCon,iD,&argv[3]);
if(!pDriver)
{
return 0;
}
/* create the motor */
pNew = MotorInit("ECB",argv[1],pDriver);
if(!pNew)
{
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
}
else if(strcmp(argv[2],"el734dc") == 0)
{
iD = argc - 3;
pDriver = CreateEL734DC(pCon,iD,&argv[3]);
if(!pDriver)
{
return 0;
}
/* create the motor */
pNew = MotorInit("EL734DC",argv[1],pDriver);
if(!pNew)
{
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
}
else if (strcmp(argv[2],"sim") == 0)
if (strcmp(argv[2],"sim") == 0)
{
iD = argc - 3;
pDriver = CreateSIM(pCon,iD,&argv[3]);
@@ -972,30 +1073,20 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
pNew->ParArray[HLOW].iCode = usUser;
pNew->ParArray[HUPP].iCode = usUser;
}
else if (strcmp(argv[2],"pipiezo") == 0)
{
pDriver = MakePiPiezo(pSics->pTcl,argv[3]);
if(!pDriver)
{
SCWrite(pCon,pTcl->result,eError);
return 0;
}
/* create the motor */
pNew = MotorInit("PIPIEZO",argv[1],pDriver);
if(!pNew)
{
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
}
else
{
sprintf(pBueffel,"Motor Type %s not recognized for motor %s",
argv[2],argv[1]);
SCWrite(pCon,pBueffel,eError);
return 0;
site = getSite();
if(site != NULL)
{
pNew = site->CreateMotor(pCon,argc-1,&argv[1]);
}
if(pNew == NULL)
{
sprintf(pBueffel,"Motor Type %s not recognized for motor %s",
argv[2],argv[1]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
}
/* create the interpreter command */