323 lines
8.5 KiB
C
323 lines
8.5 KiB
C
/*-------------------------------------------------------------------------
|
|
C h o c o A d a p t e r
|
|
|
|
This is a drivable adapter for the ChopperController object (or also generic
|
|
controller object). It allows to modify one of the variables supported by
|
|
the controller through the normal SICS drive command. For more information
|
|
see file choco.w or choco.tex.
|
|
|
|
|
|
Mark Koennecke, January 1998
|
|
---------------------------------------------------------------------------*/
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include <tcl.h>
|
|
#include "fortify.h"
|
|
#include "sics.h"
|
|
#include "choco.h"
|
|
#define CHADAINTERNAL
|
|
#include "chadapter.h"
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
static void *AdapterGetInterface(void *pData, int iID)
|
|
{
|
|
pCHAdapter self = NULL;
|
|
|
|
self = (pCHAdapter)pData;
|
|
assert(self);
|
|
if(iID == DRIVEID)
|
|
{
|
|
return self->pInt;
|
|
}
|
|
return NULL;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static int CHHalt(void *pData)
|
|
{
|
|
pCHAdapter self = NULL;
|
|
|
|
self = (pCHAdapter)pData;
|
|
assert(self);
|
|
|
|
self->pDriv->Halt(self->pDriv);
|
|
|
|
return 1;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static int CHLimits(void *pData, float fVal, char *error, int iErrlen)
|
|
{
|
|
pCHAdapter self = NULL;
|
|
|
|
self = (pCHAdapter)pData;
|
|
assert(self);
|
|
|
|
if(fVal < self->fLower)
|
|
{
|
|
strncpy(error,"Lower limit violated",iErrlen);
|
|
return 0;
|
|
}
|
|
if(fVal > self->fUpper)
|
|
{
|
|
strncpy(error,"Upper limit violated",iErrlen);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static float CHGetValue(void *pData, SConnection *pCon)
|
|
{
|
|
pCHAdapter self = NULL;
|
|
float fVal;
|
|
int iRet;
|
|
char pBueffel[80];
|
|
|
|
self = (pCHAdapter)pData;
|
|
assert(self);
|
|
|
|
iRet = self->pDriv->GetPar(self->pDriv,self->pParName,pBueffel,79);
|
|
if(!iRet)
|
|
{
|
|
fVal = -9999999.99;
|
|
self->pDriv->GetError(self->pDriv,&iRet,pBueffel,79);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return fVal;
|
|
}
|
|
sscanf(pBueffel,"%f",&fVal);
|
|
return fVal;
|
|
}
|
|
/*-----------------------------------------------------------------------*/
|
|
static int CHStatus(void *pData, SConnection *pCon)
|
|
{
|
|
pCHAdapter self = NULL;
|
|
int iRet, iCode;
|
|
static int iRetry = 0;
|
|
char pBueffel[80], pError[132];
|
|
|
|
self = (pCHAdapter)pData;
|
|
assert(self);
|
|
|
|
iRet = self->pDriv->CheckPar(self->pDriv,self->pParName);
|
|
switch(iRet)
|
|
{
|
|
case OKOK:
|
|
case HWIdle:
|
|
iRetry = 0;
|
|
return HWIdle;
|
|
case HWFault:
|
|
self->pDriv->GetError(self->pDriv,&iCode,pBueffel,79);
|
|
iRet = self->pDriv->TryFixIt(self->pDriv,iCode);
|
|
sprintf(pError,"ERROR: %s",pBueffel);
|
|
SCWrite(pCon,pError,eError);
|
|
if(iRet == CHFAIL || iRetry >= 3)
|
|
{
|
|
iRetry = 0;
|
|
return HWFault;
|
|
}
|
|
else
|
|
{
|
|
iRetry++;
|
|
self->pDriv->SetPar(self->pDriv,self->pParName,
|
|
self->fTarget);
|
|
return HWBusy;
|
|
}
|
|
break;
|
|
case HWBusy:
|
|
return HWBusy;
|
|
}
|
|
return HWFault;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static long CHSetValue(void *pData, SConnection *pCon, float fValue)
|
|
{
|
|
pCHAdapter self = NULL;
|
|
char pBueffel[80], pError[132];
|
|
int iRet, iCode, i;
|
|
|
|
self = (pCHAdapter)pData;
|
|
assert(self);
|
|
|
|
/* check privilege */
|
|
if(!SCMatchRights(pCon,usUser))
|
|
{
|
|
SCWrite(pCon,"ERROR: Insufficient privilege for driving",eError);
|
|
return 0;
|
|
}
|
|
|
|
for(i = 0; i < 3; i++)
|
|
{
|
|
iRet = self->pDriv->SetPar(self->pDriv,self->pParName,fValue);
|
|
if(iRet)
|
|
{
|
|
self->fTarget = fValue;
|
|
return 1;
|
|
}
|
|
self->pDriv->GetError(self->pDriv,&iCode,pBueffel,79);
|
|
sprintf(pError,"ERROR: %s",pBueffel);
|
|
SCWrite(pCon,pError,eError);
|
|
iRet = self->pDriv->TryFixIt(self->pDriv,iCode);
|
|
if(iRet == CHFAIL)
|
|
return 0;
|
|
}
|
|
return 0;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static void KillAdapter(void *pData)
|
|
{
|
|
pCHAdapter self = NULL;
|
|
|
|
self = (pCHAdapter)pData;
|
|
if(!self)
|
|
return;
|
|
|
|
if(self->pDes)
|
|
DeleteDescriptor(self->pDes);
|
|
|
|
if(self->pInt)
|
|
free(self->pInt);
|
|
|
|
if(self->pParName);
|
|
free(self->pParName);
|
|
|
|
free(self);
|
|
}
|
|
/*-----------------------------------------------------------------------
|
|
Syntax: ChopperAdapter name choppercontroller parname upper lower
|
|
*/
|
|
int CHAdapterFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
char pBueffel[256];
|
|
pCHAdapter pNew = NULL;
|
|
pChoco pChopper = NULL;
|
|
CommandList *pCom = NULL;
|
|
pDummy pDum = NULL;
|
|
double dUpper, dLower;
|
|
int iRet;
|
|
|
|
/* do we have enough arguments? */
|
|
if(argc < 6)
|
|
{
|
|
SCWrite(pCon,
|
|
"ERROR: Insufficient number of arguments to ChopperAdapter",
|
|
eError);
|
|
return 0;
|
|
}
|
|
|
|
/* find the chopper first */
|
|
pCom = FindCommand(pSics,argv[2]);
|
|
if(pCom)
|
|
{
|
|
pDum = (pDummy)pCom->pData;
|
|
if(pDum)
|
|
{
|
|
if(strcmp(pDum->pDescriptor->name,"Chopper") == 0)
|
|
{
|
|
pChopper = (pChoco)pCom->pData;
|
|
}
|
|
}
|
|
}
|
|
if(!pChopper)
|
|
{
|
|
sprintf(pBueffel,"ERROR: %s is NO chopper controller",
|
|
argv[3]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
|
|
/* interpret limits */
|
|
iRet = Tcl_GetDouble(pSics->pTcl,argv[5],&dUpper);
|
|
if(iRet != TCL_OK)
|
|
{
|
|
sprintf(pBueffel,
|
|
"ERROR: expected numeric argument for upper limit, got %s",
|
|
argv[4]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
iRet = Tcl_GetDouble(pSics->pTcl,argv[4],&dLower);
|
|
if(iRet != TCL_OK)
|
|
{
|
|
sprintf(pBueffel,
|
|
"ERROR: expected numeric argument for lower limit, got %s",
|
|
argv[5]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
|
|
/* allocate new adapter data structure */
|
|
pNew = (pCHAdapter)malloc(sizeof(CHAdapter));
|
|
if(!pNew)
|
|
{
|
|
SCWrite(pCon,"ERROR: out of memory in ChopperAdapter",eError);
|
|
return 0;
|
|
}
|
|
memset(pNew,0,sizeof(CHAdapter));
|
|
|
|
pNew->pDes = CreateDescriptor("ChopperAdapter");
|
|
pNew->pDriv = CHGetDriver(pChopper);
|
|
pNew->pInt = CreateDrivableInterface();
|
|
pNew->pParName = strdup(argv[3]);
|
|
if( !pNew->pDes || !pNew->pDriv || !pNew->pInt || !pNew->pParName)
|
|
{
|
|
SCWrite(pCon,"ERROR: out of memory in ChopperAdapter",eError);
|
|
return 0;
|
|
}
|
|
|
|
/* initialize other fields */
|
|
pNew->fTarget = 0.;
|
|
pNew->fLower = (float)dLower;
|
|
pNew->fUpper = (float)dUpper;
|
|
pNew->pDes->GetInterface = AdapterGetInterface;
|
|
pNew->pInt->Halt = CHHalt;
|
|
pNew->pInt->CheckLimits = CHLimits;
|
|
pNew->pInt->SetValue = CHSetValue;
|
|
pNew->pInt->CheckStatus = CHStatus;
|
|
pNew->pInt->GetValue = CHGetValue;
|
|
|
|
/* install command */
|
|
iRet = AddCommand(pSics, argv[1],CHAdapterAction,KillAdapter,pNew);
|
|
if(!iRet)
|
|
{
|
|
sprintf(pBueffel,
|
|
"ERROR: duplicate ChopperAdapter command %s NOT created",
|
|
argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
KillAdapter(pNew);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
int CHAdapterAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
pCHAdapter self = NULL;
|
|
int iRet;
|
|
char pBueffel[132];
|
|
float fValue;
|
|
|
|
self = (pCHAdapter)pData;
|
|
assert(self);
|
|
|
|
/* only action: get value */
|
|
fValue = CHGetValue(self,pCon);
|
|
if(fValue < -99000)
|
|
{
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
sprintf(pBueffel,"%s = %f",argv[0],fValue);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|