sics/selector.c

/*--------------------------------------------------------------------------

			S E L E C T O R
			
	This is the implementation file for the the code necessary to deal
	with an energy selector, monochromator. This is the Crystal
	variety.	
	

	Mark Koennecke,    January 1997

	Copyright:

	Labor fuer Neutronenstreuung
	Paul Scherrer Institut
	CH-5423 Villigen-PSI


        The authors hereby grant permission to use, copy, modify, distribute,
        and license this software and its documentation for any purpose, provided
        that existing copyright notices are retained in all copies and that this
        notice is included verbatim in any distributions. No written agreement,
        license, or royalty fee is required for any of the authorized uses.
        Modifications to this software may be copyrighted by their authors
        and need not follow the licensing terms described here, provided that
        the new terms are clearly indicated on the first page of each file where
        they apply.

        IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY
        FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
        ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY
        DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE
        POSSIBILITY OF SUCH DAMAGE.

        THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES,
        INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
        FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT.  THIS SOFTWARE
        IS PROVIDED ON AN "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE
        NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
        MODIFICATIONS.
----------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <math.h>
#include "fortify.h"
#include "sics.h"
#include "motor.h"
#include "splitter.h"
#include "devexec.h"
#include "selector.h"

#define SS 0
#define B1C1 1
#define B1C2 2
#define B1MIN 3
#define B1MAX 4
#define B2C1 5
#define B2C2 6
#define B2MIN 7
#define B2MAX 8
#define LATD  9
#define RIGHTS 10
/*------------------------------------------------------------------------*/
typedef struct __SicsSelector {
  ObjectDescriptor *pDes;
  ObPar *pParams;
  char *name;
  pMotor pTheta;
  pMotor pTwoTheta;
  pMotor pBend1;
  pMotor pBend2;
  char *pType;
} SicsSelector;
/*-------------------------------------------------------------------------*/
char *MonoGetType(pSicsSelector self)
{
  assert(self);
  return self->pType;
}

/*---------------------------------------------------------------------------*/
pSicsSelector CreateSelector(char *name, pMotor pTheta, pMotor pTwoTheta,
                             pMotor pBend1, pMotor pBend2)
{
  pSicsSelector pRes = NULL;
  int iRet;
  float fVal;

  assert(pTheta);
  assert(pTwoTheta);

  /* allocate memory */
  pRes = (pSicsSelector) malloc(sizeof(SicsSelector));
  if (!pRes) {
    return NULL;
  }

  /* create ObjectDescriptor */
  pRes->pDes = CreateDescriptor("CrystalSelector");
  if (!pRes->pDes) {
    free(pRes);
    return NULL;
  }

  /* create Parameter Array */
  pRes->pParams = ObParCreate(11);
  if (!pRes->pParams) {
    free(pRes->pDes);
    free(pRes);
    return NULL;
  }

  /* create all the parameters */
  ObParInit(pRes->pParams, SS, "ss", 1., usUser);
  ObParInit(pRes->pParams, B1C1, "vk1", 1., usInternal);
  ObParInit(pRes->pParams, B1C2, "vk2", 1., usInternal);
  ObParInit(pRes->pParams, B2C1, "hk1", 1., usInternal);
  ObParInit(pRes->pParams, B2C2, "hk2", 1., usInternal);
  ObParInit(pRes->pParams, LATD, "dd", 2.087, usMugger);
  ObParInit(pRes->pParams, RIGHTS, "access", usUser, usMugger);

  /* assign motors */
  pRes->pTheta = pTheta;
  pRes->pTwoTheta = pTwoTheta;

  /* provide default values for Bender Parameters in order to make
     things look nice
   */
  fVal = 0.;
  ObParInit(pRes->pParams, B1MIN, "vmin", fVal, usInternal);
  ObParInit(pRes->pParams, B1MAX, "vmax", fVal, usInternal);
  ObParInit(pRes->pParams, B2MIN, "hmin", fVal, usInternal);
  ObParInit(pRes->pParams, B2MAX, "hmax", fVal, usInternal);


  /* handle benders, if present */
  pRes->pBend1 = pBend1;
  if (pBend1) {
    iRet = MotorGetPar(pBend1, "hardlowerlim", &fVal);
    assert(iRet);
    ObParInit(pRes->pParams, B1MIN, "vmin", fVal, usInternal);
    iRet = MotorGetPar(pBend1, "hardupperlim", &fVal);
    assert(iRet);
    ObParInit(pRes->pParams, B1MAX, "vmax", fVal, usInternal);
  }
  pRes->pBend2 = pBend2;
  if (pBend2) {
    iRet = MotorGetPar(pBend2, "hardlowerlim", &fVal);
    assert(iRet);
    ObParInit(pRes->pParams, B2MIN, "hmin", fVal, usInternal);
    iRet = MotorGetPar(pBend2, "hardupperlim", &fVal);
    assert(iRet);
    ObParInit(pRes->pParams, B2MAX, "hmax", fVal, usInternal);
  }
  pRes->name = strdup(name);
  pRes->pType = strdup("Unknown");
  return pRes;
}

/*---------------------------------------------------------------------------*/
void DeleteSelector(void *self)
{
  pSicsSelector pSelf;

  assert(self);

  pSelf = (pSicsSelector) self;

  if (pSelf->pDes) {
    DeleteDescriptor(pSelf->pDes);
  }
  if (pSelf->pParams) {
    ObParDelete(pSelf->pParams);
  }
  if (pSelf->name) {
    free(pSelf->name);
  }
  if (pSelf->pType) {
    free(pSelf->pType);
  }
  free(pSelf);
}

/*--------------------------------------------------------------------------
  Syntax:
     MonoInit name Type ThetaMotor TwoThetaMotor Bend1Motor Bend2Motor 
  with Bend1Motor, Bend2Motor beeing optional.
*/

int MonoInit(SConnection * pCon, SicsInterp * pSics, void *pData,
             int argc, char *argv[])
{
  pSicsSelector pRes = NULL;
  pMotor pTheta = NULL;
  pMotor pTwoTheta = NULL;
  pMotor pBend1 = NULL;
  pMotor pBend2 = NULL;
  TokenList *pList = NULL;
  int iRet = 0;
  TokenList *pCurrent;
  char pBueffel[132];
  char pName[132];
  char *pType = NULL;

  assert(pCon);
  assert(pSics);

  /* split arguments */
  argtolower(argc, argv);
  pList = SplitArguments(argc, argv);
  if (!pList) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: parsing arguments in %s", argv[0]);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* advance and search name */
  pCurrent = pList->pNext;
  if (!pCurrent) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERRROR: Insufficient number of arguments to %s",
            argv[0]);
    SCWrite(pCon, pBueffel, eError);
    goto end;
  }
  strcpy(pName, pCurrent->text);

  /* advance and find Type string */
  pCurrent = pCurrent->pNext;
  if (!pCurrent) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERRROR: Insufficient number of arguments to %s",
            argv[0]);
    SCWrite(pCon, pBueffel, eError);
    goto end;
  }
  pType = pCurrent->text;

  /* advance and find Theta motor */
  pCurrent = pCurrent->pNext;
  if (!pCurrent) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERRROR: Insufficient number of arguments to %s",
            argv[0]);
    SCWrite(pCon, pBueffel, eError);
    goto end;
  }
  pTheta = FindMotor(pSics, pCurrent->text);
  if (!pTheta) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: Cannot find motor %s for driving Theta",
            pCurrent->text);
    SCWrite(pCon, pBueffel, eError);
    goto end;
  }

  /* advance and find Two Theta motor */
  pCurrent = pCurrent->pNext;
  if (!pCurrent) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERRROR: Insufficient number of arguments to %s",
            argv[0]);
    SCWrite(pCon, pBueffel, eError);
    goto end;
  }
  pTwoTheta = FindMotor(pSics, pCurrent->text);
  if (!pTwoTheta) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: Cannot find motor %s for driving Two Theta",
            pCurrent->text);
    SCWrite(pCon, pBueffel, eError);
    goto end;
  }
  iRet = 1;                     /* we are now able to install a monochromator */

  /* try find first bending motor */
  pCurrent = pCurrent->pNext;
  if (pCurrent) {
    pBend1 = FindMotor(pSics, pCurrent->text);
    if (!pBend1) {
      snprintf(pBueffel,sizeof(pBueffel)-1,
              "ERROR: Cannot find motor %s for driving vertical bender",
              pCurrent->text);
      SCWrite(pCon, pBueffel, eError);
      goto end;
    }
  } else {
    goto end;
  }

  /* find second bender */
  pCurrent = pCurrent->pNext;
  if (pCurrent) {
    pBend2 = FindMotor(pSics, pCurrent->text);
    if (!pBend2) {
      snprintf(pBueffel,sizeof(pBueffel)-1,
              "ERROR: Cannot find motor %s for driving horizontal bender",
              pCurrent->text);
      SCWrite(pCon, pBueffel, eError);
      goto end;
    }
  }

end:
  if (iRet) {
    pRes = CreateSelector(pName, pTheta, pTwoTheta, pBend1, pBend2);
    if (!pRes) {
      iRet = 0;
    } else {
      if (pRes->pType) {
        free(pRes->pType);
      }
      pRes->pType = strdup(pType);
      iRet =
          AddCommand(pSics, pName, MonoAction, DeleteSelector,
                     (void *) pRes);
      if (!iRet) {
        snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: duplicate command %s not created",
                argv[2]);
        SCWrite(pCon, pBueffel, eError);
        DeleteTokenList(pList);
        DeleteSelector((void *) pRes);
        return 0;
      }
    }
  }

  DeleteTokenList(pList);
  return iRet;

}

/*---------------------------------------------------------------------------
  a private function used in MonoAction to print all relevant features of 
  the monochromator
*/
static void MonoList(pSicsSelector self, SConnection * pCon)
{
  char pBueffel[512];
  int i, iLen;

  /* print known parameters */
  iLen = ObParLength(self->pParams);
  snprintf(pBueffel,sizeof(pBueffel)-1, "Parameter Listing for  %s\n", self->name);
  SCWrite(pCon, pBueffel, eValue);
  for (i = 0; i < iLen; i++) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "%s.%s = %f\n", self->name,
            self->pParams[i].name, self->pParams[i].fVal);
    SCWrite(pCon, pBueffel, eValue);
  }

  /* print motornames as well */
  snprintf(pBueffel,sizeof(pBueffel)-1, "%s.ThetaMotor = %s\n", self->name,
          self->pTheta->name);
  SCWrite(pCon, pBueffel, eValue);
  snprintf(pBueffel,sizeof(pBueffel)-1, "%s.TwoThetaMotor = %s\n", self->name,
          self->pTwoTheta->name);
  SCWrite(pCon, pBueffel, eValue);
  if (self->pBend1) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "%s.VerticalBenderMotor = %s\n", self->name,
            self->pBend1->name);
    SCWrite(pCon, pBueffel, eValue);
  }
  if (self->pBend2) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "%s.HorizontalBenderMotor = %s\n", self->name,
            self->pBend2->name);
    SCWrite(pCon, pBueffel, eValue);
  }
}

/*-----------------------------------------------------------------------------
 The Action function. Syntax:
    mononame list           -- list all pars 
    mononame parname        -- prints parameter 
    mononame parname value  -- tries changing the parameter 
*/

int MonoAction(SConnection * pCon, SicsInterp * pSics, void *pData,
               int argc, char *argv[])
{
  pSicsSelector pSelf = NULL;
  TokenList *pList = NULL;
  TokenList *pCurrent;
  int iRet;
  char pBueffel[132];
  char pName[132];
  float fVal;
  ObPar *pPar = NULL;

  assert(pCon);
  assert(pSics);
  assert(pData);

  pSelf = (pSicsSelector) pData;

  /* split arguments */
  argtolower(argc, argv);
  pList = SplitArguments(argc, argv);
  if (!pList) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: parsing arguments in %s", argv[0]);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  pCurrent = pList->pNext;
  /* now we can have "list" or a parametername */
  /* check for list first */
  if (!pCurrent) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: Insufficient number of arguments to %s",
            argv[0]);
    SCWrite(pCon, pBueffel, eError);
    goto end;
  }
  if (strcmp(pCurrent->text, "list") == 0) {
    MonoList(pSelf, pCon);
    iRet = 1;
    goto end;
  }

  /* must be parametername now */
  strcpy(pName, pCurrent->text);

  /* find out if it is a set or a get. On set there is another paremeter,
     else none 
   */
  pCurrent = pCurrent->pNext;
  if (pCurrent) {               /* set */
    /* check if input is proper */
    if (pCurrent->Type == eFloat) {
      fVal = pCurrent->fVal;
    } else if (pCurrent->Type == eInt) {
      fVal = (float) pCurrent->iVal;
    } else {
      snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: Illegal parameter %s given to %s",
              pCurrent->text, argv[0]);
      SCWrite(pCon, pBueffel, eError);
      goto end;
    }
    iRet = ObParSet(pSelf->pParams, argv[0], pName, fVal, pCon);
    goto end;
  } else {                      /* get */

    pPar = ObParFind(pSelf->pParams, pName);
    if (!pPar) {
      snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: Parameter %s not found in %s", pName,
              argv[0]);
      SCWrite(pCon, pBueffel, eError);
      goto end;
    } else {
      snprintf(pBueffel,sizeof(pBueffel)-1, "%s.%s = %f", argv[0], pName, pPar->fVal);
      SCWrite(pCon, pBueffel, eValue);
      iRet = 1;
      DeleteTokenList(pList);
      return 1;
    }
  }
end:
  DeleteTokenList(pList);
  if (iRet)
    SCSendOK(pCon);
  return iRet;
}

/*--------------------------------------------------------------------------*/
struct SelPos {
  float fTheta, fTwoTheta, fVert, fHor;
};
#define PI 3.14159265358979323846264338327950
#define RD 57.29577951308232087679815481410517
/* ------------------- C has no proper abs -------------------------------*/
static double absd(double f)
{
  if (f < .0) {
    f = -f;
  }
  return f;
}

/*-------------------------------------------------------------------------*/
static struct SelPos CalculatePosition(pSicsSelector self,
                                       float fWaveLength)
{
  struct SelPos pRes;
  double fD;

  /* Theta, TwoTheta first */
  fD = fWaveLength / (2.0 * ObVal(self->pParams, LATD));
  if (fD > 1.0) {
    pRes.fTheta = 1000.;        /* error indication: energy to big */
    return pRes;
  }
  fD = asin(fD);
  pRes.fTheta = fD * ObVal(self->pParams, SS) * RD;
  pRes.fTwoTheta = 2 * pRes.fTheta;

  /* now first bender */
  if (self->pBend1) {
    pRes.fVert = ObVal(self->pParams, B1C1) + (ObVal(self->pParams, B1C2) /
                                               sin(fD));
  }


  /* now second bender */
  if (self->pBend2) {
    pRes.fHor = ObVal(self->pParams, B2C1) + (ObVal(self->pParams, B2C2) /
                                              sin(fD));
  }
  return pRes;
}

/*--------------------------------------------------------------------------*/
int MonoLimits(pSicsSelector self, float fWaveLength,
               char *error, int iErrLen)
{
  struct SelPos sNeu;
  char pBueffel[132];
  pIDrivable pDrivInt = NULL;

  /* get Position */
  sNeu = CalculatePosition(self, fWaveLength);
  if (sNeu.fTheta > 900.) {     /* invalid wavelength or energy */
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: Invalid wavelength or energy to high: %f",
            fWaveLength);
    strlcpy(error, pBueffel, iErrLen - 1);
    return 0;
  }

  /* check each motor in turn */
  pDrivInt =
      self->pTheta->pDescriptor->GetInterface(self->pTheta, DRIVEID);
  if (!pDrivInt->CheckLimits(self->pTheta, sNeu.fTheta, error, iErrLen)) {
    return 0;
  }
  pDrivInt = self->pTwoTheta->pDescriptor->GetInterface(self->pTwoTheta,
                                                        DRIVEID);
  if (!pDrivInt->CheckLimits(self->pTwoTheta, sNeu.fTwoTheta,
                             error, iErrLen)) {
    return 0;
  }

  if (self->pBend1) {
    pDrivInt =
        self->pBend1->pDescriptor->GetInterface(self->pBend1, DRIVEID);
    if (!pDrivInt->CheckLimits(self->pBend1, sNeu.fVert, error, iErrLen)) {
      return 0;
    }
  }
  if (self->pBend2) {
    pDrivInt =
        self->pBend2->pDescriptor->GetInterface(self->pBend2, DRIVEID);
    if (!pDrivInt->CheckLimits(self->pBend2, sNeu.fHor, error, iErrLen)) {
      return 0;
    }
  }
  return 1;
}

/*--------------------------------------------------------------------------*/
int MonoHalt(pSicsSelector self)
{
  pIDrivable pDrivInt = NULL;

  /* halt each motor in turn */
  pDrivInt =
      self->pTheta->pDescriptor->GetInterface(self->pTheta, DRIVEID);
  if (pDrivInt) {
    pDrivInt->Halt(self->pTheta);
  }

  pDrivInt = self->pTwoTheta->pDescriptor->GetInterface(self->pTwoTheta,
                                                        DRIVEID);
  if (pDrivInt) {
    pDrivInt->Halt(self->pTwoTheta);
  }

  if (self->pBend1) {
    pDrivInt = self->pBend1->pDescriptor->GetInterface(self->pBend1,
                                                       DRIVEID);
    if (pDrivInt) {
      pDrivInt->Halt(self->pBend1);
    }
  }
  if (self->pBend2) {
    pDrivInt = self->pBend2->pDescriptor->GetInterface(self->pBend2,
                                                       DRIVEID);
    if (pDrivInt) {
      pDrivInt->Halt(self->pBend2);
    }
  }


  return 1;
}

/*-------------------------------------------------------------------------*/
int MonoRun(pSicsSelector self, SConnection * pCon, float fWaveLength)
{
  struct SelPos sNeu;
  char pBueffel[132];
  int iRet;

  /* Check authorisation */
  if (!SCMatchRights(pCon, (int) ObVal(self->pParams, RIGHTS))) {
    snprintf(pBueffel,sizeof(pBueffel)-1,
            "ERROR: You are not authorised to move the monochromator %s",
            self->name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* get Position */
  sNeu = CalculatePosition(self, fWaveLength);
  if (sNeu.fTheta > 900.) {     /* invalid wavelength or energy */
    snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: Invalid wavelength or energy to high: %f",
            fWaveLength);
    return 0;
  }

  /* start each motor in turn */
  iRet = StartDevice(GetExecutor(), self->pTheta->name,
                     self->pTheta->pDescriptor,
                     self->pTheta, pCon, pCon->runLevel, sNeu.fTheta);
  if (!iRet) {
    return 0;
  }
  iRet = StartDevice(GetExecutor(), self->pTwoTheta->name,
                     self->pTwoTheta->pDescriptor,
                     self->pTwoTheta, pCon, pCon->runLevel, sNeu.fTwoTheta);
  if (!iRet) {
    return 0;
  }


  /* bending motors */
  if (self->pBend1) {
    iRet = StartDevice(GetExecutor(), self->pBend1->name,
                       self->pBend1->pDescriptor,
                       self->pBend1, pCon, pCon->runLevel, sNeu.fVert);
    if (!iRet) {
      return 0;
    }

  }
  if (self->pBend2) {
    iRet = StartDevice(GetExecutor(), self->pBend2->name,
                       self->pBend2->pDescriptor,
                       self->pBend2, pCon, pCon->runLevel, sNeu.fHor);
    if (!iRet) {
      return 0;
    }
  }
  return OKOK;
}

/*--------------------------------------------------------------------------*/
int MonoCheck(pSicsSelector self, SConnection * pCon)
{
  int iRet;
  pIDrivable pDrivInt = NULL;

  /* cheack each motor in turn */
  pDrivInt =
      self->pTheta->pDescriptor->GetInterface(self->pTheta, DRIVEID);
  if (pDrivInt) {
    iRet = pDrivInt->CheckStatus(self->pTheta, pCon);
    if ((iRet != OKOK) && (iRet != HWIdle)) {
      return iRet;
    }
  }

  pDrivInt = self->pTwoTheta->pDescriptor->GetInterface(self->pTwoTheta,
                                                        DRIVEID);
  if (pDrivInt) {
    iRet = pDrivInt->CheckStatus(self->pTwoTheta, pCon);
    if ((iRet != OKOK) && (iRet != HWIdle)) {
      return iRet;
    }
  }

  if (self->pBend1) {
    pDrivInt = self->pBend1->pDescriptor->GetInterface(self->pBend1,
                                                       DRIVEID);
    if (pDrivInt) {
      iRet = pDrivInt->CheckStatus(self->pBend1, pCon);
      if ((iRet != OKOK) && (iRet != HWIdle)) {
        return iRet;
      }
    }
  }

  if (self->pBend2) {
    pDrivInt = self->pBend2->pDescriptor->GetInterface(self->pBend2,
                                                       DRIVEID);
    if (pDrivInt) {
      iRet = pDrivInt->CheckStatus(self->pBend2, pCon);
      if ((iRet != OKOK) && (iRet != HWIdle)) {
        return iRet;
      }
    }
  }


  return HWIdle;
}

/*--------------------------------------------------------------------------
  returns the current Wavelength the monochromator is adjusted to
*/
float GetMonoPosition(pSicsSelector self, SConnection * pCon)
{
  double fVal, dTheta;
  float fTheta, fTwoTheta;
  char pBueffel[132];
  int iRet;

  /* get the two positions */
  iRet = MotorGetSoftPosition(self->pTheta, pCon, &fTheta);
  if (!iRet) {
    snprintf(pBueffel,sizeof(pBueffel)-1,
            "ERROR: cannot read Theta motor for monochromator %s\n",
            self->name);
    SCWrite(pCon, pBueffel, eError);
    return 0.;
  }
  iRet = MotorGetSoftPosition(self->pTwoTheta, pCon, &fTwoTheta);
  if (!iRet) {
    snprintf(pBueffel,sizeof(pBueffel)-1,
            "ERROR: cannot read TwoTheta motor for monochromator %s\n",
            self->name);
    SCWrite(pCon, pBueffel, eError);
    return 0.;
  }

  /* check for sync */
  fVal = fTwoTheta - 2 * fTheta;
  if (fVal < 0.)
    fVal = -fVal;
  if (fVal > 0.01) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "WARNING: monochromator %s out of sync by %f\n",
            self->name, fVal);
    SCWrite(pCon, pBueffel, eWarning);
  }

  /* calculate wavelength from angles */
  dTheta = (double) fTheta;
  dTheta = dTheta * (1.0 / RD);
  fVal = 2.0 * (double) ObVal(self->pParams, LATD);
  fVal = fVal * sin(dTheta) * ObVal(self->pParams, SS);
  return fVal;
}

/*--------------------------------------------------------------------------*/
int GetMonoPositions(pSicsSelector self, SConnection * pCon,
                     float *fTh, float *f2TH, float *fB1, float *fB2)
{
  int iRet;
  char pBueffel[512];

  iRet = MotorGetSoftPosition(self->pTheta, pCon, fTh);
  if (!iRet) {
    snprintf(pBueffel,sizeof(pBueffel)-1,
            "ERROR: cannot read Theta motor for monochromator %s\n",
            self->name);
    SCWrite(pCon, pBueffel, eError);
    return 0.;
  }
  iRet = MotorGetSoftPosition(self->pTwoTheta, pCon, f2TH);
  if (!iRet) {
    snprintf(pBueffel,sizeof(pBueffel)-1,
            "ERROR: cannot read TwoTheta motor for monochromator %s\n",
            self->name);
    SCWrite(pCon, pBueffel, eError);
    return 0.;
  }

  if (self->pBend1) {
    iRet = MotorGetSoftPosition(self->pBend1, pCon, fB1);
    if (!iRet) {
      snprintf(pBueffel,sizeof(pBueffel)-1,
              "ERROR: cannot read vertical bender motor for monochromator %s\n",
              self->name);
      SCWrite(pCon, pBueffel, eError);
      return 0.;
    }
  }
  if (self->pBend2) {
    iRet = MotorGetSoftPosition(self->pBend2, pCon, fB2);
    if (!iRet) {
      snprintf(pBueffel,sizeof(pBueffel)-1,
              "ERROR: cannot read horizontal bender motor for monochromator %s\n",
              self->name);
      SCWrite(pCon, pBueffel, eError);
      return 0.;
    }
  }
  return 1;
}