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

PSI sics-cvs-psi-2006

Browse Source
This commit is contained in:
koennecke
2006-05-08 02:00:00 +00:00
committed by Douglas Clowes
parent ae77364de2
commit 6e926b813f
388 changed files with 445529 additions and 14109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

312
motor.c
View File

@@ -52,6 +52,7 @@
#include "splitter.h"
#include "status.h"
#include "servlog.h"
#include "tclmotdriv.h"
#include "site.h"
/*-------------------------------------------------------------------------
some lokal defines
@@ -62,20 +63,19 @@
#define INTBATCH 2.
#define INTHALT 3.
#define HLOW 0
#define HUPP 1
#define SLOW 2
#define SUPP 3
#define SZERO 4
#define FIX 5
#define INT 6
#define PREC 7
#define USRIGHTS 8
#define SPEED 9
#define SIGN 10
#define ECOUNT 11
#define POSCOUNT 12
#define IGNOREFAULT 13
#define SLOW 0
#define SUPP 1
#define SZERO 2
#define FIX 3
#define INT 4
#define PREC 5
#define USRIGHTS 6
#define SIGN 7
#define ECOUNT 8
#define POSCOUNT 9
#define IGNOREFAULT 10
#define MOVECOUNT 11
/*------------------------------------------------------------------------
a tiny structure used in CallBack work
*/
@@ -119,6 +119,7 @@
if(self->pDrivInt->iErrorCount < 0)
self->pDrivInt->iErrorCount = 0;
self->stopped = 1;
return self->pDriver->Halt((void *)self->pDriver);
}
@@ -173,10 +174,14 @@
fputs(pBueffel,fd);
sprintf(pBueffel,"%s precision %f\n",name,ObVal(self->ParArray,PREC));
fputs(pBueffel,fd);
sprintf(pBueffel,"%s ignorefault %f\n",name,ObVal(self->ParArray,IGNOREFAULT));
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));
sprintf(pBueffel,"%s movecount %f\n",name,
ObVal(self->ParArray,MOVECOUNT));
fputs(pBueffel,fd);
return 1;
}
@@ -208,10 +213,13 @@ static int statusRunTo(pMotor self, SConnection *pCon)
if(self->retryCount >= ObVal(self->ParArray,POSCOUNT))
{
snprintf(pBueffel,255,"ERROR: aborting motor %s after %d retries",
self->name, (int)ObVal(self->ParArray,POSCOUNT));
self->name, self->retryCount);
SCWrite(pCon,pBueffel,eError);
return HWFault;
}
if(SCGetInterrupt(pCon) != eContinue){
return HWFault;
}
self->retryCount++;
snprintf(pBueffel,255,"WARNING: restarting %s, %d time",
self->name,self->retryCount);
@@ -230,10 +238,16 @@ static int checkPosition(pMotor self, SConnection *pCon)
self->fPosition = fHard;
if(absf(fHard - self->fTarget) > ObVal(self->ParArray,PREC))
{
if(SCGetInterrupt(pCon) != eContinue)
if (SCGetInterrupt(pCon) != eContinue)
{
return HWFault;
}
if(self->stopped)
{
snprintf(pBueffel,131,"WARNING: %s stopped", self->name);
SCWrite(pCon,pBueffel, eWarning);
return HWFault;
}
snprintf(pBueffel,131,"WARNING: %s off position by %f",
self->name, absf(fHard - self->fTarget));
SCWrite(pCon,pBueffel, eWarning);
@@ -248,6 +262,7 @@ static void finishDriving(pMotor self, SConnection *pCon)
MotCallback sCall;
MotorGetSoftPosition(self,pCon,&sCall.fVal);
sCall.pName = self->name;
InvokeCallBack(self->pCall, MOTDRIVE, &sCall); /* send also very last position */
InvokeCallBack(self->pCall, MOTEND, &sCall);
}
/*--------------------------------------------------------------------*/
@@ -266,9 +281,13 @@ static int reportAndFixError(pMotor self, SConnection *pCon)
newStatus = HWFault;
break;
case MOTREDO:
snprintf(pBueffel,255,"WARNING: %s on %s",pError,self->name);
SCWrite(pCon,pBueffel,eWarning);
newStatus = statusRunTo(self,pCon);
break;
case MOTOK:
snprintf(pBueffel,255,"WARNING: %s on %s",pError,self->name);
SCWrite(pCon,pBueffel,eWarning);
newStatus = HWIdle;
break;
default:
@@ -312,6 +331,14 @@ static int evaluateStatus(pMotor self, SConnection *pCon)
{
newStatus = HWPosFault;
}
if(newStatus == HWIdle || newStatus == OKOK)
{
newStatus = checkPosition(self,pCon);
if(newStatus != HWBusy)
{
finishDriving(self,pCon);
}
}
break;
case HWBusy:
newStatus = HWBusy;
@@ -335,138 +362,13 @@ static int evaluateStatus(pMotor self, SConnection *pCon)
}
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)
{
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
if(iRet == MOTFAIL)
{
snprintf(pBueffel,255,"ERROR: %s on %s",pError,self->name);
SCWrite(pCon,pBueffel,eError);
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
self->retryCount = 0;
return HWFault;
}
else if(iRet == MOTREDO)
{
self->pDriver->RunTo(self->pDriver,self->fTarget);
self->retryCount++;
if(self->retryCount >= 3)
{
self->retryCount = 0;
return HWFault;
}
return HWBusy;
}
else
{
self->retryCount = 0;
return HWBusy;
}
}
/* 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)
{
snprintf(pBueffel,255,"ERROR: %s on %s",pError,self->name);
SCWrite(pCon,pBueffel,eError);
MotorInterrupt(pCon,ObVal(self->ParArray,INT));
self->retryCount = 0;
return HWFault;
}
else if(iRet == MOTREDO)
{
/*
abort if to many positioning faults
*/
self->posFaultCount++;
if(self->posFaultCount >= 4)
{
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;
}
else
{
return HWBusy;
}
}
else if(iRet == HWWarn)
{
self->pDriver->GetError(self->pDriver,&iCode,pError,131);
snprintf(pBueffel,255,"WARNING: %s on %s",pError,self->name);
SCWrite(pCon,pBueffel,eStatus);
return HWIdle;
}
self->retryCount = 0;
return iRet;
}
/*---------------------------------------------------------------------*/
static void handleMoveCallback(pMotor self, SConnection *pCon)
{
MotCallback sCall;
self->posCount++;
if(self->posCount >= ObVal(self->ParArray,POSCOUNT))
if(self->posCount >= ObVal(self->ParArray,MOVECOUNT))
{
MotorGetSoftPosition(self,pCon,&sCall.fVal);
sCall.pName = self->name;
@@ -484,13 +386,20 @@ static void handleMoveCallback(pMotor self, SConnection *pCon)
self = (pMotor)sulf;
status = evaluateStatus(self,pCon);
if (self->pDrivInt->drivableStatus!=status) {
((SConnection *)pCon)->conEventType=STATUS;
((SConnection *)pCon)->conStatus=status;
SCWrite(pCon, "", eEvent);
self->pDrivInt->drivableStatus=status;
}
if(status == HWBusy)
{
handleMoveCallback(self,pCon);
}
return status;
}
/*---------------------------------------------------------------------------*/ pMotor MotorInit(char *drivername, char *name, MotorDriver *pDriv)
/*---------------------------------------------------------------------------*/
pMotor MotorInit(char *drivername, char *name, MotorDriver *pDriv)
{
pMotor pM = NULL;
@@ -507,14 +416,12 @@ static void handleMoveCallback(pMotor self, SConnection *pCon)
/* create and initialize parameters */
pM->ParArray = ObParCreate(14);
pM->ParArray = ObParCreate(12);
if(!pM->ParArray)
{
free(pM);
return NULL;
}
ObParInit(pM->ParArray,HLOW,"hardlowerlim",pDriv->fLower,usMugger);
ObParInit(pM->ParArray,HUPP,"hardupperlim",pDriv->fUpper,usMugger);
ObParInit(pM->ParArray,SLOW,"softlowerlim",pDriv->fLower,usUser);
ObParInit(pM->ParArray,SUPP,"softupperlim",pDriv->fUpper,usUser);
ObParInit(pM->ParArray,SZERO,"softzero",ZEROINACTIVE,usUser);
@@ -522,11 +429,11 @@ static void handleMoveCallback(pMotor self, SConnection *pCon)
ObParInit(pM->ParArray,INT,"interruptmode",INTCONT,usMugger);
ObParInit(pM->ParArray,PREC,"precision",0.01,usMugger);
ObParInit(pM->ParArray,USRIGHTS,"accesscode",(float)usUser,usMugger);
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);
ObParInit(pM->ParArray,MOVECOUNT,"movecount",10.0,usMugger);
pDriv->GetPosition(pDriv,&(pM->fPosition));
pM->fTarget = pM->fPosition;
pM->endScriptID = 0;
@@ -630,7 +537,18 @@ extern void KillPiPiezo(void *pData);
{
ObPar *pPar = NULL;
assert(self);
if(strcmp(name,"hardupperlim") == 0)
{
*fVal = self->pDriver->fUpper;
return 1;
}
if(strcmp(name,"hardlowerlim") == 0)
{
*fVal = self->pDriver->fLower;
return 1;
}
pPar = ObParFind(self->ParArray,name);
if(pPar)
{
@@ -783,17 +701,17 @@ extern void KillPiPiezo(void *pData);
fHard = fHard*ObVal(self->ParArray,SIGN);
/* check for hardware limits */
if(fHard > ObVal(self->ParArray,HUPP))
if(fHard > self->pDriver->fUpper)
{
sprintf(pBueffel,"%f violates upper hardware limit %f on %s",
fVal,ObVal(self->ParArray,HUPP),self->name);
fVal,self->pDriver->fUpper,self->name);
strncpy(pError,pBueffel,iErrLen);
return 0;
}
if(fHard < ObVal(self->ParArray,HLOW))
if(fHard < self->pDriver->fLower)
{
sprintf(pBueffel,"%f violates lower hardware limit %f on %s",
fVal,ObVal(self->ParArray,HLOW),self->name);
fVal,self->pDriver->fLower,self->name);
strncpy(pError,pBueffel,iErrLen);
return 0;
}
@@ -862,7 +780,9 @@ extern void KillPiPiezo(void *pData);
/* Boundaries OK, send command */
self->posFaultCount = 0;
self->retryCount = 0;
self->stopped = 0;
self->fTarget = fHard;
self->posCount = 0;
iRet = self->pDriver->RunTo(self->pDriver,fHard);
if(iRet != OKOK)
{ /* try three times to fix it */
@@ -952,6 +872,21 @@ extern void KillPiPiezo(void *pData);
*fHard = fVal*ObVal(self->ParArray,SIGN);
return 0;
}
/*------------------------------------------------------------------------*/
float MotorHardToSoftPosition(pMotor self, float fValue)
{
/* apply zeropoint */
if(ObVal(self->ParArray,SIGN) < 0.)
{
fValue += ObVal(self->ParArray,SZERO);
}
else
{
fValue -= ObVal(self->ParArray,SZERO);
}
/* apply sign */
return fValue*ObVal(self->ParArray,SIGN);
}
/* ------------------------------------------------------------------------*/
int MotorGetSoftPosition(pMotor self, SConnection *pCon, float *fVal)
{
@@ -968,7 +903,6 @@ extern void KillPiPiezo(void *pData);
*fVal = fValue;
return 0;
}
/* apply zeropoint */
if(ObVal(self->ParArray,SIGN) < 0.)
{
@@ -981,7 +915,10 @@ extern void KillPiPiezo(void *pData);
*fVal = fValue;
/* apply sign */
/* *fVal = MotorHardToSoftPosition(self,fValue); */
*fVal = fValue*ObVal(self->ParArray,SIGN);
return 1;
}
/*---------------------------------------------------------------------------*/
@@ -1070,8 +1007,21 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
SCWrite(pCon,pBueffel,eError);
return 0;
}
pNew->ParArray[HLOW].iCode = usUser;
pNew->ParArray[HUPP].iCode = usUser;
}else if (strcmp(argv[2],"tclmot") == 0)
{
pDriver = CreateTclMotDriv(pCon,argc,argv);
if(!pDriver)
{
return 0;
}
/* create the motor */
pNew = MotorInit("TCLMOT",argv[1],pDriver);
if(!pNew)
{
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
SCWrite(pCon,pBueffel,eError);
return 0;
}
}
else
{
@@ -1106,18 +1056,25 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
int i, iLen;
iLen = ObParLength(self->ParArray);
sprintf(pBueffel,"Parameter Listing for motor %s\n",self->name);
sprintf(pBueffel,"Parameter Listing for motor %s",self->name);
SCWrite(pCon,pBueffel,eValue);
sprintf(pBueffel,"%s.Position = %f\n", self->name,self->fPosition);
SCWrite(pCon,pBueffel,eStatus);
sprintf(pBueffel,"%s.TargetPosition = %f\n", self->name,self->fTarget);
SCWrite(pCon,pBueffel,eStatus);
snprintf(pBueffel,511,"%s.hardlowerlim = %f",self->name,
self->pDriver->fLower);
SCWrite(pCon,pBueffel,eValue);
snprintf(pBueffel,511,"%s.hardupperlim = %f",self->name,
self->pDriver->fUpper);
SCWrite(pCon,pBueffel,eValue);
for(i = 0; i < iLen; i++)
{
sprintf(pBueffel,"%s.%s = %f\n",self->name,
self->ParArray[i].name,self->ParArray[i].fVal);
SCWrite(pCon,pBueffel,eStatus);
}
sprintf(pBueffel,"%s.Position = %f\n", self->name,self->fPosition);
SCWrite(pCon,pBueffel,eStatus);
sprintf(pBueffel,"%s.TargetPosition = %f\n", self->name,self->fTarget);
SCWrite(pCon,pBueffel,eStatus);
/*
list driver parameters when appropriate
@@ -1139,6 +1096,7 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
typedef struct {
char *pName;
SConnection *pCon;
float lastValue;
} MotInfo, *pMotInfo;
/*-----------------------------------------------------------------------*/
static void KillInfo(void *pData)
@@ -1154,7 +1112,8 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
free(self);
}
/*------------------- The CallBack function for interest ------------------*/
static int InterestCallback(int iEvent, void *pEvent, void *pUser)
static int InterestCallback(int iEvent, void *pEvent, void *pUser,
commandContext cc)
{
pMotInfo pInfo = NULL;
char pBueffel[80];
@@ -1165,9 +1124,13 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
psCall = (MotCallback *)pEvent;
pInfo = (MotInfo *)pUser;
sprintf(pBueffel,"%s.position = %f ", pInfo->pName, psCall->fVal);
SCWrite(pInfo->pCon,pBueffel,eValue);
if (pInfo->lastValue != psCall->fVal) {
pInfo->lastValue = psCall->fVal;
(pInfo->pCon)->conEventType=POSITION;
sprintf(pBueffel,"%s.position = %f ", pInfo->pName, pInfo->lastValue);
SCWriteInContext(pInfo->pCon,pBueffel,eEvent,cc);
}
return 1;
}
/*------------------------------------------------------------------------*/
@@ -1179,7 +1142,8 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
}
}
/*------------------------ The endscript callback function ----------------*/
static int EndScriptCallback(int iEvent, void *pEvent, void *pUser)
static int EndScriptCallback(int iEvent, void *pEvent, void *pUser,
commandContext cc)
{
char *pScript = NULL;
MotCallback *psCall;
@@ -1244,7 +1208,7 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
if(!iRet)
{
sprintf(pBueffel,"Error obtaining position for %s",argv[0]);
SCWrite(pCon,pBueffel,eValue);
SCWrite(pCon,pBueffel,eError);
DeleteTokenList(pList);
return 0;
}
@@ -1286,7 +1250,17 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
}
pMoti->pName = strdup(argv[0]);
pMoti->pCon = pCon;
lID = RegisterCallback(self->pCall, MOTDRIVE, InterestCallback,
iRet = MotorGetSoftPosition(self,pCon,&fValue);
if(!iRet)
{
sprintf(pBueffel,"Failed to register interest, Reason:Error obtaining current position for %s",argv[0]);
SCWrite(pCon,pBueffel,eError);
DeleteTokenList(pList);
return 0;
}
pMoti->lastValue = fValue;
lID = RegisterCallback(self->pCall, SCGetContext(pCon),MOTDRIVE, InterestCallback,
pMoti, KillInfo);
SCRegister(pCon,pSics, self->pCall,lID);
DeleteTokenList(pList);
@@ -1318,7 +1292,7 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
return 0;
}
self->endScriptID =
RegisterCallback(self->pCall, MOTEND, EndScriptCallback,
RegisterCallback(self->pCall, SCGetContext(pCon),MOTEND, EndScriptCallback,
strdup(pCurrent->text), KillScript);
SCRegister(pCon,pSics, self->pCall,self->endScriptID);
DeleteTokenList(pList);
@@ -1371,7 +1345,7 @@ extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
}
else
{
sprintf(pBueffel, " %s.%s = %f",self->name,pName->text,fValue);
sprintf(pBueffel, "%s.%s = %f",self->name,pName->text,fValue);
SCWrite(pCon,pBueffel,eValue);
DeleteTokenList(pList);
return 1;