383 lines
9.8 KiB
C
383 lines
9.8 KiB
C
/*----------------------------------------------------------------------------
|
|
S A N S W A V E
|
|
|
|
Wavelength calculation for neutron velocity selector. Implements a
|
|
driveable interface.
|
|
|
|
copyright: see copyright.h
|
|
|
|
Mark Koennecke, October 1998
|
|
---------------------------------------------------------------------------*/
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include <tcl.h>
|
|
#include "fortify.h"
|
|
#include "sics.h"
|
|
#include "motor.h"
|
|
#include "velo.h"
|
|
#include "sanswave.h"
|
|
#include "sicshipadaba.h"
|
|
|
|
#define NOTILT 801
|
|
|
|
static int SWGetTilt(void *data, float *fTilt)
|
|
{
|
|
pDummy pDum = (pDummy)data;
|
|
pHdb node = NULL;
|
|
|
|
if(strcmp(pDum->pDescriptor->name,"VelocitySelector") == 0) {
|
|
return VSGetTilt((pVelSel)data, fTilt);
|
|
} else if(strcmp(pDum->pDescriptor->name,"NVS") == 0){
|
|
node = GetHipadabaNode(pDum->pDescriptor->parNode,"tilt");
|
|
assert(node != NULL);
|
|
*fTilt = (float)node->value.v.doubleValue;
|
|
return 1;
|
|
} else {
|
|
assert(0);
|
|
}
|
|
return 0;
|
|
}
|
|
/*-----------------------------------------------------------------------*/
|
|
typedef struct __SANSwave {
|
|
pObjectDescriptor pDes;
|
|
pIDrivable pDrivInt;
|
|
void *pSelector;
|
|
} SANSWave;
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
static void CalculateCoefficients(float fTilt, float *fA, float *fB)
|
|
{
|
|
float fSQ, fTer, fQuat;
|
|
|
|
fSQ = fTilt * fTilt;
|
|
fTer = fSQ * fTilt;
|
|
fQuat = fTer * fTilt;
|
|
|
|
*fA = 0.01223 + (0.000360495 * fTilt) + (0.000313819 * fSQ) +
|
|
(0.0000304937 * fTer) + (0.000000931533 * fQuat);
|
|
|
|
*fB = 12721.11905 - (611.74127 * fTilt) - (12.44417 * fSQ) -
|
|
(0.12411 * fTer) + (0.00583 * fQuat);
|
|
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
int CalculateLambda(float fRot, float fTilt, float *fLambda)
|
|
{
|
|
float fA, fB;
|
|
|
|
if (fRot < 10.0) {
|
|
*fLambda = 0.0;
|
|
return 1;
|
|
}
|
|
|
|
CalculateCoefficients(fTilt, &fA, &fB);
|
|
|
|
*fLambda = fA + fB / fRot;
|
|
return 1;
|
|
}
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
static int SWHalt(void *pData)
|
|
{
|
|
pSANSWave self = NULL;
|
|
pIDrivable pDriv = NULL;
|
|
|
|
self = (pSANSWave) pData;
|
|
assert(self);
|
|
|
|
pDriv = GetDrivableInterface(self->pSelector);
|
|
assert(pDriv);
|
|
|
|
return pDriv->Halt(self->pSelector);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------*/
|
|
static int SWLimits(void *pData, float fVal, char *error, int iErrLen)
|
|
{
|
|
pSANSWave self = NULL;
|
|
pIDrivable pDriv = NULL;
|
|
int iRet;
|
|
float fTilt, fA, fB, fRot;
|
|
|
|
self = (pSANSWave) pData;
|
|
assert(self);
|
|
|
|
pDriv = GetDrivableInterface(self->pSelector);
|
|
assert(pDriv);
|
|
|
|
/* get tilt */
|
|
iRet = SWGetTilt(self->pSelector, &fTilt);
|
|
if (!iRet) {
|
|
strlcpy(error, "Failed to obtain tilt angle", iErrLen);
|
|
return 0;
|
|
}
|
|
|
|
/* Calculate rotation speed */
|
|
CalculateCoefficients(fTilt, &fA, &fB);
|
|
fRot = fB / (fVal - fA);
|
|
return pDriv->CheckLimits(self->pSelector, fRot, error, iErrLen);
|
|
}
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
static long SWSet(void *pData, SConnection * pCon, float fVal)
|
|
{
|
|
pSANSWave self = NULL;
|
|
pIDrivable pDriv = NULL;
|
|
int iRet, i;
|
|
float fTilt, fA, fB, fRot;
|
|
pDummy pDum = NULL;
|
|
|
|
self = (pSANSWave) pData;
|
|
assert(self);
|
|
|
|
pDriv = GetDrivableInterface(self->pSelector);
|
|
assert(pDriv);
|
|
|
|
/* get tilt */
|
|
fTilt = -910;
|
|
for (i = 0; i < 3; i++) {
|
|
iRet = SWGetTilt(self->pSelector, &fTilt);
|
|
if (iRet) {
|
|
break;
|
|
} else {
|
|
SCWrite(pCon, "WARNING: trouble reading tilt angle", eWarning);
|
|
}
|
|
}
|
|
if (fTilt < -900) { /* failed to get tilt */
|
|
SCWrite(pCon, "ERROR: failed to read tilt angle 3 times", eError);
|
|
return 0;
|
|
}
|
|
|
|
/* Calculate rotation speed */
|
|
CalculateCoefficients(fTilt, &fA, &fB);
|
|
fRot = fB / (fVal - fA);
|
|
return pDriv->SetValue(self->pSelector, pCon, fRot);
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*/
|
|
static int SWStatus(void *pData, SConnection * pCon)
|
|
{
|
|
pSANSWave self = NULL;
|
|
pIDrivable pDriv = NULL;
|
|
|
|
self = (pSANSWave) pData;
|
|
assert(self);
|
|
|
|
pDriv = GetDrivableInterface(self->pSelector);
|
|
assert(pDriv);
|
|
|
|
return pDriv->CheckStatus(self->pSelector, pCon);
|
|
}
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
static float SWGetValue(void *pData, SConnection * pCon)
|
|
{
|
|
pSANSWave self = NULL;
|
|
pIDrivable pDriv = NULL;
|
|
float fRot, fTilt, fA, fB, fLambda;
|
|
int i, iRet;
|
|
|
|
self = (pSANSWave) pData;
|
|
assert(self);
|
|
|
|
pDriv = GetDrivableInterface(self->pSelector);
|
|
assert(pDriv);
|
|
|
|
/* get tilt */
|
|
fTilt = -910;
|
|
for (i = 0; i < 3; i++) {
|
|
iRet = SWGetTilt(self->pSelector, &fTilt);
|
|
if (iRet) {
|
|
break;
|
|
} else {
|
|
SCWrite(pCon, "WARNING: trouble reading tilt angle", eWarning);
|
|
}
|
|
}
|
|
if (fTilt < -900) { /* failed to get tilt */
|
|
SCWrite(pCon, "ERROR: failed to read tilt angle 3 times", eError);
|
|
return -99999.99;
|
|
}
|
|
|
|
/* get rotation speed */
|
|
fRot = pDriv->GetValue(self->pSelector, pCon);
|
|
if (fRot < -9999.) {
|
|
SCWrite(pCon, "ERROR: cannot reading rotation speed", eError);
|
|
return -99999.99;
|
|
}
|
|
|
|
CalculateLambda(fRot, fTilt, &fLambda);
|
|
return fLambda;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
static void *SWGetInterface(void *pData, int iID)
|
|
{
|
|
pSANSWave self = NULL;
|
|
|
|
self = (pSANSWave) pData;
|
|
assert(self);
|
|
|
|
if (iID == DRIVEID) {
|
|
return self->pDrivInt;
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
static void KillSANSWave(void *pData)
|
|
{
|
|
pSANSWave self = NULL;
|
|
|
|
self = (pSANSWave) pData;
|
|
if (!self)
|
|
return;
|
|
|
|
if (self->pDes)
|
|
DeleteDescriptor(self->pDes);
|
|
|
|
if (self->pDrivInt)
|
|
free(self->pDrivInt);
|
|
|
|
free(self);
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------
|
|
arguments: name, name of velocity selctor
|
|
*/
|
|
int MakeSANSWave(SConnection * pCon, SicsInterp * pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
pSANSWave pNew = NULL;
|
|
CommandList *pCom = NULL;
|
|
char pBueffel[512];
|
|
int iRet;
|
|
pDummy pDum = NULL;
|
|
|
|
assert(pCon);
|
|
assert(pSics);
|
|
|
|
/* enough arguments ? */
|
|
if (argc < 3) {
|
|
SCWrite(pCon, "ERROR: not enough arguments to MakeSANSWave", eError);
|
|
return 0;
|
|
}
|
|
|
|
/* allocate space */
|
|
pNew = (pSANSWave) malloc(sizeof(SANSWave));
|
|
if (!pNew) {
|
|
SCWrite(pCon, "ERROR: out of memory in MakeSANSWave", eError);
|
|
return 0;
|
|
}
|
|
memset(pNew, 0, sizeof(SANSWave));
|
|
|
|
|
|
/* the last arument must denote a velocity selector */
|
|
pCom = FindCommand(pSics, argv[2]);
|
|
if (!pCom) {
|
|
snprintf(pBueffel,511, "ERROR: velocity selector %s NOT found", argv[2]);
|
|
SCWrite(pCon, pBueffel, eError);
|
|
KillSANSWave(pNew);
|
|
return 0;
|
|
}
|
|
pNew->pSelector = pCom->pData;
|
|
if (!pNew->pSelector) {
|
|
snprintf(pBueffel,511, "ERROR: velocity selector %s is invalid", argv[2]);
|
|
SCWrite(pCon, pBueffel, eError);
|
|
KillSANSWave(pNew);
|
|
return 0;
|
|
}
|
|
pDum = (pDummy) pNew->pSelector;
|
|
if (strcmp(pDum->pDescriptor->name, "VelocitySelector") != 0
|
|
&& strcmp(pDum->pDescriptor->name,"NVS") != 0) {
|
|
snprintf(pBueffel,511, "ERROR: velocity selector %s is invalid", argv[2]);
|
|
SCWrite(pCon, pBueffel, eError);
|
|
KillSANSWave(pNew);
|
|
return 0;
|
|
}
|
|
|
|
/* initialise the rest of the data structure */
|
|
pNew->pDes = CreateDescriptor("SANSWave");
|
|
pNew->pDrivInt = CreateDrivableInterface();
|
|
if ((!pNew->pDes) || (!pNew->pDrivInt)) {
|
|
SCWrite(pCon, "ERROR: out of memory in MakeSANSWave", eError);
|
|
KillSANSWave(pNew);
|
|
return 0;
|
|
}
|
|
pNew->pDes->GetInterface = SWGetInterface;
|
|
|
|
pNew->pDrivInt->Halt = SWHalt;
|
|
pNew->pDrivInt->CheckLimits = SWLimits;
|
|
pNew->pDrivInt->SetValue = SWSet;
|
|
pNew->pDrivInt->CheckStatus = SWStatus;
|
|
pNew->pDrivInt->GetValue = SWGetValue;
|
|
|
|
/* install command */
|
|
iRet = AddCommand(pSics, argv[1], SANSWaveAction, KillSANSWave, pNew);
|
|
if (!iRet) {
|
|
snprintf(pBueffel,511, "ERROR: duplicate command %s NOT created", argv[1]);
|
|
SCWrite(pCon, pBueffel, eError);
|
|
KillSANSWave(pNew);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
int SANSWaveAction(SConnection * pCon, SicsInterp * pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
pSANSWave self = NULL;
|
|
float fLambda, fTilt, fRot, fA, fB;
|
|
double dNum;
|
|
int iRet;
|
|
char pBueffel[256];
|
|
|
|
self = (pSANSWave) pData;
|
|
assert(self);
|
|
assert(pCon);
|
|
assert(pSics);
|
|
|
|
if (argc > 2) {
|
|
strtolower(argv[1]);
|
|
/* whatever we are asked to do, we need the current tilt angle */
|
|
iRet = SWGetTilt(self->pSelector, &fTilt);
|
|
if (!iRet) {
|
|
SCWrite(pCon, "ERROR: failed to read tilt angle", eError);
|
|
return 0;
|
|
}
|
|
/* the second argument must be a number */
|
|
iRet = Tcl_GetDouble(pSics->pTcl, argv[2], &dNum);
|
|
if (iRet != TCL_OK) {
|
|
snprintf(pBueffel,255, "ERROR: cannot convert %s to number", argv[2]);
|
|
SCWrite(pCon, pBueffel, eError);
|
|
return 0;
|
|
}
|
|
if (strcmp(argv[1], "rot") == 0) { /* calculate rotation for wl */
|
|
CalculateCoefficients(fTilt, &fA, &fB);
|
|
fRot = fB / (dNum - fA);
|
|
sprintf(pBueffel, "%f nm = %f RPM", (float) dNum, fRot);
|
|
SCWrite(pCon, pBueffel, eValue);
|
|
return 1;
|
|
} else if (strcmp(argv[1], "wl") == 0) { /* calculate wl from rot */
|
|
CalculateLambda((float) dNum, fTilt, &fLambda);
|
|
sprintf(pBueffel, "%f RPM = %f nm", (float) dNum, fLambda);
|
|
SCWrite(pCon, pBueffel, eValue);
|
|
return 1;
|
|
} else {
|
|
snprintf(pBueffel,255, "ERROR: subcommand %s NOT understood", argv[1]);
|
|
SCWrite(pCon, pBueffel, eError);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
fLambda = self->pDrivInt->GetValue(self, pCon);
|
|
if (fLambda < -90000) {
|
|
return 0;
|
|
}
|
|
|
|
sprintf(pBueffel, "%s = %f", argv[0], fLambda);
|
|
SCWrite(pCon, pBueffel, eValue);
|
|
return 1;
|
|
}
|