sicspsi/amorset.c

/*-------------------------------------------------------------------
 AMORSET together with  amorcomp and amordrive implement the position
 control facility for the reflectometer AMOR. This uses the algorithm
 with the beam height as the baseline.
 
 copyright: see file COPYRIGHT
 
 Mark Koennecke, October 2005
 
 Commented support for slit 5 away as this is gone now
 
 Mark Koennecke, March 2009

 Added elliptic guide,

 Mark Koennecke, August 2011
--------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include "amorset.h"
#include <motorlist.h>
#include "amorcomp.h"
#include "amordrive.h"
#include <trigd.h>
#include <lld.h>
#include "motor.h"
/*--------------------- type defines ------------------------------*/
#define TYM2T 1
#define TYS2T 2
#define TYATH 3

#define ABS(x) (x < 0 ? -(x) : (x))
/*------------ The meat of it all: The settings calculation --------*/
static int readMotors(pamorSet self, SConnection * pCon)
{
  int result, status;
  float val;

  result = LLDcreate(sizeof(MotControl));
  status = addMotorToList(result, "d1t", .0);
  if (status != 1) {
    SCWrite(pCon, "ERROR: configuration error: d1t not found", eError);
    return -1;
  }
  status = addMotorToList(result, "d2t", .0);
  if (status != 1) {
    SCWrite(pCon, "ERROR: configuration error: d2t not found", eError);
    return -1;
  }
  status = addMotorToList(result, "d3t", .0);
  if (status != 1) {
    SCWrite(pCon, "ERROR: configuration error: d3t not found", eError);
    return -1;
  }
  status = addMotorToList(result, "d4t", .0);
  if (status != 1) {
    SCWrite(pCon, "ERROR: configuration error: d4t not found", eError);
    return -1;
  }
  /*
  status = addMotorToList(result, "d5t", .0);
  if (status != 1) {
    SCWrite(pCon, "ERROR: configuration error: d5t not found", eError);
    return -1;
  }
  */
  val = self->listDrive->GetValue(&result, pCon);
  if (val < -99999) {
    LLDdelete(result);
    return -1;
  }
  return result;
}

/*------------------------------------------------------------------*/
static int calcAmorSettings(pamorSet self, SConnection * pCon)
{
  int readList;
  double val, dist, com = .0, soz, mot, sah, sdh, aoz, tmp;

  /*
   * read motors
   */
  readList = readMotors(self, pCon);
  if (readList < 0) {
    SCWrite(pCon,
            "ERROR: failed to read motors for amor settings", eError);
    return 0;
  }
        /**
	 * initialize drive list
	 */
  LLDdelete(self->driveList);
  self->driveList = LLDcreate(sizeof(MotControl));

  /*
   * soz
   */
  dist = ABS(calcCompPosition(&self->S) - calcCompPosition(&self->M));
  soz = dist * Tand(-self->targetm2t);
  addMotorToList(self->driveList, "soz", soz);

  /*
   * monochromator slit
   */
  if (self->DS.activeFlag == 1) {
    dist = ABS(calcCompPosition(&self->DS) - calcCompPosition(&self->M));
    val = dist * Tand(-self->targetm2t) - self->dspar;
    addMotorToList(self->driveList, "dbs", val);
  }

  /*
   * slit 1 is before the monochromator and does not need to be
   * driven when m2t changes. This is here to make sure that d1b is 
   * in a feasible position.
   */
  if (self->D1.activeFlag == 1) {
    mot = getListMotorPosition(readList, "d1t");
    if (mot < -99999) {
      SCWrite(pCon, "WARNING: skipping d1 because of bad read on d1t",
              eWarning);
    } else {
      val = -.5 * mot;
      addMotorToList(self->driveList, "d1b", val);
    }
  }

  /*
   * slit 2
   */
  if (self->D2.activeFlag == 1) {
    dist = ABS(calcCompPosition(&self->D2) - calcCompPosition(&self->M));
    mot = getListMotorPosition(readList, "d2t");
    if (mot < -99999) {
      SCWrite(pCon, "WARNING: skipping d2 because of bad read on d2t",
              eWarning);
    } else {
      val = dist * Tand(-self->targetm2t) - .5 * mot;
      addMotorToList(self->driveList, "d2b", val);
    }
  }

  /*
   * slit 3
   */
  if (self->D3.activeFlag == 1) {
    dist = ABS(calcCompPosition(&self->D3) - calcCompPosition(&self->M));
    mot = getListMotorPosition(readList, "d3t");
    if (mot < -99999) {
      SCWrite(pCon, "WARNING: skipping d3 because of bad read on d3t",
              eWarning);
    } else {
      val = dist * Tand(-self->targetm2t) - .5 * mot;
      addMotorToList(self->driveList, "d3b", val);
    }
  }

  /*
   * slit 5
   */
  /*
  if (self->D5.activeFlag == 1) {
    dist = ABS(calcCompPosition(&self->D5) - calcCompPosition(&self->S));
    mot = getListMotorPosition(readList, "d5t");
    if (mot < -99999) {
      SCWrite(pCon, "WARNING: skipping d5 because of bad read on d5t",
              eWarning);
    } else {
      val = soz + dist * Tand(com) - .5 * mot;
      addMotorToList(self->driveList, "d5b", val);
    }
  }
*/
  
  /*
   * Analyzer
   */
  if (self->A.activeFlag == 1) {
    com = self->targets2t - self->targetm2t + 2 * self->targetath;  
    sah = ABS(calcCompPosition(&self->A) - calcCompPosition(&self->S));
    aoz = soz + sah * Tand(self->targets2t - self->targetm2t);
    addMotorToList(self->driveList, "aoz", aoz);
    addMotorToList(self->driveList, "aom", self->targets2t - self->targetm2t + self->targetath);
    
    /*
     * detector, evil analyzer in
     */
    if(self->D.activeFlag == 1){
	    sdh = ABS(calcCompPosition(&self->D) - calcCompPosition(&self->S));
	    addMotorToList(self->driveList, "com", com);
	    tmp = soz -aoz;
	    val = sah - sqrt(sah*sah + tmp*tmp) + (sdh - sqrt(sah*sah + tmp*tmp))*(Cosd(com) -1.);
	    addMotorToList(self->driveList, "cox", -val);
	    val = aoz + (sdh -sqrt(sah*sah + tmp*tmp))*Sind(com);
	    addMotorToList(self->driveList, "coz", val);
    }
  } else {
	  /*
	   * detector, analyzer gone for good
	   */
	  com = self->targets2t - self->targetm2t;
	  if (self->D.activeFlag == 1) {
		addMotorToList(self->driveList, "com", com);
	    dist = ABS(calcCompPosition(&self->D) - calcCompPosition(&self->S));
	    val = -dist * (Cosd(com) - 1.);
	    addMotorToList(self->driveList, "cox", -val);
	    val = dist * Sind(com) + soz;
	    addMotorToList(self->driveList, "coz", val);
	  }
  }

  /*
   * slit 4
   */
  if (self->D4.activeFlag == 1) {
    dist = ABS(calcCompPosition(&self->D4) - calcCompPosition(&self->S));
    mot = getListMotorPosition(readList, "d4t");
    if (mot < -99999) {
      SCWrite(pCon, "WARNING: skipping d4 because of bad read on d4t",
              eWarning);
    } else {
      if(self->A.activeFlag == 1){
	val = soz + dist * Tand(self->targets2t - self->targetm2t) - .5*mot;
      } else {
	val = soz + dist * Tand(com) - .5 * mot;
      }
      addMotorToList(self->driveList, "d4b", val);
    }
  }
  printMotorList(self->driveList,pCon);

  LLDdelete(readList);
  self->mustDrive = 0;
  return 1;
}

/*----------------------------------------------------------------*/
static int updateActualPositions(pamorSet self, SConnection * pCon)
{
  int readList, status;
  float val, dist, tmp, com, aom, soz, aoz, sah;

        /**
	 * read some motors
	 */
  readList = LLDcreate(sizeof(MotControl));
  addMotorToList(readList, "soz", 125);
  addMotorToList(readList, "aoz", 125);
  addMotorToList(readList, "com", 125);
  addMotorToList(readList, "aom", 125);
  val = self->listDrive->GetValue(&readList, pCon);
  if (val < -99999.) {
    SCWrite(pCon,
            "ERROR: failed to read motors, values for m2t,s2t,ath invalid",
            eError);
    LLDdelete(readList);
    return 0;
  }
  val = getListMotorPosition(readList, "soz");
  soz = val;
  dist = ABS(calcCompPosition(&self->S) - calcCompPosition(&self->M));
  tmp = val / dist;
  if (ABS(tmp) > .0001) {
    self->actualm2t = -Atand(tmp);
  } else {
    self->actualm2t = .0;
  }
  if(self->A.activeFlag == 1){
	  com = getListMotorPosition(readList, "com");
	  aom = getListMotorPosition(readList, "aom");
	  aoz = getListMotorPosition(readList, "aoz");
	  sah = ABS(calcCompPosition(&self->A) - calcCompPosition(&self->S));
	  self->actuals2t = Atand((aoz-soz)/sah) + self->actualm2t;
	  self->actualath = -(self->actuals2t - self->actualm2t - aom);
  } else {
	  com = getListMotorPosition(readList, "com");
	  self->actuals2t = com + self->actualm2t;

	  val = getListMotorPosition(readList, "aom");
	  self->actualath = val - com;
  }
  
  LLDdelete(readList);
  self->mustRecalculate = 1;
  return 1;
}

/*=================== SICS internal interface functions============*/
static void *AMOSETGetInterface(void *data, int iD)
{
  pamorSet self = NULL;

  /*
   * this object shall never be driven directly
   */
  return NULL;
}

/*----------------------------------------------------------------
 This routine can return either OKOK or HWFault when thing
 go wrong. However, the return value of Halt is usually ignored!
------------------------------------------------------------------*/
static int AMOSETHalt(void *data)
{
  pamorSet self = NULL;

  self = (pamorSet) data;

  self->listDrive->Halt(&self->driveList);
  return OKOK;
}

/*----------------------------------------------------------------
 This routine can return either 1 or 0. 1 means the position can 
 be reached, 0 NOT
 If 0, error shall contain up to errlen characters of information
 about which limit was violated
------------------------------------------------------------------*/
static int AMOSETCheckLimits(void *data, float val,
                             char *error, int errlen)
{
  pamorSet self = NULL;
  return 1;
}

/*----------------------------------------------------------------
 This routine can return 0 when a limit problem occurred 
 OKOK when the motor was successfully started 
 HWFault when a problem occured starting the device
 Possible errors shall be printed to pCon
 For real motors, this is supposed to try at least three times
 to start the motor in question
 val is the value to drive the motor too
------------------------------------------------------------------*/
static long AMOSETSetValue(void *data, SConnection * pCon, float val)
{
  pamorSet self = NULL;
  return OKOK;
}

/*----------------------------------------------------------------
 Checks the status of a running motor. Possible return values
   HWBusy The motor is still running
   OKOK or HWIdle when the motor finished driving
   HWFault when a hardware problem ocurred
   HWPosFault when the hardware cannot reach a position
 Errors are duly to be printed to pCon
 For real motors CheckStatus again shall try hard to fix any 
 issues with the motor 
------------------------------------------------------------------*/
static int AMOSETCheckStatus(void *data, SConnection * pCon)
{
  pamorSet self = NULL;
  int status;

  self = (pamorSet) data;

  if (self->mustDrive == 1) {
    status = calcAmorSettings(self, pCon);
    if (status <= 0) {
      return HWFault;
    }
    if (self->verbose == 1) {
      printMotorList(self->driveList, pCon);
    }
    status = self->listDrive->SetValue(&self->driveList, pCon, .37);
    return HWBusy;
  } else {
    self->mustRecalculate = 1;
    return self->listDrive->CheckStatus(&self->driveList, pCon);
  }
}

/*----------------------------------------------------------------
 GetValue is supposed to read a motor position
 On errors, -99999999.99 is returned and messages printed to pCon
------------------------------------------------------------------*/
static float AMOSETGetValue(void *data, SConnection * pCon)
{
  pamorSet self = NULL;
  float val = -99999999.99;

  self = (pamorSet) data;

  return val;
}

/*================ external functions for amordrive ============*/
void amorSetMotor(pamorSet amor, int type, double value)
{
  switch (type) {
  case TYM2T:
    amor->targetm2t = value;
    break;
  case TYS2T:
    amor->targets2t = value;
    break;
  case TYATH:
    amor->targetath = value;
    break;
  default:
    assert(0);
    break;
  }
  amor->mustDrive = 1;
}

/*----------------------------------------------------------------*/
double amorGetMotor(pamorSet amor, SConnection * pCon, int type)
{
  if (amor->mustRecalculate == 1) {
    updateActualPositions(amor, pCon);
  }
  switch (type) {
  case TYM2T:
    return amor->actualm2t;
    break;
  case TYS2T:
    return amor->actuals2t;
    break;
  case TYATH:
    return amor->actualath;
    break;
  default:
    assert(0);
    break;
  }
  return -99999.999;
}

/*----------------------------------------------------------------
 Live and Deatch of objects.........
 returns NULL on failure, a new datastructure else
------------------------------------------------------------------*/
static int amorSetSave(void *data, char *name, FILE * fd)
{
  pamorSet self = NULL;

  self = (pamorSet) data;
  if (self == NULL) {
    return 0;
  }
  fprintf(fd, "%s dspar %f\n", name, self->dspar);
  fprintf(fd, "%s detectoroffset %f\n", name, self->detectoroffset);
  fprintf(fd, "%s verbose %d\n", name, self->verbose);
  fprintf(fd, "%s targets %f %f %f\n", name, self->targetath,
          self->targetm2t, self->targets2t);
  saveAmorComp(fd, name, "chopper", &self->chopper);
  saveAmorComp(fd, name, "mono", &self->M);
  saveAmorComp(fd, name, "ds", &self->DS);
  saveAmorComp(fd, name, "slit1", &self->D1);
  saveAmorComp(fd, name, "slit2", &self->D2);
  saveAmorComp(fd, name, "slit3", &self->D3);
  saveAmorComp(fd, name, "sample", &self->S);
  saveAmorComp(fd, name, "slit4", &self->D4);
  saveAmorComp(fd, name, "lens", &self->EL);
  /*
  saveAmorComp(fd, name, "slit5", &self->D5);
  */
  saveAmorComp(fd, name, "ana", &self->A);
  saveAmorComp(fd, name, "detector", &self->D);
  return 1;
}

/*---------------------------------------------------------------*/
static pamorSet AMOSETMakeObject()
{
  pamorSet self = NULL;

  self = (pamorSet) malloc(sizeof(amorSet));
  if (self == NULL) {
    return NULL;
  }
  memset(self, 0, sizeof(amorSet));
  self->pDes = CreateDescriptor("AmorSet");
  self->pDriv = CreateDrivableInterface();
  self->listDrive = makeMotListInterface();
  self->driveList = LLDcreate(sizeof(MotControl));
  if (self->pDes == NULL || self->pDriv == NULL ||
      self->listDrive == NULL || self->driveList < 0) {
    free(self);
    return NULL;
  }

  self->pDes->GetInterface = AMOSETGetInterface;
  self->pDes->SaveStatus = amorSetSave;
  self->pDriv->Halt = AMOSETHalt;
  self->pDriv->CheckLimits = AMOSETCheckLimits;
  self->pDriv->SetValue = AMOSETSetValue;
  self->pDriv->CheckStatus = AMOSETCheckStatus;
  self->pDriv->GetValue = AMOSETGetValue;

  return self;
}

/*-----------------------------------------------------------------*/
static void killAmorSet(void *data)
{
  pamorSet self = (pamorSet) data;

  if (self == NULL) {
    return;
  }

  if (self->pDes != NULL) {
    DeleteDescriptor(self->pDes);
  }
  if (self->pDriv != NULL) {
    free(self->pDriv);
  }
  if (self->listDrive != NULL) {
    free(self->listDrive);
  }
  LLDdelete(self->driveList);
  free(self);
}

/*---------------------------------------------------------------*/
static int testRequiredMotors(SConnection * pCon)
{
  char motList[][20] = { "soz", "com",
    "cox", "coz", "dbs",
    "d1t", "d1b",
    "d2b", "d2t",
    "d3b", "d3t", "d4b", "d4t",
     "aoz", "aom"
  }; /* removed d5b, d5t from this list */
  
  int i = 0, status = 1;
  pMotor pMot = NULL;
  char pBueffel[132];


  for(i = 0; i < 15; i++) {
    pMot = NULL;
    pMot = FindMotor(pServ->pSics, motList[i]);
    if (pMot == NULL) {
      snprintf(pBueffel, 131, "ERROR: motor %s for amorset not found",
               motList[i]);
      SCWrite(pCon, pBueffel, eError);
      status = 0;
    }
  }
  return status;
}

/*======================= interpreter interface section ============*/
int AmorSetFactory(SConnection * pCon, SicsInterp * pSics, void *pData,
                   int argc, char *argv[])
{
  int status;
  pamorSet pNew = NULL;
  pamorDrive pTuk = NULL;

  if (testRequiredMotors(pCon) == 0) {
    SCWrite(pCon,
            "ERROR: aborting initialization of amorset due to missing motors",
            eError);
    return 0;
  }

  pNew = AMOSETMakeObject();
  if (pNew == NULL) {
    SCWrite(pCon, "ERROR: out of memory creating amorset", eError);
    return 0;
  }
  status = AddCommand(pSics, "amorset", AmorSetAction, killAmorSet, pNew);
  if (!status) {
    SCWrite(pCon, "ERROR: duplicate command amorset NOT created", eError);
    return 0;
  }
  pTuk = makeAmorDrive(pNew, TYM2T);
  if (pTuk == NULL) {
    SCWrite(pCon, "ERROR: failed to allocate data for m2t", eError);
    return 0;
  }
  status = AddCommand(pSics, "m2t", AmorDriveAction, killAmorDrive, pTuk);
  if (!status) {
    SCWrite(pCon, "ERROR: duplicate command amorset m2t reated", eError);
    return 0;
  }
  pTuk = makeAmorDrive(pNew, TYATH);
  if (pTuk == NULL) {
    SCWrite(pCon, "ERROR: failed to allocate data for ath", eError);
    return 0;
  }
  status = AddCommand(pSics, "ath", AmorDriveAction, killAmorDrive, pTuk);
  if (!status) {
    SCWrite(pCon, "ERROR: duplicate command amorset ath reated", eError);
    return 0;
  }
  pTuk = makeAmorDrive(pNew, TYS2T);
  if (pTuk == NULL) {
    SCWrite(pCon, "ERROR: failed to allocate data for s2t", eError);
    return 0;
  }
  status = AddCommand(pSics, "s2t", AmorDriveAction, killAmorDrive, pTuk);
  if (!status) {
    SCWrite(pCon, "ERROR: duplicate command amorset s2t reated", eError);
    return 0;
  }
  SCSendOK(pCon);
  return 1;
}

/*-----------------------------------------------------------------------*/
static pamorComp locateComponent(pamorSet self, char *name)
{
  if (strcmp(name, "mono") == 0) {
    return &self->M;
  } else if (strcmp(name, "ds") == 0) {
    return &self->DS;
  } else if (strcmp(name, "slit1") == 0) {
    return &self->D1;
  } else if (strcmp(name, "slit2") == 0) {
    return &self->D2;
  } else if (strcmp(name, "slit3") == 0) {
    return &self->D3;
  } else if (strcmp(name, "lens") == 0) {
    return &self->EL;
  } else if (strcmp(name, "sample") == 0) {
    return &self->S;
  } else if (strcmp(name, "slit4") == 0) {
    return &self->D4;
 /*
  } else if (strcmp(name, "slit5") == 0) {
    return &self->D5;
 */
  } else if (strcmp(name, "detector") == 0) {
    return &self->D;
  } else if (strcmp(name, "ana") == 0) {
    return &self->A;
  } else if (strcmp(name, "chopper") == 0) {
    return &self->chopper;
  } else {
    return NULL;
  }
}

/*----------------------------------------------------------------------*/
static double calcCD(pamorSet self)
{
  double soz, cmh, smh, sdh, cd, dist;

  dist = ABS(calcCompPosition(&self->S) - calcCompPosition(&self->M));
  soz = dist * Cotd(self->targetm2t);
  cmh = calcCompPosition(&self->M);
  smh = calcCompPosition(&self->S) - calcCompPosition(&self->M);
  sdh = calcCompPosition(&self->D) - calcCompPosition(&self->M);
  cd = cmh + sqrt(smh * smh + soz * soz) + sdh;
  return cd;
}

/*-----------------------------------------------------------------------*/
static double calcChopperDetectorDistance(pamorSet self)
{
  double dist, diff, soz;

  dist = ABS(calcCompPosition(&self->S) - calcCompPosition(&self->M));
  soz = dist * Tand(-self->targetm2t);

  dist =
      ABS(calcCompPosition(&self->M) - calcCompPosition(&self->chopper));
  diff = calcCompPosition(&self->M) - calcCompPosition(&self->S);
  dist += sqrt(diff * diff + soz * soz);
  dist += ABS(calcCompPosition(&self->S) - calcCompPosition(&self->D));
  dist += self->detectoroffset;
  return dist;
}

/*-----------------------------------------------------------------------*/
int AmorSetAction(SConnection * pCon, SicsInterp * pSics, void *pData,
                  int argc, char *argv[])
{
  pamorSet self = NULL;
  pamorComp comp = NULL;
  char pBueffel[132];

  self = (pamorSet) pData;
  assert(self);

  if (argc < 2) {
    SCWrite(pCon, "ERROR: not enough arguments to amorset", eError);
    return 0;
  }

  /*
   * catch component commands
   */
  strtolower(argv[1]);
  comp = locateComponent(self, argv[1]);
  if (comp != NULL) {
    return handleCompCommand(comp, pCon, argc, argv);
  }

  /*
   * now it is for us ....
   */
  if (strcmp(argv[1], "dspar") == 0) {
    if (argc > 2) {
      if (!SCMatchRights(pCon, usMugger)) {
        return 0;
      }
      self->dspar = atof(argv[2]);
      SCSendOK(pCon);
      return 1;
    } else {
      snprintf(pBueffel, 131, "%s dspar = %f", argv[0], self->dspar);
      SCWrite(pCon, pBueffel, eValue);
      return 1;
    }
  } else if (strcmp(argv[1], "targets") == 0) {
    if (argc >= 5) {
      if (!SCMatchRights(pCon, usMugger)) {
        return 0;
      }
      self->targetath = atof(argv[2]);
      self->targetm2t = atof(argv[3]);
      self->targets2t = atof(argv[4]);
      SCSendOK(pCon);
      return 1;
    } else {
      snprintf(pBueffel, 131, "%s targets = %f %f %F", argv[0],
               self->targetath, self->targetm2t, self->targets2t);
      SCWrite(pCon, pBueffel, eValue);
      return 1;
    }
  } else if (strcmp(argv[1], "verbose") == 0) {
    if (argc > 2) {
      if (!SCMatchRights(pCon, usMugger)) {
        return 0;
      }
      self->verbose = atoi(argv[2]);
      SCSendOK(pCon);
      return 1;
    } else {
      snprintf(pBueffel, 131, "%s verbose = %d", argv[0], self->verbose);
      SCWrite(pCon, pBueffel, eValue);
      return 1;
    }
  } else if (strcmp(argv[1], "detectoroffset") == 0) {
    if (argc > 2) {
      if (!SCMatchRights(pCon, usUser)) {
        return 0;
      }
      self->detectoroffset = atof(argv[2]);
      SCSendOK(pCon);
      return 1;
    } else {
      snprintf(pBueffel, 131, "%s detectoroffset = %f", argv[0],
               self->detectoroffset);
      SCWrite(pCon, pBueffel, eValue);
      return 1;
    }
  } else if (strcmp(argv[1], "cd") == 0) {
    snprintf(pBueffel, 131, "%s cd = %f", argv[0], calcCD(self));
    SCWrite(pCon, pBueffel, eValue);
    return 1;
  } else if (strcmp(argv[1], "cdd") == 0) {
    snprintf(pBueffel, 131, "%s cdd = %f", argv[0],
             calcChopperDetectorDistance(self));
    SCWrite(pCon, pBueffel, eValue);
    return 1;
  } else {
    snprintf(pBueffel, 131, "ERROR: unknown subcommand %s to amorset",
             argv[1]);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  return 1;
}