- New drivers for EL737 and EL734 high performance
- Changes to makefiles SKIPPED: psi/dornier2.c psi/el734hp.c psi/el737driv.c psi/el737hpdriv.c psi/make_gen psi/makefile_alpha psi/psi.c psi/velodorn.c psi/velodorn.h psi/velodorn.w psi/hardsup/el737_utility.c psi/hardsup/makefile_alpha psi/tecs/makefile_alpha
This commit is contained in:
142
motor.c
142
motor.c
@ -74,6 +74,7 @@
|
||||
#define SPEED 9
|
||||
#define SIGN 10
|
||||
#define ECOUNT 11
|
||||
#define POSCOUNT 12
|
||||
/*------------------------------------------------------------------------
|
||||
a tiny structure used in CallBack work
|
||||
*/
|
||||
@ -173,6 +174,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;
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
@ -195,18 +199,145 @@
|
||||
SCSetInterrupt(pCon,iVal);
|
||||
}
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int evaluateStatus(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;
|
||||
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)
|
||||
{
|
||||
self->posFaultCount++;
|
||||
if(self->posFaultCount >= 4)
|
||||
{
|
||||
self->posFaultCount = 0;
|
||||
self->retryCount = 0;
|
||||
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))
|
||||
{
|
||||
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;
|
||||
}
|
||||
/*--------------------------------------------------------------------------
|
||||
Refactor
|
||||
--------------------------------------------------------------------------*/
|
||||
static int MotorStatus(void *sulf, SConnection *pCon)
|
||||
static int MotorStatus2(void *sulf, SConnection *pCon)
|
||||
{
|
||||
float fHard;
|
||||
pMotor self;
|
||||
int iRet,i, iCode;
|
||||
static int iRetry = 0;
|
||||
char pError[132];
|
||||
char pBueffel[256];
|
||||
static int iPosFault = 0;
|
||||
static int iPosFault = 0, iRetry = 0;
|
||||
MotCallback sCall;
|
||||
|
||||
|
||||
@ -354,7 +485,7 @@
|
||||
|
||||
|
||||
/* create and initialize parameters */
|
||||
pM->ParArray = ObParCreate(12);
|
||||
pM->ParArray = ObParCreate(13);
|
||||
if(!pM->ParArray)
|
||||
{
|
||||
free(pM);
|
||||
@ -372,6 +503,7 @@
|
||||
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,"poscount",20.0,usMugger);
|
||||
pDriv->GetPosition(pDriv,&(pM->fPosition));
|
||||
pM->fTarget = pM->fPosition;
|
||||
pM->endScriptID = 0;
|
||||
@ -696,6 +828,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)
|
||||
|
Reference in New Issue
Block a user