Added to repository.
r986 | ffr | 2006-05-09 09:17:15 +1000 (Tue, 09 May 2006) | 2 lines
This commit is contained in:
Ferdi Franceschini
committed by
Douglas Clowes
Douglas Clowes
parent
a72513165b
commit
81359a740f
403
site_ansto/refl2t.c
Normal file
403
site_ansto/refl2t.c
Normal file
@@ -0,0 +1,403 @@
|
||||
/*---------------------------------------------------------------------------
|
||||
REFL2T.c
|
||||
|
||||
Paul Hathaway, Nick Hauser, Mark Koennecke, February 2004
|
||||
---------------------------------------------------------------------------*/
|
||||
#include <stdlib.h>
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include "fortify.h"
|
||||
#include <tcl.h>
|
||||
#include "sics.h"
|
||||
#include "motor.h"
|
||||
#include "obpar.h"
|
||||
|
||||
#define DEBUG 1
|
||||
|
||||
#define MAXMOT 3
|
||||
#define MAXPAR 13
|
||||
|
||||
#define PI 3.14157
|
||||
|
||||
#include "refl2t.h"
|
||||
|
||||
|
||||
/* detector height movement */
|
||||
#define MOTCOZ 0
|
||||
/* detector movement along main axis */
|
||||
#define MOTCOY 1
|
||||
/* whole sample table height movement */
|
||||
#define MOTSOZ 2
|
||||
|
||||
|
||||
/*======================================================================
|
||||
The core of it all: The calculation of the settings for the various
|
||||
motors.
|
||||
========================================================================*/
|
||||
static int CalculateREFL(pRefl2T self, SConnection *pCon, float fNew)
|
||||
{
|
||||
float fDetPos, HeightOffset, DetLength;
|
||||
int iRet;
|
||||
|
||||
/* assume: solid sample, horizontal beam, surface at height zero */
|
||||
/* read vals of motor coy */
|
||||
iRet = MotorGetSoftPosition(self->aEngine[MOTCOY],pCon,&fDetPos);
|
||||
if(iRet != 1)
|
||||
{
|
||||
return iRet;
|
||||
}
|
||||
|
||||
HeightOffset = ObVal(self->aParameter,PARDHO);
|
||||
DetLength = ObVal(self->aParameter,PARDDD) + fDetPos;
|
||||
|
||||
/* calculate det height above beam axis */
|
||||
/* add detector offset height */
|
||||
|
||||
fNew = DetLength * tan((double)(fNew * PI / 180.0)) + HeightOffset ;
|
||||
self->fTarget = fNew;
|
||||
return 1;
|
||||
|
||||
/* Replacement code using MotEntry structure
|
||||
self->toStart[MOTCOZ].pMot = self->aEngine[MOTCOZ];
|
||||
strcpy(self->toStart[MOTCOZ].pName,self->aEngine[MOTCOZ]->name);
|
||||
self->toStart[MOTCOZ].fTarget = DetLength * tan((double)(fNew * PI / 180.0)) + HeightOffset;
|
||||
*/
|
||||
}
|
||||
|
||||
/*========================================================================
|
||||
Definition of interface functions.
|
||||
=========================================================================*/
|
||||
static long R2TSetValue(void *pData, SConnection *pCon, float fNew)
|
||||
{
|
||||
int i, iRet;
|
||||
pIDrivable pDriv = NULL;
|
||||
pRefl2T self = (pRefl2T) pData;
|
||||
|
||||
assert(self);
|
||||
|
||||
/* calculation */
|
||||
iRet = CalculateREFL(self,pCon,fNew);
|
||||
if(iRet != 1)
|
||||
{
|
||||
return iRet;
|
||||
}
|
||||
|
||||
/* start them all */
|
||||
/* for(i = 0; i < self->iStart; i++)
|
||||
{*/
|
||||
pDriv = self->aEngine[MOTCOZ].pMot->pDescriptor->GetInterface(
|
||||
self->aEngine[MOTCOZ].pMot,DRIVEID); /* DRIVEID ??? */
|
||||
if(pDriv != NULL)
|
||||
{
|
||||
iRet = pDriv->SetValue(self->tEngine[MOTCOZ].pMot,pCon,
|
||||
self->fTarget); /* works for one motor MOTCOZ */
|
||||
if(iRet != OKOK)
|
||||
{
|
||||
return iRet;
|
||||
}
|
||||
}
|
||||
/* } */
|
||||
return OKOK;
|
||||
}
|
||||
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static int R2THalt(void *pData)
|
||||
{
|
||||
int i, iRet;
|
||||
pIDrivable pDriv = NULL;
|
||||
pRefl2T self = (pRefl2T) pData;
|
||||
|
||||
assert(self);
|
||||
|
||||
/* stop them all */
|
||||
/*for(i = 0; i < self->iStart; i++)
|
||||
{*/
|
||||
pDriv = self->aEngine[MOTCOZ].pMot->pDescriptor->GetInterface(
|
||||
self->aEngine[MOTCOZ].pMot,DRIVEID);
|
||||
if(pDriv != NULL)
|
||||
{
|
||||
iRet = pDriv->Halt(self->aEngine[MOTCOZ].pMot);
|
||||
}
|
||||
/* } */
|
||||
return OKOK;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int R2TCheck(void *pData, float fNew, char *error, int iErrLen)
|
||||
{
|
||||
int i, iRet;
|
||||
pIDrivable pDriv = NULL;
|
||||
pRefl2T self = (pRefl2T) pData;
|
||||
SConnection *pDumCon = NULL;
|
||||
|
||||
/* Define code for this function */
|
||||
assert(self);
|
||||
pDumCon = SCCreateDummyConnection(pServ->pSics);
|
||||
assert(pDumCon);
|
||||
|
||||
/* calculation */
|
||||
iRet = CalculateREFL(self,pDumCon,fNew);
|
||||
SCDeleteConnection(pDumCon);
|
||||
if(iRet != 1)
|
||||
{
|
||||
return iRet;
|
||||
}
|
||||
|
||||
/* check them all */
|
||||
/* for(i = 0; i < self->iStart; i++)
|
||||
{*/
|
||||
pDriv = self->aEngine[MOTCOZ].pMot->pDescriptor->GetInterface(
|
||||
self->aEngine[MOTCOZ].pMot,DRIVEID);
|
||||
if(pDriv != NULL)
|
||||
{
|
||||
iRet = pDriv->CheckLimits(self->aEngine[MOTCOZ].pMot,
|
||||
self->aEngine[MOTCOZ].fTarget,
|
||||
error,iErrLen);
|
||||
if(iRet != 1)
|
||||
{
|
||||
return iRet;
|
||||
}
|
||||
}
|
||||
/* } */
|
||||
return 1;
|
||||
}
|
||||
|
||||
static float R2TGetValue(void *pData, SConnection *pCon)
|
||||
{
|
||||
|
||||
pRefl2T self = (pRefl2T) pData;
|
||||
|
||||
assert(self);
|
||||
|
||||
return fResult;
|
||||
}
|
||||
|
||||
static void *R2TGetInterface(void *pData, int iID)
|
||||
{
|
||||
pRefl2T self = (pRefl2T) pData;
|
||||
|
||||
assert(self);
|
||||
if(iID == DRIVEID)
|
||||
{
|
||||
return self->pDriv;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
static void Refl2TKill(void *pData)
|
||||
{
|
||||
pRefl2T self = (pRefl2T) pData;
|
||||
|
||||
if(self == NULL)
|
||||
return;
|
||||
|
||||
if(self->pDes)
|
||||
DeleteDescriptor(self->pDes);
|
||||
|
||||
if(self->pDriv)
|
||||
free(self->pDriv);
|
||||
|
||||
if(self->aParameter)
|
||||
ObParDelete(self->aParameter);
|
||||
|
||||
free(self);
|
||||
}
|
||||
/*--------------------------------------------------------------------------
|
||||
Initialization: All is done from the Factory function. This takes an Tcl
|
||||
array as parameter which is supposed to hold the names of all motors.
|
||||
This must fail if one of the motors cannot be accessed.
|
||||
--------------------------------------------------------------------------*/
|
||||
int Refl2TFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pRefl2T pNew;
|
||||
int i, iRet;
|
||||
char pBueffel[512];
|
||||
char *pMot = NULL;
|
||||
|
||||
if(argc < 1)
|
||||
{
|
||||
SCWrite(pCon,
|
||||
"ERROR: Insufficient number of arguments to Refl2tFactory",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* allocate space ..............*/
|
||||
pNew = (pRefl2T)malloc(sizeof(Refl2T));
|
||||
if(!pNew)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: out of memory in Refl2TFactory",eError);
|
||||
return 0;
|
||||
}
|
||||
memset(pNew,0,sizeof(Refl2T));
|
||||
pNew->pDes = CreateDescriptor("Refl2T");
|
||||
pNew->aParameter = ObParCreate(MAXPAR);
|
||||
pNew->pDriv = CreateDrivableInterface();
|
||||
if( (!pNew->pDes) || (!pNew->aParameter) || (!pNew->pDriv) )
|
||||
{
|
||||
SCWrite(pCon,"ERROR: out of memory in Refl2TFactory",eError);
|
||||
Refl2TKill(pNew);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* find the motors*/
|
||||
pMot = Tcl_GetVar2(pSics->pTcl,argv[2],"coz",TCL_GLOBAL_ONLY);
|
||||
if(!pMot)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: no value for coz motr found",eError);
|
||||
Refl2TKill(pNew);
|
||||
return 0;
|
||||
}
|
||||
pNew->aMot[MOTCOZ] = FindMotor(pSics,pMot);
|
||||
if(!pNew->aEngine[MOTCOZ])
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
Refl2TKill(pNew);
|
||||
return 0;
|
||||
}
|
||||
|
||||
pMot = Tcl_GetVar2(pSics->pTcl,argv[2],"coy",TCL_GLOBAL_ONLY);
|
||||
if(!pMot)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: no value for coy motor found",eError);
|
||||
Refl2TKill(pNew);
|
||||
return 0;
|
||||
}
|
||||
pNew->aEngine[MOTCOY] = FindMotor(pSics,pMot);
|
||||
if(!pNew->aEngine[MOTCOY])
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
Refl2TKill(pNew);
|
||||
return 0;
|
||||
}
|
||||
|
||||
pMot = Tcl_GetVar2(pSics->pTcl,argv[2],"soz",TCL_GLOBAL_ONLY);
|
||||
if(!pMot)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: no value for coz motor found",eError);
|
||||
Refl2TKill(pNew);
|
||||
return 0;
|
||||
}
|
||||
pNew->aEngine[MOTSOZ] = FindMotor(pSics,pMot);
|
||||
if(!pNew->aEngine[MOTSOZ])
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: motor %s NOT found!", pMot);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
Refl2TKill(pNew);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* initialize parameters */
|
||||
ObParInit(pNew->aParameter,PARDDD,"detectord",1400.,usMugger);
|
||||
ObParInit(pNew->aParameter,PARDHO,"heightoffset",50.,usMugger);
|
||||
ObParInit(pNew->aParameter,PARDSO,"stageoffset",50.,usMugger);
|
||||
|
||||
/* initialize interfaces */
|
||||
pNew->pDes->GetInterface = R2TGetInterface;
|
||||
pNew->pDes->SaveStatus = R2TSave;
|
||||
pNew->pDriv->Halt = R2THalt;
|
||||
pNew->pDriv->CheckLimits = R2TCheck;
|
||||
pNew->pDriv->SetValue = R2TSetValue;
|
||||
pNew->pDriv->CheckStatus = R2TStatus;
|
||||
pNew->pDriv->GetValue = R2TGetValue;
|
||||
|
||||
/* install commands */
|
||||
iRet = AddCommand(pSics,argv[1],
|
||||
Refl2TAction,Refl2TKill,pNew);
|
||||
if(!iRet)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: duplicate command %s NOT created",
|
||||
argv[1]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
Refl2TKill(pNew);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------*/
|
||||
int Refl2TAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pRefl2T self = (pRefl2T)pData;
|
||||
char pBueffel[256];
|
||||
float fVal;
|
||||
double dVal;
|
||||
ObPar *pPar = NULL;
|
||||
int iRet;
|
||||
|
||||
assert(self);
|
||||
|
||||
if(argc > 1)
|
||||
{
|
||||
strtolower(argv[1]);
|
||||
/* deal with list */
|
||||
if(strcmp(argv[1],"list") == 0)
|
||||
{
|
||||
R2TList(self,pCon,argv[0]);
|
||||
return 1;
|
||||
}
|
||||
/* otherwise it should be a parameter command */
|
||||
if(argc >= 3)
|
||||
{
|
||||
iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dVal);
|
||||
if(iRet != TCL_OK)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: failed to convert %s to number",
|
||||
argv[2]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
iRet = ObParSet(self->aParameter,argv[0],argv[1],(float)dVal,pCon);
|
||||
if(iRet)
|
||||
{
|
||||
SCSendOK(pCon);
|
||||
}
|
||||
return iRet;
|
||||
}
|
||||
else /* argc = 2 */
|
||||
{
|
||||
pPar = ObParFind(self->aParameter,argv[1]);
|
||||
if(!pPar)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: parameter %s NOT found",argv[1]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
sprintf(pBueffel,"%s.%s = %f",argv[0],pPar->name, pPar->fVal);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
fVal = self->pDriv->GetValue(self,pCon);
|
||||
sprintf(pBueffel," %s = %f", argv[0], fVal);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
static void R2TList(pRefl2T self, SConnection *pCon, char *name)
|
||||
{
|
||||
char pBueffel[132];
|
||||
Tcl_DString tString;
|
||||
|
||||
assert(pCon);
|
||||
assert(self);
|
||||
|
||||
Tcl_DStringInit(&tString);
|
||||
sprintf(pBueffel,
|
||||
"%s.detectord %f \n", name, ObVal(self->aParameter,PARDDD));
|
||||
Tcl_DStringInit(&tString);
|
||||
sprintf(pBueffel,
|
||||
"%s.heightoffset %f \n", name, ObVal(self->aParameter,PARDHO));
|
||||
Tcl_DStringInit(&tString);
|
||||
sprintf(pBueffel,
|
||||
"%s.sampleoffset %f \n", name, ObVal(self->aParameter,PARDSO));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user