404 lines
11 KiB
C
404 lines
11 KiB
C
/*---------------------------------------------------------------------------
|
|
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));
|
|
}
|
|
|